Extending Phaser 3 Geom.Rectangle to Box2D-Lite, Part XX: Angle Constraint

The AngleJoint class represents a joint connecting two bodies limiting their angles. This is similar to the RevoluteJoint when limits are enforced. When the angle limits are equal this joint will force both body’s angles to be identical. Regardless of the limits, the bodies are allowed to translate freely.

The angular limits are relative to the initial angle between the two bodies.

class AngleJoint extends Joint {

    constructor(b1, b2, frequency = 60, dampingRatio = 1.0, jointMass = -1) {
        super(b1, b2, frequency, dampingRatio, jointMass);

        this.impulseSum = 0.0;
        this.initialAngleOffset = b2.rotation - b1.rotation;

    }

  preStep(inv_dt) {
        // Calculate Jacobian J and effective mass M
        // J = [0 -1 0 1]
        // M = (J · M^-1 · J^t)^-1
        let k = this.body1.invI + this.body2.invI + this.gamma;
        this.m = 1.0 / k;
        let error = this.body2.rotation - this.body1.rotation - this.initialAngleOffset;
        if (Settings.positionCorrection)
            this.bias = error * this.beta * inv_dt;
        else
            this.bias = 0.0;
        if (Settings.warmStarting)
            this.applyImpulse(this.impulseSum);
    }

  solve() {
        // Calculate corrective impulse: Pc
        // Pc = J^t · λ (λ: lagrangian multiplier)
        // λ = (J · M^-1 · J^t)^-1 ⋅ -(J·v+b)
        let jv = this.body2.angularVelocity - this.body1.angularVelocity;
        // Check out below for the reason why the (accumulated impulse * gamma) term is on the right hand side
        // https://pybullet.org/Bullet/phpBB3/viewtopic.php?f=4&t=1354
        let lambda = this.m * -(jv + this.bias + this.impulseSum * this.gamma);
        this.applyImpulse(lambda);
        if (Settings.warmStarting)
            this.impulseSum += lambda;
    }