the-world-engine
Version:
three.js based, unity like game engine for browser
194 lines (187 loc) • 7.54 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 b2FrictionJointDef extends b2JointDef {
constructor() {
super(b2JointType.e_frictionJoint);
this.localAnchorA = new b2Vec2;
this.localAnchorB = new b2Vec2;
this.maxForce = 0;
this.maxTorque = 0;
}
Initialize(t, i, s) {
this.bodyA = t;
this.bodyB = i;
this.bodyA.GetLocalPoint(s, this.localAnchorA);
this.bodyB.GetLocalPoint(s, this.localAnchorB);
}
}
export class b2FrictionJoint extends b2Joint {
constructor(t) {
super(t);
this.m_localAnchorA = new b2Vec2;
this.m_localAnchorB = new b2Vec2;
this.m_linearImpulse = new b2Vec2;
this.m_angularImpulse = 0;
this.m_maxForce = 0;
this.m_maxTorque = 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_linearMass = new b2Mat22;
this.m_angularMass = 0;
this.m_qA = new b2Rot;
this.m_qB = new b2Rot;
this.m_lalcA = new b2Vec2;
this.m_lalcB = new b2Vec2;
this.m_K = new b2Mat22;
this.m_localAnchorA.Copy(b2Maybe(t.localAnchorA, b2Vec2.ZERO));
this.m_localAnchorB.Copy(b2Maybe(t.localAnchorB, b2Vec2.ZERO));
this.m_linearImpulse.SetZero();
this.m_maxForce = b2Maybe(t.maxForce, 0);
this.m_maxTorque = b2Maybe(t.maxTorque, 0);
this.m_linearMass.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 i = t.positions[this.m_indexA].a;
const s = 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(i), b = this.m_qB.SetAngle(e);
b2Vec2.SubVV(this.m_localAnchorA, this.m_localCenterA, this.m_lalcA);
const r = b2Rot.MulRV(c, this.m_lalcA, this.m_rA);
b2Vec2.SubVV(this.m_localAnchorB, this.m_localCenterB, this.m_lalcB);
const a = b2Rot.MulRV(b, this.m_lalcB, this.m_rB);
const V = this.m_invMassA, l = this.m_invMassB;
const d = this.m_invIA, u = this.m_invIB;
const w = this.m_K;
w.ex.x = V + l + d * r.y * r.y + u * a.y * a.y;
w.ex.y = -d * r.x * r.y - u * a.x * a.y;
w.ey.x = w.ex.y;
w.ey.y = V + l + d * r.x * r.x + u * a.x * a.x;
w.GetInverse(this.m_linearMass);
this.m_angularMass = d + u;
if (this.m_angularMass > 0) {
this.m_angularMass = 1 / this.m_angularMass;
}
if (t.step.warmStarting) {
this.m_linearImpulse.SelfMul(t.step.dtRatio);
this.m_angularImpulse *= t.step.dtRatio;
const i = this.m_linearImpulse;
s.SelfMulSub(V, i);
h -= d * (b2Vec2.CrossVV(this.m_rA, i) + this.m_angularImpulse);
n.SelfMulAdd(l, i);
o += u * (b2Vec2.CrossVV(this.m_rB, i) + this.m_angularImpulse);
} else {
this.m_linearImpulse.SetZero();
this.m_angularImpulse = 0;
}
t.velocities[this.m_indexA].w = h;
t.velocities[this.m_indexB].w = o;
}
SolveVelocityConstraints(t) {
const i = t.velocities[this.m_indexA].v;
let s = 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;
const r = t.step.dt;
{
const t = e - s;
let i = -this.m_angularMass * t;
const h = this.m_angularImpulse;
const n = r * this.m_maxTorque;
this.m_angularImpulse = b2Clamp(this.m_angularImpulse + i, -n, n);
i = this.m_angularImpulse - h;
s -= c * i;
e += b * i;
}
{
const t = b2Vec2.SubVV(b2Vec2.AddVCrossSV(h, e, this.m_rB, b2Vec2.s_t0), b2Vec2.AddVCrossSV(i, s, this.m_rA, b2Vec2.s_t1), b2FrictionJoint.SolveVelocityConstraints_s_Cdot_v2);
const a = b2Mat22.MulMV(this.m_linearMass, t, b2FrictionJoint.SolveVelocityConstraints_s_impulseV).SelfNeg();
const V = b2FrictionJoint.SolveVelocityConstraints_s_oldImpulseV.Copy(this.m_linearImpulse);
this.m_linearImpulse.SelfAdd(a);
const l = r * this.m_maxForce;
if (this.m_linearImpulse.LengthSquared() > l * l) {
this.m_linearImpulse.Normalize();
this.m_linearImpulse.SelfMul(l);
}
b2Vec2.SubVV(this.m_linearImpulse, V, a);
i.SelfMulSub(n, a);
s -= c * b2Vec2.CrossVV(this.m_rA, a);
h.SelfMulAdd(o, a);
e += b * b2Vec2.CrossVV(this.m_rB, a);
}
t.velocities[this.m_indexA].w = s;
t.velocities[this.m_indexB].w = e;
}
SolvePositionConstraints(t) {
return true;
}
GetAnchorA(t) {
return this.m_bodyA.GetWorldPoint(this.m_localAnchorA, t);
}
GetAnchorB(t) {
return this.m_bodyB.GetWorldPoint(this.m_localAnchorB, t);
}
GetReactionForce(t, i) {
i.x = t * this.m_linearImpulse.x;
i.y = t * this.m_linearImpulse.y;
return i;
}
GetReactionTorque(t) {
return t * this.m_angularImpulse;
}
GetLocalAnchorA() {
return this.m_localAnchorA;
}
GetLocalAnchorB() {
return this.m_localAnchorB;
}
SetMaxForce(t) {
this.m_maxForce = t;
}
GetMaxForce() {
return this.m_maxForce;
}
SetMaxTorque(t) {
this.m_maxTorque = t;
}
GetMaxTorque() {
return this.m_maxTorque;
}
Dump(t) {
const i = this.m_bodyA.m_islandIndex;
const s = this.m_bodyB.m_islandIndex;
t(" const jd: b2FrictionJointDef = new b2FrictionJointDef();\n");
t(" jd.bodyA = bodies[%d];\n", i);
t(" jd.bodyB = bodies[%d];\n", s);
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.maxForce = %.15f;\n", this.m_maxForce);
t(" jd.maxTorque = %.15f;\n", this.m_maxTorque);
t(" joints[%d] = this.m_world.CreateJoint(jd);\n", this.m_index);
}
}
b2FrictionJoint.SolveVelocityConstraints_s_Cdot_v2 = new b2Vec2;
b2FrictionJoint.SolveVelocityConstraints_s_impulseV = new b2Vec2;
b2FrictionJoint.SolveVelocityConstraints_s_oldImpulseV = new b2Vec2;