the-world-engine
Version:
three.js based, unity like game engine for browser
363 lines (343 loc) • 15.4 kB
JavaScript
import { b2_linearSlop, b2Maybe, b2_maxFloat } from "../common/b2_settings.js";
import { b2Abs, b2Clamp, b2Vec2, b2Rot, b2Max, b2Transform } from "../common/b2_math.js";
import { b2Joint, b2JointDef, b2JointType } from "./b2_joint.js";
import { b2Color } from "../common/b2_draw.js";
export class b2DistanceJointDef extends b2JointDef {
constructor() {
super(b2JointType.e_distanceJoint);
this.localAnchorA = new b2Vec2;
this.localAnchorB = new b2Vec2;
this.length = 1;
this.minLength = 0;
this.maxLength = b2_maxFloat;
this.stiffness = 0;
this.damping = 0;
}
Initialize(t, s, i, h) {
this.bodyA = t;
this.bodyB = s;
this.bodyA.GetLocalPoint(i, this.localAnchorA);
this.bodyB.GetLocalPoint(h, this.localAnchorB);
this.length = b2Max(b2Vec2.DistanceVV(i, h), b2_linearSlop);
this.minLength = this.length;
this.maxLength = this.length;
}
}
export class b2DistanceJoint extends b2Joint {
constructor(t) {
super(t);
this.m_stiffness = 0;
this.m_damping = 0;
this.m_bias = 0;
this.m_length = 0;
this.m_minLength = 0;
this.m_maxLength = 0;
this.m_localAnchorA = new b2Vec2;
this.m_localAnchorB = new b2Vec2;
this.m_gamma = 0;
this.m_impulse = 0;
this.m_lowerImpulse = 0;
this.m_upperImpulse = 0;
this.m_indexA = 0;
this.m_indexB = 0;
this.m_u = new b2Vec2;
this.m_rA = new b2Vec2;
this.m_rB = new b2Vec2;
this.m_localCenterA = new b2Vec2;
this.m_localCenterB = new b2Vec2;
this.m_currentLength = 0;
this.m_invMassA = 0;
this.m_invMassB = 0;
this.m_invIA = 0;
this.m_invIB = 0;
this.m_softMass = 0;
this.m_mass = 0;
this.m_qA = new b2Rot;
this.m_qB = new b2Rot;
this.m_lalcA = new b2Vec2;
this.m_lalcB = new b2Vec2;
this.m_localAnchorA.Copy(b2Maybe(t.localAnchorA, b2Vec2.ZERO));
this.m_localAnchorB.Copy(b2Maybe(t.localAnchorB, b2Vec2.ZERO));
this.m_length = b2Max(b2Maybe(t.length, this.GetCurrentLength()), b2_linearSlop);
this.m_minLength = b2Max(b2Maybe(t.minLength, this.m_length), b2_linearSlop);
this.m_maxLength = b2Max(b2Maybe(t.maxLength, this.m_length), this.m_minLength);
this.m_stiffness = b2Maybe(t.stiffness, 0);
this.m_damping = b2Maybe(t.damping, 0);
}
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 + this.m_lowerImpulse - this.m_upperImpulse) * this.m_u.x;
s.y = t * (this.m_impulse + this.m_lowerImpulse - this.m_upperImpulse) * this.m_u.y;
return s;
}
GetReactionTorque(t) {
return 0;
}
GetLocalAnchorA() {
return this.m_localAnchorA;
}
GetLocalAnchorB() {
return this.m_localAnchorB;
}
SetLength(t) {
this.m_impulse = 0;
this.m_length = b2Max(b2_linearSlop, t);
return this.m_length;
}
GetLength() {
return this.m_length;
}
SetMinLength(t) {
this.m_lowerImpulse = 0;
this.m_minLength = b2Clamp(t, b2_linearSlop, this.m_maxLength);
return this.m_minLength;
}
SetMaxLength(t) {
this.m_upperImpulse = 0;
this.m_maxLength = b2Max(t, this.m_minLength);
return this.m_maxLength;
}
GetCurrentLength() {
const t = this.m_bodyA.GetWorldPoint(this.m_localAnchorA, new b2Vec2);
const s = this.m_bodyB.GetWorldPoint(this.m_localAnchorB, new b2Vec2);
return b2Vec2.DistanceVV(t, s);
}
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: b2DistanceJointDef = new b2DistanceJointDef();\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.length = %.15f;\n", this.m_length);
t(" jd.minLength = %.15f;\n", this.m_minLength);
t(" jd.maxLength = %.15f;\n", this.m_maxLength);
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);
}
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 n = t.velocities[this.m_indexA].w;
const e = t.positions[this.m_indexB].c;
const c = t.positions[this.m_indexB].a;
const o = t.velocities[this.m_indexB].v;
let b = t.velocities[this.m_indexB].w;
const a = this.m_qA.SetAngle(i), r = this.m_qB.SetAngle(c);
b2Vec2.SubVV(this.m_localAnchorA, this.m_localCenterA, this.m_lalcA);
b2Rot.MulRV(a, this.m_lalcA, this.m_rA);
b2Vec2.SubVV(this.m_localAnchorB, this.m_localCenterB, this.m_lalcB);
b2Rot.MulRV(r, this.m_lalcB, this.m_rB);
this.m_u.x = e.x + this.m_rB.x - s.x - this.m_rA.x;
this.m_u.y = e.y + this.m_rB.y - s.y - this.m_rA.y;
this.m_currentLength = this.m_u.Length();
if (this.m_currentLength > b2_linearSlop) {
this.m_u.SelfMul(1 / this.m_currentLength);
} else {
this.m_u.SetZero();
this.m_mass = 0;
this.m_impulse = 0;
this.m_lowerImpulse = 0;
this.m_upperImpulse = 0;
}
const V = b2Vec2.CrossVV(this.m_rA, this.m_u);
const l = b2Vec2.CrossVV(this.m_rB, this.m_u);
let D = this.m_invMassA + this.m_invIA * V * V + this.m_invMassB + this.m_invIB * l * l;
this.m_mass = D !== 0 ? 1 / D : 0;
if (this.m_stiffness > 0 && this.m_minLength < this.m_maxLength) {
const s = this.m_currentLength - this.m_length;
const i = this.m_damping;
const h = this.m_stiffness;
const n = t.step.dt;
this.m_gamma = n * (i + n * h);
this.m_gamma = this.m_gamma !== 0 ? 1 / this.m_gamma : 0;
this.m_bias = s * n * h * this.m_gamma;
D += this.m_gamma;
this.m_softMass = D !== 0 ? 1 / D : 0;
} else {
this.m_gamma = 0;
this.m_bias = 0;
this.m_softMass = this.m_mass;
}
if (t.step.warmStarting) {
this.m_impulse *= t.step.dtRatio;
this.m_lowerImpulse *= t.step.dtRatio;
this.m_upperImpulse *= t.step.dtRatio;
const s = b2Vec2.MulSV(this.m_impulse + this.m_lowerImpulse - this.m_upperImpulse, this.m_u, b2DistanceJoint.InitVelocityConstraints_s_P);
h.SelfMulSub(this.m_invMassA, s);
n -= this.m_invIA * b2Vec2.CrossVV(this.m_rA, s);
o.SelfMulAdd(this.m_invMassB, s);
b += this.m_invIB * b2Vec2.CrossVV(this.m_rB, s);
} else {
this.m_impulse = 0;
}
t.velocities[this.m_indexA].w = n;
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 n = t.velocities[this.m_indexB].w;
if (this.m_minLength < this.m_maxLength) {
if (this.m_stiffness > 0) {
const t = b2Vec2.AddVCrossSV(s, i, this.m_rA, b2DistanceJoint.SolveVelocityConstraints_s_vpA);
const e = b2Vec2.AddVCrossSV(h, n, this.m_rB, b2DistanceJoint.SolveVelocityConstraints_s_vpB);
const c = b2Vec2.DotVV(this.m_u, b2Vec2.SubVV(e, t, b2Vec2.s_t0));
const o = -this.m_softMass * (c + this.m_bias + this.m_gamma * this.m_impulse);
this.m_impulse += o;
const b = b2Vec2.MulSV(o, this.m_u, b2DistanceJoint.SolveVelocityConstraints_s_P);
s.SelfMulSub(this.m_invMassA, b);
i -= this.m_invIA * b2Vec2.CrossVV(this.m_rA, b);
h.SelfMulAdd(this.m_invMassB, b);
n += this.m_invIB * b2Vec2.CrossVV(this.m_rB, b);
}
{
const e = this.m_currentLength - this.m_minLength;
const c = b2Max(0, e) * t.step.inv_dt;
const o = b2Vec2.AddVCrossSV(s, i, this.m_rA, b2DistanceJoint.SolveVelocityConstraints_s_vpA);
const b = b2Vec2.AddVCrossSV(h, n, this.m_rB, b2DistanceJoint.SolveVelocityConstraints_s_vpB);
const a = b2Vec2.DotVV(this.m_u, b2Vec2.SubVV(b, o, b2Vec2.s_t0));
let r = -this.m_mass * (a + c);
const V = this.m_lowerImpulse;
this.m_lowerImpulse = b2Max(0, this.m_lowerImpulse + r);
r = this.m_lowerImpulse - V;
const l = b2Vec2.MulSV(r, this.m_u, b2DistanceJoint.SolveVelocityConstraints_s_P);
s.SelfMulSub(this.m_invMassA, l);
i -= this.m_invIA * b2Vec2.CrossVV(this.m_rA, l);
h.SelfMulAdd(this.m_invMassB, l);
n += this.m_invIB * b2Vec2.CrossVV(this.m_rB, l);
}
{
const e = this.m_maxLength - this.m_currentLength;
const c = b2Max(0, e) * t.step.inv_dt;
const o = b2Vec2.AddVCrossSV(s, i, this.m_rA, b2DistanceJoint.SolveVelocityConstraints_s_vpA);
const b = b2Vec2.AddVCrossSV(h, n, this.m_rB, b2DistanceJoint.SolveVelocityConstraints_s_vpB);
const a = b2Vec2.DotVV(this.m_u, b2Vec2.SubVV(o, b, b2Vec2.s_t0));
let r = -this.m_mass * (a + c);
const V = this.m_upperImpulse;
this.m_upperImpulse = b2Max(0, this.m_upperImpulse + r);
r = this.m_upperImpulse - V;
const l = b2Vec2.MulSV(-r, this.m_u, b2DistanceJoint.SolveVelocityConstraints_s_P);
s.SelfMulSub(this.m_invMassA, l);
i -= this.m_invIA * b2Vec2.CrossVV(this.m_rA, l);
h.SelfMulAdd(this.m_invMassB, l);
n += this.m_invIB * b2Vec2.CrossVV(this.m_rB, l);
}
} else {
const t = b2Vec2.AddVCrossSV(s, i, this.m_rA, b2DistanceJoint.SolveVelocityConstraints_s_vpA);
const e = b2Vec2.AddVCrossSV(h, n, this.m_rB, b2DistanceJoint.SolveVelocityConstraints_s_vpB);
const c = b2Vec2.DotVV(this.m_u, b2Vec2.SubVV(e, t, b2Vec2.s_t0));
const o = -this.m_mass * c;
this.m_impulse += o;
const b = b2Vec2.MulSV(o, this.m_u, b2DistanceJoint.SolveVelocityConstraints_s_P);
s.SelfMulSub(this.m_invMassA, b);
i -= this.m_invIA * b2Vec2.CrossVV(this.m_rA, b);
h.SelfMulAdd(this.m_invMassB, b);
n += this.m_invIB * b2Vec2.CrossVV(this.m_rB, b);
}
t.velocities[this.m_indexA].w = i;
t.velocities[this.m_indexB].w = n;
}
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 n = t.positions[this.m_indexB].a;
const e = this.m_qA.SetAngle(i), c = this.m_qB.SetAngle(n);
const o = b2Rot.MulRV(e, this.m_lalcA, this.m_rA);
const b = b2Rot.MulRV(c, this.m_lalcB, this.m_rB);
const a = this.m_u;
a.x = h.x + b.x - s.x - o.x;
a.y = h.y + b.y - s.y - o.y;
const r = this.m_u.Normalize();
let V;
if (this.m_minLength == this.m_maxLength) {
V = r - this.m_minLength;
} else if (r < this.m_minLength) {
V = r - this.m_minLength;
} else if (this.m_maxLength < r) {
V = r - this.m_maxLength;
} else {
return true;
}
const l = -this.m_mass * V;
const D = b2Vec2.MulSV(l, a, b2DistanceJoint.SolvePositionConstraints_s_P);
s.SelfMulSub(this.m_invMassA, D);
i -= this.m_invIA * b2Vec2.CrossVV(o, D);
h.SelfMulAdd(this.m_invMassB, D);
n += this.m_invIB * b2Vec2.CrossVV(b, D);
t.positions[this.m_indexA].a = i;
t.positions[this.m_indexB].a = n;
return b2Abs(V) < b2_linearSlop;
}
Draw(t) {
const s = this.m_bodyA.GetTransform();
const i = this.m_bodyB.GetTransform();
const h = b2Transform.MulXV(s, this.m_localAnchorA, b2DistanceJoint.Draw_s_pA);
const n = b2Transform.MulXV(i, this.m_localAnchorB, b2DistanceJoint.Draw_s_pB);
const e = b2Vec2.SubVV(n, h, b2DistanceJoint.Draw_s_axis);
e.Normalize();
const c = b2DistanceJoint.Draw_s_c1;
const o = b2DistanceJoint.Draw_s_c2;
const b = b2DistanceJoint.Draw_s_c3;
const a = b2DistanceJoint.Draw_s_c4;
t.DrawSegment(h, n, a);
const r = b2Vec2.AddVMulSV(h, this.m_length, e, b2DistanceJoint.Draw_s_pRest);
t.DrawPoint(r, 8, c);
if (this.m_minLength != this.m_maxLength) {
if (this.m_minLength > b2_linearSlop) {
const s = b2Vec2.AddVMulSV(h, this.m_minLength, e, b2DistanceJoint.Draw_s_pMin);
t.DrawPoint(s, 4, o);
}
if (this.m_maxLength < b2_maxFloat) {
const s = b2Vec2.AddVMulSV(h, this.m_maxLength, e, b2DistanceJoint.Draw_s_pMax);
t.DrawPoint(s, 4, b);
}
}
}
}
b2DistanceJoint.InitVelocityConstraints_s_P = new b2Vec2;
b2DistanceJoint.SolveVelocityConstraints_s_vpA = new b2Vec2;
b2DistanceJoint.SolveVelocityConstraints_s_vpB = new b2Vec2;
b2DistanceJoint.SolveVelocityConstraints_s_P = new b2Vec2;
b2DistanceJoint.SolvePositionConstraints_s_P = new b2Vec2;
b2DistanceJoint.Draw_s_pA = new b2Vec2;
b2DistanceJoint.Draw_s_pB = new b2Vec2;
b2DistanceJoint.Draw_s_axis = new b2Vec2;
b2DistanceJoint.Draw_s_c1 = new b2Color(.7, .7, .7);
b2DistanceJoint.Draw_s_c2 = new b2Color(.3, .9, .3);
b2DistanceJoint.Draw_s_c3 = new b2Color(.9, .3, .3);
b2DistanceJoint.Draw_s_c4 = new b2Color(.4, .4, .4);
b2DistanceJoint.Draw_s_pRest = new b2Vec2;
b2DistanceJoint.Draw_s_pMin = new b2Vec2;
b2DistanceJoint.Draw_s_pMax = new b2Vec2;