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

three.js based, unity like game engine for browser

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

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

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

export const b2_minPulleyLength = 2;

export class b2PulleyJointDef extends b2JointDef {
    constructor() {
        super(b2JointType.e_pulleyJoint);
        this.groundAnchorA = new b2Vec2(-1, 1);
        this.groundAnchorB = new b2Vec2(1, 1);
        this.localAnchorA = new b2Vec2(-1, 0);
        this.localAnchorB = new b2Vec2(1, 0);
        this.lengthA = 0;
        this.lengthB = 0;
        this.ratio = 1;
        this.collideConnected = true;
    }
    Initialize(t, s, i, h, e, n, o) {
        this.bodyA = t;
        this.bodyB = s;
        this.groundAnchorA.Copy(i);
        this.groundAnchorB.Copy(h);
        this.bodyA.GetLocalPoint(e, this.localAnchorA);
        this.bodyB.GetLocalPoint(n, this.localAnchorB);
        this.lengthA = b2Vec2.DistanceVV(e, i);
        this.lengthB = b2Vec2.DistanceVV(n, h);
        this.ratio = o;
    }
}

export class b2PulleyJoint extends b2Joint {
    constructor(t) {
        super(t);
        this.m_groundAnchorA = new b2Vec2;
        this.m_groundAnchorB = new b2Vec2;
        this.m_lengthA = 0;
        this.m_lengthB = 0;
        this.m_localAnchorA = new b2Vec2;
        this.m_localAnchorB = new b2Vec2;
        this.m_constant = 0;
        this.m_ratio = 0;
        this.m_impulse = 0;
        this.m_indexA = 0;
        this.m_indexB = 0;
        this.m_uA = new b2Vec2;
        this.m_uB = new b2Vec2;
        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 = 0;
        this.m_qA = new b2Rot;
        this.m_qB = new b2Rot;
        this.m_lalcA = new b2Vec2;
        this.m_lalcB = new b2Vec2;
        this.m_groundAnchorA.Copy(b2Maybe(t.groundAnchorA, new b2Vec2(-1, 1)));
        this.m_groundAnchorB.Copy(b2Maybe(t.groundAnchorB, new b2Vec2(1, 0)));
        this.m_localAnchorA.Copy(b2Maybe(t.localAnchorA, new b2Vec2(-1, 0)));
        this.m_localAnchorB.Copy(b2Maybe(t.localAnchorB, new b2Vec2(1, 0)));
        this.m_lengthA = b2Maybe(t.lengthA, 0);
        this.m_lengthB = b2Maybe(t.lengthB, 0);
        this.m_ratio = b2Maybe(t.ratio, 1);
        this.m_constant = b2Maybe(t.lengthA, 0) + this.m_ratio * b2Maybe(t.lengthB, 0);
        this.m_impulse = 0;
    }
    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 e = t.velocities[this.m_indexA].w;
        const n = t.positions[this.m_indexB].c;
        const o = t.positions[this.m_indexB].a;
        const b = t.velocities[this.m_indexB].v;
        let c = t.velocities[this.m_indexB].w;
        const l = this.m_qA.SetAngle(i), r = this.m_qB.SetAngle(o);
        b2Vec2.SubVV(this.m_localAnchorA, this.m_localCenterA, this.m_lalcA);
        b2Rot.MulRV(l, 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_uA.Copy(s).SelfAdd(this.m_rA).SelfSub(this.m_groundAnchorA);
        this.m_uB.Copy(n).SelfAdd(this.m_rB).SelfSub(this.m_groundAnchorB);
        const V = this.m_uA.Length();
        const u = this.m_uB.Length();
        if (V > 10 * b2_linearSlop) {
            this.m_uA.SelfMul(1 / V);
        } else {
            this.m_uA.SetZero();
        }
        if (u > 10 * b2_linearSlop) {
            this.m_uB.SelfMul(1 / u);
        } else {
            this.m_uB.SetZero();
        }
        const y = b2Vec2.CrossVV(this.m_rA, this.m_uA);
        const w = b2Vec2.CrossVV(this.m_rB, this.m_uB);
        const a = this.m_invMassA + this.m_invIA * y * y;
        const J = this.m_invMassB + this.m_invIB * w * w;
        this.m_mass = a + this.m_ratio * this.m_ratio * J;
        if (this.m_mass > 0) {
            this.m_mass = 1 / this.m_mass;
        }
        if (t.step.warmStarting) {
            this.m_impulse *= t.step.dtRatio;
            const s = b2Vec2.MulSV(-this.m_impulse, this.m_uA, b2PulleyJoint.InitVelocityConstraints_s_PA);
            const i = b2Vec2.MulSV(-this.m_ratio * this.m_impulse, this.m_uB, b2PulleyJoint.InitVelocityConstraints_s_PB);
            h.SelfMulAdd(this.m_invMassA, s);
            e += this.m_invIA * b2Vec2.CrossVV(this.m_rA, s);
            b.SelfMulAdd(this.m_invMassB, i);
            c += this.m_invIB * b2Vec2.CrossVV(this.m_rB, i);
        } else {
            this.m_impulse = 0;
        }
        t.velocities[this.m_indexA].w = e;
        t.velocities[this.m_indexB].w = c;
    }
    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 = b2Vec2.AddVCrossSV(s, i, this.m_rA, b2PulleyJoint.SolveVelocityConstraints_s_vpA);
        const o = b2Vec2.AddVCrossSV(h, e, this.m_rB, b2PulleyJoint.SolveVelocityConstraints_s_vpB);
        const b = -b2Vec2.DotVV(this.m_uA, n) - this.m_ratio * b2Vec2.DotVV(this.m_uB, o);
        const c = -this.m_mass * b;
        this.m_impulse += c;
        const l = b2Vec2.MulSV(-c, this.m_uA, b2PulleyJoint.SolveVelocityConstraints_s_PA);
        const r = b2Vec2.MulSV(-this.m_ratio * c, this.m_uB, b2PulleyJoint.SolveVelocityConstraints_s_PB);
        s.SelfMulAdd(this.m_invMassA, l);
        i += this.m_invIA * b2Vec2.CrossVV(this.m_rA, l);
        h.SelfMulAdd(this.m_invMassB, r);
        e += this.m_invIB * b2Vec2.CrossVV(this.m_rB, r);
        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);
        b2Vec2.SubVV(this.m_localAnchorA, this.m_localCenterA, this.m_lalcA);
        const b = b2Rot.MulRV(n, this.m_lalcA, this.m_rA);
        b2Vec2.SubVV(this.m_localAnchorB, this.m_localCenterB, this.m_lalcB);
        const c = b2Rot.MulRV(o, this.m_lalcB, this.m_rB);
        const l = this.m_uA.Copy(s).SelfAdd(b).SelfSub(this.m_groundAnchorA);
        const r = this.m_uB.Copy(h).SelfAdd(c).SelfSub(this.m_groundAnchorB);
        const V = l.Length();
        const u = r.Length();
        if (V > 10 * b2_linearSlop) {
            l.SelfMul(1 / V);
        } else {
            l.SetZero();
        }
        if (u > 10 * b2_linearSlop) {
            r.SelfMul(1 / u);
        } else {
            r.SetZero();
        }
        const y = b2Vec2.CrossVV(b, l);
        const w = b2Vec2.CrossVV(c, r);
        const a = this.m_invMassA + this.m_invIA * y * y;
        const J = this.m_invMassB + this.m_invIB * w * w;
        let d = a + this.m_ratio * this.m_ratio * J;
        if (d > 0) {
            d = 1 / d;
        }
        const f = this.m_constant - V - this.m_ratio * u;
        const P = b2Abs(f);
        const A = -d * f;
        const j = b2Vec2.MulSV(-A, l, b2PulleyJoint.SolvePositionConstraints_s_PA);
        const p = b2Vec2.MulSV(-this.m_ratio * A, r, b2PulleyJoint.SolvePositionConstraints_s_PB);
        s.SelfMulAdd(this.m_invMassA, j);
        i += this.m_invIA * b2Vec2.CrossVV(b, j);
        h.SelfMulAdd(this.m_invMassB, p);
        e += this.m_invIB * b2Vec2.CrossVV(c, p);
        t.positions[this.m_indexA].a = i;
        t.positions[this.m_indexB].a = e;
        return P < 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) {
        s.x = t * this.m_impulse * this.m_uB.x;
        s.y = t * this.m_impulse * this.m_uB.y;
        return s;
    }
    GetReactionTorque(t) {
        return 0;
    }
    GetGroundAnchorA() {
        return this.m_groundAnchorA;
    }
    GetGroundAnchorB() {
        return this.m_groundAnchorB;
    }
    GetLengthA() {
        return this.m_lengthA;
    }
    GetLengthB() {
        return this.m_lengthB;
    }
    GetRatio() {
        return this.m_ratio;
    }
    GetCurrentLengthA() {
        const t = this.m_bodyA.GetWorldPoint(this.m_localAnchorA, b2PulleyJoint.GetCurrentLengthA_s_p);
        const s = this.m_groundAnchorA;
        return b2Vec2.DistanceVV(t, s);
    }
    GetCurrentLengthB() {
        const t = this.m_bodyB.GetWorldPoint(this.m_localAnchorB, b2PulleyJoint.GetCurrentLengthB_s_p);
        const s = this.m_groundAnchorB;
        return b2Vec2.DistanceVV(t, s);
    }
    Dump(t) {
        const s = this.m_bodyA.m_islandIndex;
        const i = this.m_bodyB.m_islandIndex;
        t("  const jd: b2PulleyJointDef = new b2PulleyJointDef();\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.groundAnchorA.Set(%.15f, %.15f);\n", this.m_groundAnchorA.x, this.m_groundAnchorA.y);
        t("  jd.groundAnchorB.Set(%.15f, %.15f);\n", this.m_groundAnchorB.x, this.m_groundAnchorB.y);
        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.lengthA = %.15f;\n", this.m_lengthA);
        t("  jd.lengthB = %.15f;\n", this.m_lengthB);
        t("  jd.ratio = %.15f;\n", this.m_ratio);
        t("  joints[%d] = this.m_world.CreateJoint(jd);\n", this.m_index);
    }
    ShiftOrigin(t) {
        this.m_groundAnchorA.SelfSub(t);
        this.m_groundAnchorB.SelfSub(t);
    }
}

b2PulleyJoint.InitVelocityConstraints_s_PA = new b2Vec2;

b2PulleyJoint.InitVelocityConstraints_s_PB = new b2Vec2;

b2PulleyJoint.SolveVelocityConstraints_s_vpA = new b2Vec2;

b2PulleyJoint.SolveVelocityConstraints_s_vpB = new b2Vec2;

b2PulleyJoint.SolveVelocityConstraints_s_PA = new b2Vec2;

b2PulleyJoint.SolveVelocityConstraints_s_PB = new b2Vec2;

b2PulleyJoint.SolvePositionConstraints_s_PA = new b2Vec2;

b2PulleyJoint.SolvePositionConstraints_s_PB = new b2Vec2;

b2PulleyJoint.GetCurrentLengthA_s_p = new b2Vec2;

b2PulleyJoint.GetCurrentLengthB_s_p = new b2Vec2;