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

three.js based, unity like game engine for browser

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

import { b2Abs, b2Clamp, b2Vec2, b2Mat22, 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 b2RevoluteJointDef extends b2JointDef {
    constructor() {
        super(b2JointType.e_revoluteJoint);
        this.localAnchorA = new b2Vec2(0, 0);
        this.localAnchorB = new b2Vec2(0, 0);
        this.referenceAngle = 0;
        this.enableLimit = false;
        this.lowerAngle = 0;
        this.upperAngle = 0;
        this.enableMotor = false;
        this.motorSpeed = 0;
        this.maxMotorTorque = 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 b2RevoluteJoint extends b2Joint {
    constructor(t) {
        super(t);
        this.m_localAnchorA = new b2Vec2;
        this.m_localAnchorB = new b2Vec2;
        this.m_impulse = new b2Vec2;
        this.m_motorImpulse = 0;
        this.m_lowerImpulse = 0;
        this.m_upperImpulse = 0;
        this.m_enableMotor = false;
        this.m_maxMotorTorque = 0;
        this.m_motorSpeed = 0;
        this.m_enableLimit = false;
        this.m_referenceAngle = 0;
        this.m_lowerAngle = 0;
        this.m_upperAngle = 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_K = new b2Mat22;
        this.m_angle = 0;
        this.m_axialMass = 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_referenceAngle = b2Maybe(t.referenceAngle, 0);
        this.m_impulse.SetZero();
        this.m_motorImpulse = 0;
        this.m_lowerAngle = b2Maybe(t.lowerAngle, 0);
        this.m_upperAngle = b2Maybe(t.upperAngle, 0);
        this.m_maxMotorTorque = b2Maybe(t.maxMotorTorque, 0);
        this.m_motorSpeed = b2Maybe(t.motorSpeed, 0);
        this.m_enableLimit = b2Maybe(t.enableLimit, false);
        this.m_enableMotor = b2Maybe(t.enableMotor, false);
    }
    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 e = t.velocities[this.m_indexA].w;
        const h = t.positions[this.m_indexB].a;
        const o = t.velocities[this.m_indexB].v;
        let n = t.velocities[this.m_indexB].w;
        const b = this.m_qA.SetAngle(s), c = this.m_qB.SetAngle(h);
        b2Vec2.SubVV(this.m_localAnchorA, this.m_localCenterA, this.m_lalcA);
        b2Rot.MulRV(b, this.m_lalcA, this.m_rA);
        b2Vec2.SubVV(this.m_localAnchorB, this.m_localCenterB, this.m_lalcB);
        b2Rot.MulRV(c, this.m_lalcB, this.m_rB);
        const r = this.m_invMassA, l = this.m_invMassB;
        const a = this.m_invIA, u = this.m_invIB;
        this.m_K.ex.x = r + l + this.m_rA.y * this.m_rA.y * a + this.m_rB.y * this.m_rB.y * u;
        this.m_K.ey.x = -this.m_rA.y * this.m_rA.x * a - this.m_rB.y * this.m_rB.x * u;
        this.m_K.ex.y = this.m_K.ey.x;
        this.m_K.ey.y = r + l + this.m_rA.x * this.m_rA.x * a + this.m_rB.x * this.m_rB.x * u;
        this.m_axialMass = a + u;
        let V;
        if (this.m_axialMass > 0) {
            this.m_axialMass = 1 / this.m_axialMass;
            V = false;
        } else {
            V = true;
        }
        this.m_angle = h - s - this.m_referenceAngle;
        if (this.m_enableLimit === false || V) {
            this.m_lowerImpulse = 0;
            this.m_upperImpulse = 0;
        }
        if (this.m_enableMotor === false || V) {
            this.m_motorImpulse = 0;
        }
        if (t.step.warmStarting) {
            this.m_impulse.SelfMul(t.step.dtRatio);
            this.m_motorImpulse *= t.step.dtRatio;
            this.m_lowerImpulse *= t.step.dtRatio;
            this.m_upperImpulse *= t.step.dtRatio;
            const s = this.m_motorImpulse + this.m_lowerImpulse - this.m_upperImpulse;
            const h = b2RevoluteJoint.InitVelocityConstraints_s_P.Set(this.m_impulse.x, this.m_impulse.y);
            i.SelfMulSub(r, h);
            e -= a * (b2Vec2.CrossVV(this.m_rA, h) + s);
            o.SelfMulAdd(l, h);
            n += u * (b2Vec2.CrossVV(this.m_rB, h) + s);
        } else {
            this.m_impulse.SetZero();
            this.m_motorImpulse = 0;
            this.m_lowerImpulse = 0;
            this.m_upperImpulse = 0;
        }
        t.velocities[this.m_indexA].w = e;
        t.velocities[this.m_indexB].w = n;
    }
    SolveVelocityConstraints(t) {
        const s = t.velocities[this.m_indexA].v;
        let i = t.velocities[this.m_indexA].w;
        const e = t.velocities[this.m_indexB].v;
        let h = t.velocities[this.m_indexB].w;
        const o = this.m_invMassA, n = this.m_invMassB;
        const b = this.m_invIA, c = this.m_invIB;
        const r = b + c === 0;
        if (this.m_enableMotor && !r) {
            const s = h - i - this.m_motorSpeed;
            let e = -this.m_axialMass * s;
            const o = this.m_motorImpulse;
            const n = t.step.dt * this.m_maxMotorTorque;
            this.m_motorImpulse = b2Clamp(this.m_motorImpulse + e, -n, n);
            e = this.m_motorImpulse - o;
            i -= b * e;
            h += c * e;
        }
        if (this.m_enableLimit && !r) {
            {
                const s = this.m_angle - this.m_lowerAngle;
                const e = h - i;
                let o = -this.m_axialMass * (e + b2Max(s, 0) * t.step.inv_dt);
                const n = this.m_lowerImpulse;
                this.m_lowerImpulse = b2Max(this.m_lowerImpulse + o, 0);
                o = this.m_lowerImpulse - n;
                i -= b * o;
                h += c * o;
            }
            {
                const s = this.m_upperAngle - this.m_angle;
                const e = i - h;
                let o = -this.m_axialMass * (e + b2Max(s, 0) * t.step.inv_dt);
                const n = this.m_upperImpulse;
                this.m_upperImpulse = b2Max(this.m_upperImpulse + o, 0);
                o = this.m_upperImpulse - n;
                i += b * o;
                h -= c * o;
            }
        }
        {
            const t = b2Vec2.SubVV(b2Vec2.AddVCrossSV(e, h, this.m_rB, b2Vec2.s_t0), b2Vec2.AddVCrossSV(s, i, this.m_rA, b2Vec2.s_t1), b2RevoluteJoint.SolveVelocityConstraints_s_Cdot_v2);
            const r = this.m_K.Solve(-t.x, -t.y, b2RevoluteJoint.SolveVelocityConstraints_s_impulse_v2);
            this.m_impulse.x += r.x;
            this.m_impulse.y += r.y;
            s.SelfMulSub(o, r);
            i -= b * b2Vec2.CrossVV(this.m_rA, r);
            e.SelfMulAdd(n, r);
            h += c * b2Vec2.CrossVV(this.m_rB, r);
        }
        t.velocities[this.m_indexA].w = i;
        t.velocities[this.m_indexB].w = h;
    }
    SolvePositionConstraints(t) {
        const s = t.positions[this.m_indexA].c;
        let i = t.positions[this.m_indexA].a;
        const e = t.positions[this.m_indexB].c;
        let h = t.positions[this.m_indexB].a;
        const o = this.m_qA.SetAngle(i), n = this.m_qB.SetAngle(h);
        let b = 0;
        let c = 0;
        const r = this.m_invIA + this.m_invIB === 0;
        if (this.m_enableLimit && !r) {
            const t = h - i - this.m_referenceAngle;
            let s = 0;
            if (b2Abs(this.m_upperAngle - this.m_lowerAngle) < 2 * b2_angularSlop) {
                s = b2Clamp(t - this.m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
            } else if (t <= this.m_lowerAngle) {
                s = b2Clamp(t - this.m_lowerAngle + b2_angularSlop, -b2_maxAngularCorrection, 0);
            } else if (t >= this.m_upperAngle) {
                s = b2Clamp(t - this.m_upperAngle - b2_angularSlop, 0, b2_maxAngularCorrection);
            }
            const e = -this.m_axialMass * s;
            i -= this.m_invIA * e;
            h += this.m_invIB * e;
            b = b2Abs(s);
        }
        {
            o.SetAngle(i);
            n.SetAngle(h);
            b2Vec2.SubVV(this.m_localAnchorA, this.m_localCenterA, this.m_lalcA);
            const t = b2Rot.MulRV(o, this.m_lalcA, this.m_rA);
            b2Vec2.SubVV(this.m_localAnchorB, this.m_localCenterB, this.m_lalcB);
            const b = b2Rot.MulRV(n, this.m_lalcB, this.m_rB);
            const r = b2Vec2.SubVV(b2Vec2.AddVV(e, b, b2Vec2.s_t0), b2Vec2.AddVV(s, t, b2Vec2.s_t1), b2RevoluteJoint.SolvePositionConstraints_s_C_v2);
            c = r.Length();
            const l = this.m_invMassA, a = this.m_invMassB;
            const u = this.m_invIA, V = this.m_invIB;
            const f = this.m_K;
            f.ex.x = l + a + u * t.y * t.y + V * b.y * b.y;
            f.ex.y = -u * t.x * t.y - V * b.x * b.y;
            f.ey.x = f.ex.y;
            f.ey.y = l + a + u * t.x * t.x + V * b.x * b.x;
            const R = f.Solve(r.x, r.y, b2RevoluteJoint.SolvePositionConstraints_s_impulse).SelfNeg();
            s.SelfMulSub(l, R);
            i -= u * b2Vec2.CrossVV(t, R);
            e.SelfMulAdd(a, R);
            h += V * b2Vec2.CrossVV(b, R);
        }
        t.positions[this.m_indexA].a = i;
        t.positions[this.m_indexB].a = h;
        return c <= b2_linearSlop && b <= 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_lowerImpulse - this.m_upperImpulse);
    }
    GetLocalAnchorA() {
        return this.m_localAnchorA;
    }
    GetLocalAnchorB() {
        return this.m_localAnchorB;
    }
    GetReferenceAngle() {
        return this.m_referenceAngle;
    }
    GetJointAngle() {
        return this.m_bodyB.m_sweep.a - this.m_bodyA.m_sweep.a - this.m_referenceAngle;
    }
    GetJointSpeed() {
        return this.m_bodyB.m_angularVelocity - this.m_bodyA.m_angularVelocity;
    }
    IsMotorEnabled() {
        return this.m_enableMotor;
    }
    EnableMotor(t) {
        if (t !== this.m_enableMotor) {
            this.m_bodyA.SetAwake(true);
            this.m_bodyB.SetAwake(true);
            this.m_enableMotor = t;
        }
    }
    GetMotorTorque(t) {
        return t * this.m_motorImpulse;
    }
    GetMotorSpeed() {
        return this.m_motorSpeed;
    }
    SetMaxMotorTorque(t) {
        if (t !== this.m_maxMotorTorque) {
            this.m_bodyA.SetAwake(true);
            this.m_bodyB.SetAwake(true);
            this.m_maxMotorTorque = t;
        }
    }
    GetMaxMotorTorque() {
        return this.m_maxMotorTorque;
    }
    IsLimitEnabled() {
        return this.m_enableLimit;
    }
    EnableLimit(t) {
        if (t !== this.m_enableLimit) {
            this.m_bodyA.SetAwake(true);
            this.m_bodyB.SetAwake(true);
            this.m_enableLimit = t;
            this.m_lowerImpulse = 0;
            this.m_upperImpulse = 0;
        }
    }
    GetLowerLimit() {
        return this.m_lowerAngle;
    }
    GetUpperLimit() {
        return this.m_upperAngle;
    }
    SetLimits(t, s) {
        if (t !== this.m_lowerAngle || s !== this.m_upperAngle) {
            this.m_bodyA.SetAwake(true);
            this.m_bodyB.SetAwake(true);
            this.m_lowerImpulse = 0;
            this.m_upperImpulse = 0;
            this.m_lowerAngle = t;
            this.m_upperAngle = s;
        }
    }
    SetMotorSpeed(t) {
        if (t !== this.m_motorSpeed) {
            this.m_bodyA.SetAwake(true);
            this.m_bodyB.SetAwake(true);
            this.m_motorSpeed = t;
        }
    }
    Dump(t) {
        const s = this.m_bodyA.m_islandIndex;
        const i = this.m_bodyB.m_islandIndex;
        t("  const jd: b2RevoluteJointDef = new b2RevoluteJointDef();\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.enableLimit = %s;\n", this.m_enableLimit ? "true" : "false");
        t("  jd.lowerAngle = %.15f;\n", this.m_lowerAngle);
        t("  jd.upperAngle = %.15f;\n", this.m_upperAngle);
        t("  jd.enableMotor = %s;\n", this.m_enableMotor ? "true" : "false");
        t("  jd.motorSpeed = %.15f;\n", this.m_motorSpeed);
        t("  jd.maxMotorTorque = %.15f;\n", this.m_maxMotorTorque);
        t("  joints[%d] = this.m_world.CreateJoint(jd);\n", this.m_index);
    }
    Draw(t) {
        const s = this.m_bodyA.GetTransform();
        const i = this.m_bodyB.GetTransform();
        const e = b2Transform.MulXV(s, this.m_localAnchorA, b2RevoluteJoint.Draw_s_pA);
        const h = b2Transform.MulXV(i, this.m_localAnchorB, b2RevoluteJoint.Draw_s_pB);
        const o = b2RevoluteJoint.Draw_s_c1;
        const n = b2RevoluteJoint.Draw_s_c2;
        const b = b2RevoluteJoint.Draw_s_c3;
        const c = b2RevoluteJoint.Draw_s_c4;
        const r = b2RevoluteJoint.Draw_s_c5;
        t.DrawPoint(e, 5, c);
        t.DrawPoint(h, 5, r);
        const l = this.m_bodyA.GetAngle();
        const a = this.m_bodyB.GetAngle();
        const u = a - l - this.m_referenceAngle;
        const V = .5;
        const f = b2RevoluteJoint.Draw_s_r.Set(V * Math.cos(u), V * Math.sin(u));
        t.DrawSegment(h, b2Vec2.AddVV(h, f, b2Vec2.s_t0), o);
        t.DrawCircle(h, V, o);
        if (this.m_enableLimit) {
            const s = b2RevoluteJoint.Draw_s_rlo.Set(V * Math.cos(this.m_lowerAngle), V * Math.sin(this.m_lowerAngle));
            const i = b2RevoluteJoint.Draw_s_rhi.Set(V * Math.cos(this.m_upperAngle), V * Math.sin(this.m_upperAngle));
            t.DrawSegment(h, b2Vec2.AddVV(h, s, b2Vec2.s_t0), n);
            t.DrawSegment(h, b2Vec2.AddVV(h, i, b2Vec2.s_t0), b);
        }
        const R = b2RevoluteJoint.Draw_s_color_;
        t.DrawSegment(s.p, e, R);
        t.DrawSegment(e, h, R);
        t.DrawSegment(i.p, h, R);
    }
}

b2RevoluteJoint.InitVelocityConstraints_s_P = new b2Vec2;

b2RevoluteJoint.SolveVelocityConstraints_s_Cdot_v2 = new b2Vec2;

b2RevoluteJoint.SolveVelocityConstraints_s_impulse_v2 = new b2Vec2;

b2RevoluteJoint.SolvePositionConstraints_s_C_v2 = new b2Vec2;

b2RevoluteJoint.SolvePositionConstraints_s_impulse = new b2Vec2;

b2RevoluteJoint.Draw_s_pA = new b2Vec2;

b2RevoluteJoint.Draw_s_pB = new b2Vec2;

b2RevoluteJoint.Draw_s_c1 = new b2Color(.7, .7, .7);

b2RevoluteJoint.Draw_s_c2 = new b2Color(.3, .9, .3);

b2RevoluteJoint.Draw_s_c3 = new b2Color(.9, .3, .3);

b2RevoluteJoint.Draw_s_c4 = new b2Color(.3, .3, .9);

b2RevoluteJoint.Draw_s_c5 = new b2Color(.4, .4, .4);

b2RevoluteJoint.Draw_s_color_ = new b2Color(.5, .8, .8);

b2RevoluteJoint.Draw_s_r = new b2Vec2;

b2RevoluteJoint.Draw_s_rlo = new b2Vec2;

b2RevoluteJoint.Draw_s_rhi = new b2Vec2;