the-world-engine
Version:
three.js based, unity like game engine for browser
221 lines (214 loc) • 8.56 kB
JavaScript
import { b2Maybe } from "../common/b2_settings.js";
import { b2Clamp, b2Vec2, b2Mat22, b2Rot } from "../common/b2_math.js";
import { b2Joint, b2JointDef, b2JointType } from "./b2_joint.js";
export class b2MotorJointDef extends b2JointDef {
constructor() {
super(b2JointType.e_motorJoint);
this.linearOffset = new b2Vec2(0, 0);
this.angularOffset = 0;
this.maxForce = 1;
this.maxTorque = 1;
this.correctionFactor = .3;
}
Initialize(t, s) {
this.bodyA = t;
this.bodyB = s;
this.bodyA.GetLocalPoint(this.bodyB.GetPosition(), this.linearOffset);
const i = this.bodyA.GetAngle();
const h = this.bodyB.GetAngle();
this.angularOffset = h - i;
}
}
export class b2MotorJoint extends b2Joint {
constructor(t) {
super(t);
this.m_linearOffset = new b2Vec2;
this.m_angularOffset = 0;
this.m_linearImpulse = new b2Vec2;
this.m_angularImpulse = 0;
this.m_maxForce = 0;
this.m_maxTorque = 0;
this.m_correctionFactor = .3;
this.m_indexA = 0;
this.m_indexB = 0;
this.m_rA = new b2Vec2;
this.m_rB = new b2Vec2;
this.m_localCenterA = new b2Vec2;
this.m_localCenterB = new b2Vec2;
this.m_linearError = new b2Vec2;
this.m_angularError = 0;
this.m_invMassA = 0;
this.m_invMassB = 0;
this.m_invIA = 0;
this.m_invIB = 0;
this.m_linearMass = new b2Mat22;
this.m_angularMass = 0;
this.m_qA = new b2Rot;
this.m_qB = new b2Rot;
this.m_K = new b2Mat22;
this.m_linearOffset.Copy(b2Maybe(t.linearOffset, b2Vec2.ZERO));
this.m_linearImpulse.SetZero();
this.m_maxForce = b2Maybe(t.maxForce, 0);
this.m_maxTorque = b2Maybe(t.maxTorque, 0);
this.m_correctionFactor = b2Maybe(t.correctionFactor, .3);
}
GetAnchorA(t) {
const s = this.m_bodyA.GetPosition();
t.x = s.x;
t.y = s.y;
return t;
}
GetAnchorB(t) {
const s = this.m_bodyB.GetPosition();
t.x = s.x;
t.y = s.y;
return t;
}
GetReactionForce(t, s) {
return b2Vec2.MulSV(t, this.m_linearImpulse, s);
}
GetReactionTorque(t) {
return t * this.m_angularImpulse;
}
SetLinearOffset(t) {
if (!b2Vec2.IsEqualToV(t, this.m_linearOffset)) {
this.m_bodyA.SetAwake(true);
this.m_bodyB.SetAwake(true);
this.m_linearOffset.Copy(t);
}
}
GetLinearOffset() {
return this.m_linearOffset;
}
SetAngularOffset(t) {
if (t !== this.m_angularOffset) {
this.m_bodyA.SetAwake(true);
this.m_bodyB.SetAwake(true);
this.m_angularOffset = t;
}
}
GetAngularOffset() {
return this.m_angularOffset;
}
SetMaxForce(t) {
this.m_maxForce = t;
}
GetMaxForce() {
return this.m_maxForce;
}
SetMaxTorque(t) {
this.m_maxTorque = t;
}
GetMaxTorque() {
return this.m_maxTorque;
}
InitVelocityConstraints(t) {
this.m_indexA = this.m_bodyA.m_islandIndex;
this.m_indexB = this.m_bodyB.m_islandIndex;
this.m_localCenterA.Copy(this.m_bodyA.m_sweep.localCenter);
this.m_localCenterB.Copy(this.m_bodyB.m_sweep.localCenter);
this.m_invMassA = this.m_bodyA.m_invMass;
this.m_invMassB = this.m_bodyB.m_invMass;
this.m_invIA = this.m_bodyA.m_invI;
this.m_invIB = this.m_bodyB.m_invI;
const s = t.positions[this.m_indexA].c;
const i = t.positions[this.m_indexA].a;
const h = t.velocities[this.m_indexA].v;
let e = t.velocities[this.m_indexA].w;
const o = t.positions[this.m_indexB].c;
const n = t.positions[this.m_indexB].a;
const c = t.velocities[this.m_indexB].v;
let b = t.velocities[this.m_indexB].w;
const r = this.m_qA.SetAngle(i), V = this.m_qB.SetAngle(n);
const a = b2Rot.MulRV(r, b2Vec2.SubVV(this.m_linearOffset, this.m_localCenterA, b2Vec2.s_t0), this.m_rA);
const f = b2Rot.MulRV(V, b2Vec2.NegV(this.m_localCenterB, b2Vec2.s_t0), this.m_rB);
const u = this.m_invMassA, l = this.m_invMassB;
const M = this.m_invIA, d = this.m_invIB;
const m = this.m_K;
m.ex.x = u + l + M * a.y * a.y + d * f.y * f.y;
m.ex.y = -M * a.x * a.y - d * f.x * f.y;
m.ey.x = m.ex.y;
m.ey.y = u + l + M * a.x * a.x + d * f.x * f.x;
m.GetInverse(this.m_linearMass);
this.m_angularMass = M + d;
if (this.m_angularMass > 0) {
this.m_angularMass = 1 / this.m_angularMass;
}
b2Vec2.SubVV(b2Vec2.AddVV(o, f, b2Vec2.s_t0), b2Vec2.AddVV(s, a, b2Vec2.s_t1), this.m_linearError);
this.m_angularError = n - i - this.m_angularOffset;
if (t.step.warmStarting) {
this.m_linearImpulse.SelfMul(t.step.dtRatio);
this.m_angularImpulse *= t.step.dtRatio;
const s = this.m_linearImpulse;
h.SelfMulSub(u, s);
e -= M * (b2Vec2.CrossVV(a, s) + this.m_angularImpulse);
c.SelfMulAdd(l, s);
b += d * (b2Vec2.CrossVV(f, s) + this.m_angularImpulse);
} else {
this.m_linearImpulse.SetZero();
this.m_angularImpulse = 0;
}
t.velocities[this.m_indexA].w = e;
t.velocities[this.m_indexB].w = b;
}
SolveVelocityConstraints(t) {
const s = t.velocities[this.m_indexA].v;
let i = t.velocities[this.m_indexA].w;
const h = t.velocities[this.m_indexB].v;
let e = t.velocities[this.m_indexB].w;
const o = this.m_invMassA, n = this.m_invMassB;
const c = this.m_invIA, b = this.m_invIB;
const r = t.step.dt;
const V = t.step.inv_dt;
{
const t = e - i + V * this.m_correctionFactor * this.m_angularError;
let s = -this.m_angularMass * t;
const h = this.m_angularImpulse;
const o = r * this.m_maxTorque;
this.m_angularImpulse = b2Clamp(this.m_angularImpulse + s, -o, o);
s = this.m_angularImpulse - h;
i -= c * s;
e += b * s;
}
{
const t = this.m_rA;
const a = this.m_rB;
const f = b2Vec2.AddVV(b2Vec2.SubVV(b2Vec2.AddVV(h, b2Vec2.CrossSV(e, a, b2Vec2.s_t0), b2Vec2.s_t0), b2Vec2.AddVV(s, b2Vec2.CrossSV(i, t, b2Vec2.s_t1), b2Vec2.s_t1), b2Vec2.s_t2), b2Vec2.MulSV(V * this.m_correctionFactor, this.m_linearError, b2Vec2.s_t3), b2MotorJoint.SolveVelocityConstraints_s_Cdot_v2);
const u = b2Mat22.MulMV(this.m_linearMass, f, b2MotorJoint.SolveVelocityConstraints_s_impulse_v2).SelfNeg();
const l = b2MotorJoint.SolveVelocityConstraints_s_oldImpulse_v2.Copy(this.m_linearImpulse);
this.m_linearImpulse.SelfAdd(u);
const M = r * this.m_maxForce;
if (this.m_linearImpulse.LengthSquared() > M * M) {
this.m_linearImpulse.Normalize();
this.m_linearImpulse.SelfMul(M);
}
b2Vec2.SubVV(this.m_linearImpulse, l, u);
s.SelfMulSub(o, u);
i -= c * b2Vec2.CrossVV(t, u);
h.SelfMulAdd(n, u);
e += b * b2Vec2.CrossVV(a, u);
}
t.velocities[this.m_indexA].w = i;
t.velocities[this.m_indexB].w = e;
}
SolvePositionConstraints(t) {
return true;
}
Dump(t) {
const s = this.m_bodyA.m_islandIndex;
const i = this.m_bodyB.m_islandIndex;
t(" const jd: b2MotorJointDef = new b2MotorJointDef();\n");
t(" jd.bodyA = bodies[%d];\n", s);
t(" jd.bodyB = bodies[%d];\n", i);
t(" jd.collideConnected = %s;\n", this.m_collideConnected ? "true" : "false");
t(" jd.linearOffset.Set(%.15f, %.15f);\n", this.m_linearOffset.x, this.m_linearOffset.y);
t(" jd.angularOffset = %.15f;\n", this.m_angularOffset);
t(" jd.maxForce = %.15f;\n", this.m_maxForce);
t(" jd.maxTorque = %.15f;\n", this.m_maxTorque);
t(" jd.correctionFactor = %.15f;\n", this.m_correctionFactor);
t(" joints[%d] = this.m_world.CreateJoint(jd);\n", this.m_index);
}
}
b2MotorJoint.SolveVelocityConstraints_s_Cdot_v2 = new b2Vec2;
b2MotorJoint.SolveVelocityConstraints_s_impulse_v2 = new b2Vec2;
b2MotorJoint.SolveVelocityConstraints_s_oldImpulse_v2 = new b2Vec2;