the-world-engine
Version:
three.js based, unity like game engine for browser
270 lines (258 loc) • 11.2 kB
JavaScript
import { b2_linearSlop, b2_angularSlop, b2Maybe } from "../common/b2_settings.js";
import { b2Abs, b2Vec2, b2Vec3, b2Mat33, b2Rot } from "../common/b2_math.js";
import { b2Joint, b2JointDef, b2JointType } from "./b2_joint.js";
export class b2WeldJointDef extends b2JointDef {
constructor() {
super(b2JointType.e_weldJoint);
this.localAnchorA = new b2Vec2;
this.localAnchorB = new b2Vec2;
this.referenceAngle = 0;
this.stiffness = 0;
this.damping = 0;
}
Initialize(t, s, i) {
this.bodyA = t;
this.bodyB = s;
this.bodyA.GetLocalPoint(i, this.localAnchorA);
this.bodyB.GetLocalPoint(i, this.localAnchorB);
this.referenceAngle = this.bodyB.GetAngle() - this.bodyA.GetAngle();
}
}
export class b2WeldJoint extends b2Joint {
constructor(t) {
super(t);
this.m_stiffness = 0;
this.m_damping = 0;
this.m_bias = 0;
this.m_localAnchorA = new b2Vec2;
this.m_localAnchorB = new b2Vec2;
this.m_referenceAngle = 0;
this.m_gamma = 0;
this.m_impulse = new b2Vec3(0, 0, 0);
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_invMassA = 0;
this.m_invMassB = 0;
this.m_invIA = 0;
this.m_invIB = 0;
this.m_mass = new b2Mat33;
this.m_qA = new b2Rot;
this.m_qB = new b2Rot;
this.m_lalcA = new b2Vec2;
this.m_lalcB = new b2Vec2;
this.m_K = new b2Mat33;
this.m_stiffness = b2Maybe(t.stiffness, 0);
this.m_damping = b2Maybe(t.damping, 0);
this.m_localAnchorA.Copy(b2Maybe(t.localAnchorA, b2Vec2.ZERO));
this.m_localAnchorB.Copy(b2Maybe(t.localAnchorB, b2Vec2.ZERO));
this.m_referenceAngle = b2Maybe(t.referenceAngle, 0);
this.m_impulse.SetZero();
}
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].a;
const i = t.velocities[this.m_indexA].v;
let h = t.velocities[this.m_indexA].w;
const e = t.positions[this.m_indexB].a;
const n = t.velocities[this.m_indexB].v;
let o = t.velocities[this.m_indexB].w;
const c = this.m_qA.SetAngle(s), b = this.m_qB.SetAngle(e);
b2Vec2.SubVV(this.m_localAnchorA, this.m_localCenterA, this.m_lalcA);
b2Rot.MulRV(c, this.m_lalcA, this.m_rA);
b2Vec2.SubVV(this.m_localAnchorB, this.m_localCenterB, this.m_lalcB);
b2Rot.MulRV(b, this.m_lalcB, this.m_rB);
const l = this.m_invMassA, V = this.m_invMassB;
const r = this.m_invIA, d = this.m_invIB;
const a = this.m_K;
a.ex.x = l + V + this.m_rA.y * this.m_rA.y * r + this.m_rB.y * this.m_rB.y * d;
a.ey.x = -this.m_rA.y * this.m_rA.x * r - this.m_rB.y * this.m_rB.x * d;
a.ez.x = -this.m_rA.y * r - this.m_rB.y * d;
a.ex.y = a.ey.x;
a.ey.y = l + V + this.m_rA.x * this.m_rA.x * r + this.m_rB.x * this.m_rB.x * d;
a.ez.y = this.m_rA.x * r + this.m_rB.x * d;
a.ex.z = a.ez.x;
a.ey.z = a.ez.y;
a.ez.z = r + d;
if (this.m_stiffness > 0) {
a.GetInverse22(this.m_mass);
let i = r + d;
const h = e - s - this.m_referenceAngle;
const n = this.m_damping;
const o = this.m_stiffness;
const c = t.step.dt;
this.m_gamma = c * (n + c * o);
this.m_gamma = this.m_gamma !== 0 ? 1 / this.m_gamma : 0;
this.m_bias = h * c * o * this.m_gamma;
i += this.m_gamma;
this.m_mass.ez.z = i !== 0 ? 1 / i : 0;
} else {
a.GetSymInverse33(this.m_mass);
this.m_gamma = 0;
this.m_bias = 0;
}
if (t.step.warmStarting) {
this.m_impulse.SelfMul(t.step.dtRatio);
const s = b2WeldJoint.InitVelocityConstraints_s_P.Set(this.m_impulse.x, this.m_impulse.y);
i.SelfMulSub(l, s);
h -= r * (b2Vec2.CrossVV(this.m_rA, s) + this.m_impulse.z);
n.SelfMulAdd(V, s);
o += d * (b2Vec2.CrossVV(this.m_rB, s) + this.m_impulse.z);
} else {
this.m_impulse.SetZero();
}
t.velocities[this.m_indexA].w = h;
t.velocities[this.m_indexB].w = o;
}
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 n = this.m_invMassA, o = this.m_invMassB;
const c = this.m_invIA, b = this.m_invIB;
if (this.m_stiffness > 0) {
const t = e - i;
const l = -this.m_mass.ez.z * (t + this.m_bias + this.m_gamma * this.m_impulse.z);
this.m_impulse.z += l;
i -= c * l;
e += b * l;
const V = b2Vec2.SubVV(b2Vec2.AddVCrossSV(h, e, this.m_rB, b2Vec2.s_t0), b2Vec2.AddVCrossSV(s, i, this.m_rA, b2Vec2.s_t1), b2WeldJoint.SolveVelocityConstraints_s_Cdot1);
const r = b2Mat33.MulM33XY(this.m_mass, V.x, V.y, b2WeldJoint.SolveVelocityConstraints_s_impulse1).SelfNeg();
this.m_impulse.x += r.x;
this.m_impulse.y += r.y;
const d = r;
s.SelfMulSub(n, d);
i -= c * b2Vec2.CrossVV(this.m_rA, d);
h.SelfMulAdd(o, d);
e += b * b2Vec2.CrossVV(this.m_rB, d);
} else {
const t = b2Vec2.SubVV(b2Vec2.AddVCrossSV(h, e, this.m_rB, b2Vec2.s_t0), b2Vec2.AddVCrossSV(s, i, this.m_rA, b2Vec2.s_t1), b2WeldJoint.SolveVelocityConstraints_s_Cdot1);
const l = e - i;
const V = b2Mat33.MulM33XYZ(this.m_mass, t.x, t.y, l, b2WeldJoint.SolveVelocityConstraints_s_impulse).SelfNeg();
this.m_impulse.SelfAdd(V);
const r = b2WeldJoint.SolveVelocityConstraints_s_P.Set(V.x, V.y);
s.SelfMulSub(n, r);
i -= c * (b2Vec2.CrossVV(this.m_rA, r) + V.z);
h.SelfMulAdd(o, r);
e += b * (b2Vec2.CrossVV(this.m_rB, r) + V.z);
}
t.velocities[this.m_indexA].w = i;
t.velocities[this.m_indexB].w = e;
}
SolvePositionConstraints(t) {
const s = t.positions[this.m_indexA].c;
let i = t.positions[this.m_indexA].a;
const h = t.positions[this.m_indexB].c;
let e = t.positions[this.m_indexB].a;
const n = this.m_qA.SetAngle(i), o = this.m_qB.SetAngle(e);
const c = this.m_invMassA, b = this.m_invMassB;
const l = this.m_invIA, V = this.m_invIB;
b2Vec2.SubVV(this.m_localAnchorA, this.m_localCenterA, this.m_lalcA);
const r = b2Rot.MulRV(n, this.m_lalcA, this.m_rA);
b2Vec2.SubVV(this.m_localAnchorB, this.m_localCenterB, this.m_lalcB);
const d = b2Rot.MulRV(o, this.m_lalcB, this.m_rB);
let a, J;
const f = this.m_K;
f.ex.x = c + b + r.y * r.y * l + d.y * d.y * V;
f.ey.x = -r.y * r.x * l - d.y * d.x * V;
f.ez.x = -r.y * l - d.y * V;
f.ex.y = f.ey.x;
f.ey.y = c + b + r.x * r.x * l + d.x * d.x * V;
f.ez.y = r.x * l + d.x * V;
f.ex.z = f.ez.x;
f.ey.z = f.ez.y;
f.ez.z = l + V;
if (this.m_stiffness > 0) {
const t = b2Vec2.SubVV(b2Vec2.AddVV(h, d, b2Vec2.s_t0), b2Vec2.AddVV(s, r, b2Vec2.s_t1), b2WeldJoint.SolvePositionConstraints_s_C1);
a = t.Length();
J = 0;
const n = f.Solve22(t.x, t.y, b2WeldJoint.SolvePositionConstraints_s_P).SelfNeg();
s.SelfMulSub(c, n);
i -= l * b2Vec2.CrossVV(r, n);
h.SelfMulAdd(b, n);
e += V * b2Vec2.CrossVV(d, n);
} else {
const t = b2Vec2.SubVV(b2Vec2.AddVV(h, d, b2Vec2.s_t0), b2Vec2.AddVV(s, r, b2Vec2.s_t1), b2WeldJoint.SolvePositionConstraints_s_C1);
const n = e - i - this.m_referenceAngle;
a = t.Length();
J = b2Abs(n);
const o = f.Solve33(t.x, t.y, n, b2WeldJoint.SolvePositionConstraints_s_impulse).SelfNeg();
const w = b2WeldJoint.SolvePositionConstraints_s_P.Set(o.x, o.y);
s.SelfMulSub(c, w);
i -= l * (b2Vec2.CrossVV(this.m_rA, w) + o.z);
h.SelfMulAdd(b, w);
e += V * (b2Vec2.CrossVV(this.m_rB, w) + o.z);
}
t.positions[this.m_indexA].a = i;
t.positions[this.m_indexB].a = e;
return a <= b2_linearSlop && J <= b2_angularSlop;
}
GetAnchorA(t) {
return this.m_bodyA.GetWorldPoint(this.m_localAnchorA, t);
}
GetAnchorB(t) {
return this.m_bodyB.GetWorldPoint(this.m_localAnchorB, t);
}
GetReactionForce(t, s) {
s.x = t * this.m_impulse.x;
s.y = t * this.m_impulse.y;
return s;
}
GetReactionTorque(t) {
return t * this.m_impulse.z;
}
GetLocalAnchorA() {
return this.m_localAnchorA;
}
GetLocalAnchorB() {
return this.m_localAnchorB;
}
GetReferenceAngle() {
return this.m_referenceAngle;
}
SetStiffness(t) {
this.m_stiffness = t;
}
GetStiffness() {
return this.m_stiffness;
}
SetDamping(t) {
this.m_damping = t;
}
GetDamping() {
return this.m_damping;
}
Dump(t) {
const s = this.m_bodyA.m_islandIndex;
const i = this.m_bodyB.m_islandIndex;
t(" const jd: b2WeldJointDef = new b2WeldJointDef();\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.localAnchorA.Set(%.15f, %.15f);\n", this.m_localAnchorA.x, this.m_localAnchorA.y);
t(" jd.localAnchorB.Set(%.15f, %.15f);\n", this.m_localAnchorB.x, this.m_localAnchorB.y);
t(" jd.referenceAngle = %.15f;\n", this.m_referenceAngle);
t(" jd.stiffness = %.15f;\n", this.m_stiffness);
t(" jd.damping = %.15f;\n", this.m_damping);
t(" joints[%d] = this.m_world.CreateJoint(jd);\n", this.m_index);
}
}
b2WeldJoint.InitVelocityConstraints_s_P = new b2Vec2;
b2WeldJoint.SolveVelocityConstraints_s_Cdot1 = new b2Vec2;
b2WeldJoint.SolveVelocityConstraints_s_impulse1 = new b2Vec2;
b2WeldJoint.SolveVelocityConstraints_s_impulse = new b2Vec3;
b2WeldJoint.SolveVelocityConstraints_s_P = new b2Vec2;
b2WeldJoint.SolvePositionConstraints_s_C1 = new b2Vec2;
b2WeldJoint.SolvePositionConstraints_s_P = new b2Vec2;
b2WeldJoint.SolvePositionConstraints_s_impulse = new b2Vec3;