the-world-engine/dist/esm/box2d.ts/build/dynamics/b2_gear_joint.js

three.js based, unity like game engine for browser

import { b2_linearSlop, b2Maybe } from "../common/b2_settings.js";

import { b2Vec2, b2Rot } from "../common/b2_math.js";

import { b2Joint, b2JointDef, b2JointType } from "./b2_joint.js";

export class b2GearJointDef extends b2JointDef {
    constructor() {
        super(b2JointType.e_gearJoint);
        this.ratio = 1;
    }
}

export class b2GearJoint extends b2Joint {
    constructor(t) {
        super(t);
        this.m_typeA = b2JointType.e_unknownJoint;
        this.m_typeB = b2JointType.e_unknownJoint;
        this.m_localAnchorA = new b2Vec2;
        this.m_localAnchorB = new b2Vec2;
        this.m_localAnchorC = new b2Vec2;
        this.m_localAnchorD = new b2Vec2;
        this.m_localAxisC = new b2Vec2;
        this.m_localAxisD = new b2Vec2;
        this.m_referenceAngleA = 0;
        this.m_referenceAngleB = 0;
        this.m_constant = 0;
        this.m_ratio = 0;
        this.m_impulse = 0;
        this.m_indexA = 0;
        this.m_indexB = 0;
        this.m_indexC = 0;
        this.m_indexD = 0;
        this.m_lcA = new b2Vec2;
        this.m_lcB = new b2Vec2;
        this.m_lcC = new b2Vec2;
        this.m_lcD = new b2Vec2;
        this.m_mA = 0;
        this.m_mB = 0;
        this.m_mC = 0;
        this.m_mD = 0;
        this.m_iA = 0;
        this.m_iB = 0;
        this.m_iC = 0;
        this.m_iD = 0;
        this.m_JvAC = new b2Vec2;
        this.m_JvBD = new b2Vec2;
        this.m_JwA = 0;
        this.m_JwB = 0;
        this.m_JwC = 0;
        this.m_JwD = 0;
        this.m_mass = 0;
        this.m_qA = new b2Rot;
        this.m_qB = new b2Rot;
        this.m_qC = new b2Rot;
        this.m_qD = new b2Rot;
        this.m_lalcA = new b2Vec2;
        this.m_lalcB = new b2Vec2;
        this.m_lalcC = new b2Vec2;
        this.m_lalcD = new b2Vec2;
        this.m_joint1 = t.joint1;
        this.m_joint2 = t.joint2;
        this.m_typeA = this.m_joint1.GetType();
        this.m_typeB = this.m_joint2.GetType();
        let s, i;
        this.m_bodyC = this.m_joint1.GetBodyA();
        this.m_bodyA = this.m_joint1.GetBodyB();
        const h = this.m_bodyA.m_xf;
        const e = this.m_bodyA.m_sweep.a;
        const n = this.m_bodyC.m_xf;
        const o = this.m_bodyC.m_sweep.a;
        if (this.m_typeA === b2JointType.e_revoluteJoint) {
            const i = t.joint1;
            this.m_localAnchorC.Copy(i.m_localAnchorA);
            this.m_localAnchorA.Copy(i.m_localAnchorB);
            this.m_referenceAngleA = i.m_referenceAngle;
            this.m_localAxisC.SetZero();
            s = e - o - this.m_referenceAngleA;
        } else {
            const i = t.joint1;
            this.m_localAnchorC.Copy(i.m_localAnchorA);
            this.m_localAnchorA.Copy(i.m_localAnchorB);
            this.m_referenceAngleA = i.m_referenceAngle;
            this.m_localAxisC.Copy(i.m_localXAxisA);
            const e = this.m_localAnchorC;
            const o = b2Rot.MulTRV(n.q, b2Vec2.AddVV(b2Rot.MulRV(h.q, this.m_localAnchorA, b2Vec2.s_t0), b2Vec2.SubVV(h.p, n.p, b2Vec2.s_t1), b2Vec2.s_t0), b2Vec2.s_t0);
            s = b2Vec2.DotVV(b2Vec2.SubVV(o, e, b2Vec2.s_t0), this.m_localAxisC);
        }
        this.m_bodyD = this.m_joint2.GetBodyA();
        this.m_bodyB = this.m_joint2.GetBodyB();
        const b = this.m_bodyB.m_xf;
        const c = this.m_bodyB.m_sweep.a;
        const V = this.m_bodyD.m_xf;
        const r = this.m_bodyD.m_sweep.a;
        if (this.m_typeB === b2JointType.e_revoluteJoint) {
            const s = t.joint2;
            this.m_localAnchorD.Copy(s.m_localAnchorA);
            this.m_localAnchorB.Copy(s.m_localAnchorB);
            this.m_referenceAngleB = s.m_referenceAngle;
            this.m_localAxisD.SetZero();
            i = c - r - this.m_referenceAngleB;
        } else {
            const s = t.joint2;
            this.m_localAnchorD.Copy(s.m_localAnchorA);
            this.m_localAnchorB.Copy(s.m_localAnchorB);
            this.m_referenceAngleB = s.m_referenceAngle;
            this.m_localAxisD.Copy(s.m_localXAxisA);
            const h = this.m_localAnchorD;
            const e = b2Rot.MulTRV(V.q, b2Vec2.AddVV(b2Rot.MulRV(b.q, this.m_localAnchorB, b2Vec2.s_t0), b2Vec2.SubVV(b.p, V.p, b2Vec2.s_t1), b2Vec2.s_t0), b2Vec2.s_t0);
            i = b2Vec2.DotVV(b2Vec2.SubVV(e, h, b2Vec2.s_t0), this.m_localAxisD);
        }
        this.m_ratio = b2Maybe(t.ratio, 1);
        this.m_constant = s + this.m_ratio * i;
        this.m_impulse = 0;
    }
    InitVelocityConstraints(t) {
        this.m_indexA = this.m_bodyA.m_islandIndex;
        this.m_indexB = this.m_bodyB.m_islandIndex;
        this.m_indexC = this.m_bodyC.m_islandIndex;
        this.m_indexD = this.m_bodyD.m_islandIndex;
        this.m_lcA.Copy(this.m_bodyA.m_sweep.localCenter);
        this.m_lcB.Copy(this.m_bodyB.m_sweep.localCenter);
        this.m_lcC.Copy(this.m_bodyC.m_sweep.localCenter);
        this.m_lcD.Copy(this.m_bodyD.m_sweep.localCenter);
        this.m_mA = this.m_bodyA.m_invMass;
        this.m_mB = this.m_bodyB.m_invMass;
        this.m_mC = this.m_bodyC.m_invMass;
        this.m_mD = this.m_bodyD.m_invMass;
        this.m_iA = this.m_bodyA.m_invI;
        this.m_iB = this.m_bodyB.m_invI;
        this.m_iC = this.m_bodyC.m_invI;
        this.m_iD = this.m_bodyD.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 b = t.positions[this.m_indexC].a;
        const c = t.velocities[this.m_indexC].v;
        let V = t.velocities[this.m_indexC].w;
        const r = t.positions[this.m_indexD].a;
        const a = t.velocities[this.m_indexD].v;
        let J = t.velocities[this.m_indexD].w;
        const l = this.m_qA.SetAngle(s), G = this.m_qB.SetAngle(e), w = this.m_qC.SetAngle(b), R = this.m_qD.SetAngle(r);
        this.m_mass = 0;
        if (this.m_typeA === b2JointType.e_revoluteJoint) {
            this.m_JvAC.SetZero();
            this.m_JwA = 1;
            this.m_JwC = 1;
            this.m_mass += this.m_iA + this.m_iC;
        } else {
            const t = b2Rot.MulRV(w, this.m_localAxisC, b2GearJoint.InitVelocityConstraints_s_u);
            b2Vec2.SubVV(this.m_localAnchorC, this.m_lcC, this.m_lalcC);
            const s = b2Rot.MulRV(w, this.m_lalcC, b2GearJoint.InitVelocityConstraints_s_rC);
            b2Vec2.SubVV(this.m_localAnchorA, this.m_lcA, this.m_lalcA);
            const i = b2Rot.MulRV(l, this.m_lalcA, b2GearJoint.InitVelocityConstraints_s_rA);
            this.m_JvAC.Copy(t);
            this.m_JwC = b2Vec2.CrossVV(s, t);
            this.m_JwA = b2Vec2.CrossVV(i, t);
            this.m_mass += this.m_mC + this.m_mA + this.m_iC * this.m_JwC * this.m_JwC + this.m_iA * this.m_JwA * this.m_JwA;
        }
        if (this.m_typeB === b2JointType.e_revoluteJoint) {
            this.m_JvBD.SetZero();
            this.m_JwB = this.m_ratio;
            this.m_JwD = this.m_ratio;
            this.m_mass += this.m_ratio * this.m_ratio * (this.m_iB + this.m_iD);
        } else {
            const t = b2Rot.MulRV(R, this.m_localAxisD, b2GearJoint.InitVelocityConstraints_s_u);
            b2Vec2.SubVV(this.m_localAnchorD, this.m_lcD, this.m_lalcD);
            const s = b2Rot.MulRV(R, this.m_lalcD, b2GearJoint.InitVelocityConstraints_s_rD);
            b2Vec2.SubVV(this.m_localAnchorB, this.m_lcB, this.m_lalcB);
            const i = b2Rot.MulRV(G, this.m_lalcB, b2GearJoint.InitVelocityConstraints_s_rB);
            b2Vec2.MulSV(this.m_ratio, t, this.m_JvBD);
            this.m_JwD = this.m_ratio * b2Vec2.CrossVV(s, t);
            this.m_JwB = this.m_ratio * b2Vec2.CrossVV(i, t);
            this.m_mass += this.m_ratio * this.m_ratio * (this.m_mD + this.m_mB) + this.m_iD * this.m_JwD * this.m_JwD + this.m_iB * this.m_JwB * this.m_JwB;
        }
        this.m_mass = this.m_mass > 0 ? 1 / this.m_mass : 0;
        if (t.step.warmStarting) {
            i.SelfMulAdd(this.m_mA * this.m_impulse, this.m_JvAC);
            h += this.m_iA * this.m_impulse * this.m_JwA;
            n.SelfMulAdd(this.m_mB * this.m_impulse, this.m_JvBD);
            o += this.m_iB * this.m_impulse * this.m_JwB;
            c.SelfMulSub(this.m_mC * this.m_impulse, this.m_JvAC);
            V -= this.m_iC * this.m_impulse * this.m_JwC;
            a.SelfMulSub(this.m_mD * this.m_impulse, this.m_JvBD);
            J -= this.m_iD * this.m_impulse * this.m_JwD;
        } else {
            this.m_impulse = 0;
        }
        t.velocities[this.m_indexA].w = h;
        t.velocities[this.m_indexB].w = o;
        t.velocities[this.m_indexC].w = V;
        t.velocities[this.m_indexD].w = J;
    }
    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 = t.velocities[this.m_indexC].v;
        let o = t.velocities[this.m_indexC].w;
        const b = t.velocities[this.m_indexD].v;
        let c = t.velocities[this.m_indexD].w;
        let V = b2Vec2.DotVV(this.m_JvAC, b2Vec2.SubVV(s, n, b2Vec2.s_t0)) + b2Vec2.DotVV(this.m_JvBD, b2Vec2.SubVV(h, b, b2Vec2.s_t0));
        V += this.m_JwA * i - this.m_JwC * o + (this.m_JwB * e - this.m_JwD * c);
        const r = -this.m_mass * V;
        this.m_impulse += r;
        s.SelfMulAdd(this.m_mA * r, this.m_JvAC);
        i += this.m_iA * r * this.m_JwA;
        h.SelfMulAdd(this.m_mB * r, this.m_JvBD);
        e += this.m_iB * r * this.m_JwB;
        n.SelfMulSub(this.m_mC * r, this.m_JvAC);
        o -= this.m_iC * r * this.m_JwC;
        b.SelfMulSub(this.m_mD * r, this.m_JvBD);
        c -= this.m_iD * r * this.m_JwD;
        t.velocities[this.m_indexA].w = i;
        t.velocities[this.m_indexB].w = e;
        t.velocities[this.m_indexC].w = o;
        t.velocities[this.m_indexD].w = c;
    }
    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 = t.positions[this.m_indexC].c;
        let o = t.positions[this.m_indexC].a;
        const b = t.positions[this.m_indexD].c;
        let c = t.positions[this.m_indexD].a;
        const V = this.m_qA.SetAngle(i), r = this.m_qB.SetAngle(e), a = this.m_qC.SetAngle(o), J = this.m_qD.SetAngle(c);
        const l = 0;
        let G, w;
        const R = this.m_JvAC, d = this.m_JvBD;
        let p, f, j, y;
        let u = 0;
        if (this.m_typeA === b2JointType.e_revoluteJoint) {
            R.SetZero();
            p = 1;
            j = 1;
            u += this.m_iA + this.m_iC;
            G = i - o - this.m_referenceAngleA;
        } else {
            const t = b2Rot.MulRV(a, this.m_localAxisC, b2GearJoint.SolvePositionConstraints_s_u);
            const i = b2Rot.MulRV(a, this.m_lalcC, b2GearJoint.SolvePositionConstraints_s_rC);
            const h = b2Rot.MulRV(V, this.m_lalcA, b2GearJoint.SolvePositionConstraints_s_rA);
            R.Copy(t);
            j = b2Vec2.CrossVV(i, t);
            p = b2Vec2.CrossVV(h, t);
            u += this.m_mC + this.m_mA + this.m_iC * j * j + this.m_iA * p * p;
            const e = this.m_lalcC;
            const o = b2Rot.MulTRV(a, b2Vec2.AddVV(h, b2Vec2.SubVV(s, n, b2Vec2.s_t0), b2Vec2.s_t0), b2Vec2.s_t0);
            G = b2Vec2.DotVV(b2Vec2.SubVV(o, e, b2Vec2.s_t0), this.m_localAxisC);
        }
        if (this.m_typeB === b2JointType.e_revoluteJoint) {
            d.SetZero();
            f = this.m_ratio;
            y = this.m_ratio;
            u += this.m_ratio * this.m_ratio * (this.m_iB + this.m_iD);
            w = e - c - this.m_referenceAngleB;
        } else {
            const t = b2Rot.MulRV(J, this.m_localAxisD, b2GearJoint.SolvePositionConstraints_s_u);
            const s = b2Rot.MulRV(J, this.m_lalcD, b2GearJoint.SolvePositionConstraints_s_rD);
            const i = b2Rot.MulRV(r, this.m_lalcB, b2GearJoint.SolvePositionConstraints_s_rB);
            b2Vec2.MulSV(this.m_ratio, t, d);
            y = this.m_ratio * b2Vec2.CrossVV(s, t);
            f = this.m_ratio * b2Vec2.CrossVV(i, t);
            u += this.m_ratio * this.m_ratio * (this.m_mD + this.m_mB) + this.m_iD * y * y + this.m_iB * f * f;
            const e = this.m_lalcD;
            const n = b2Rot.MulTRV(J, b2Vec2.AddVV(i, b2Vec2.SubVV(h, b, b2Vec2.s_t0), b2Vec2.s_t0), b2Vec2.s_t0);
            w = b2Vec2.DotVV(b2Vec2.SubVV(n, e, b2Vec2.s_t0), this.m_localAxisD);
        }
        const m = G + this.m_ratio * w - this.m_constant;
        let T = 0;
        if (u > 0) {
            T = -m / u;
        }
        s.SelfMulAdd(this.m_mA * T, R);
        i += this.m_iA * T * p;
        h.SelfMulAdd(this.m_mB * T, d);
        e += this.m_iB * T * f;
        n.SelfMulSub(this.m_mC * T, R);
        o -= this.m_iC * T * j;
        b.SelfMulSub(this.m_mD * T, d);
        c -= this.m_iD * T * y;
        t.positions[this.m_indexA].a = i;
        t.positions[this.m_indexB].a = e;
        t.positions[this.m_indexC].a = o;
        t.positions[this.m_indexD].a = c;
        return l < b2_linearSlop;
    }
    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) {
        return b2Vec2.MulSV(t * this.m_impulse, this.m_JvAC, s);
    }
    GetReactionTorque(t) {
        return t * this.m_impulse * this.m_JwA;
    }
    GetJoint1() {
        return this.m_joint1;
    }
    GetJoint2() {
        return this.m_joint2;
    }
    GetRatio() {
        return this.m_ratio;
    }
    SetRatio(t) {
        this.m_ratio = t;
    }
    Dump(t) {
        const s = this.m_bodyA.m_islandIndex;
        const i = this.m_bodyB.m_islandIndex;
        const h = this.m_joint1.m_index;
        const e = this.m_joint2.m_index;
        t("  const jd: b2GearJointDef = new b2GearJointDef();\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.joint1 = joints[%d];\n", h);
        t("  jd.joint2 = joints[%d];\n", e);
        t("  jd.ratio = %.15f;\n", this.m_ratio);
        t("  joints[%d] = this.m_world.CreateJoint(jd);\n", this.m_index);
    }
}

b2GearJoint.InitVelocityConstraints_s_u = new b2Vec2;

b2GearJoint.InitVelocityConstraints_s_rA = new b2Vec2;

b2GearJoint.InitVelocityConstraints_s_rB = new b2Vec2;

b2GearJoint.InitVelocityConstraints_s_rC = new b2Vec2;

b2GearJoint.InitVelocityConstraints_s_rD = new b2Vec2;

b2GearJoint.SolvePositionConstraints_s_u = new b2Vec2;

b2GearJoint.SolvePositionConstraints_s_rA = new b2Vec2;

b2GearJoint.SolvePositionConstraints_s_rB = new b2Vec2;

b2GearJoint.SolvePositionConstraints_s_rC = new b2Vec2;

b2GearJoint.SolvePositionConstraints_s_rD = new b2Vec2;