planck-v2 2.0.0

package/src/dynamics/joint/WheelJoint.ts

@@ -0,0 +1,683 @@
+/*
+ * Planck.js
+ *
+ * Copyright (c) Erin Catto, Ali Shakiba
+ *
+ * This source code is licensed under the MIT license found in the
+ * LICENSE file in the root directory of this source tree.
+ */
+
+import { options } from "../../util/options";
+import { SettingsInternal as Settings } from "../../Settings";
+import * as geo from "../../common/Geo";
+import { clamp } from "../../common/Math";
+import { Vec2Value } from "../../common/Vec2";
+import { Joint, JointOpt, JointDef } from "../Joint";
+import { Body } from "../Body";
+import { TimeStep } from "../Solver";
+
+/** @internal */ const math_abs = Math.abs;
+/** @internal */ const math_PI = Math.PI;
+
+/**
+ * Wheel joint definition. This requires defining a line of motion using an axis
+ * and an anchor point. The definition uses local anchor points and a local axis
+ * so that the initial configuration can violate the constraint slightly. The
+ * joint translation is zero when the local anchor points coincide in world
+ * space. Using local anchors and a local axis helps when saving and loading a
+ * game.
+ */
+export interface WheelJointOpt extends JointOpt {
+  /**
+   * Enable/disable the joint motor.
+   */
+  enableMotor?: boolean;
+  /**
+   * The maximum motor torque, usually in N-m.
+   */
+  maxMotorTorque?: number;
+  /**
+   * The desired motor speed in radians per second.
+   */
+  motorSpeed?: number;
+  /**
+   * Suspension frequency, zero indicates no suspension.
+   */
+  frequencyHz?: number;
+  /**
+   * Suspension damping ratio, one indicates critical damping.
+   */
+  dampingRatio?: number;
+}
+
+/**
+ * Wheel joint definition. This requires defining a line of motion using an axis
+ * and an anchor point. The definition uses local anchor points and a local axis
+ * so that the initial configuration can violate the constraint slightly. The
+ * joint translation is zero when the local anchor points coincide in world
+ * space. Using local anchors and a local axis helps when saving and loading a
+ * game.
+ */
+export interface WheelJointDef extends JointDef, WheelJointOpt {
+  /**
+   * The local anchor point relative to bodyA's origin.
+   */
+  localAnchorA: Vec2Value;
+  /**
+   * The local anchor point relative to bodyB's origin.
+   */
+  localAnchorB: Vec2Value;
+  /**
+   * The local translation axis in bodyA.
+   */
+  localAxisA: Vec2Value;
+
+  /** @internal renamed to localAxisA */
+  localAxis?: Vec2Value;
+
+  /** @internal */ anchorA?: Vec2Value;
+  /** @internal */ anchorB?: Vec2Value;
+}
+
+/** @internal */ const DEFAULTS = {
+  enableMotor: false,
+  maxMotorTorque: 0.0,
+  motorSpeed: 0.0,
+  frequencyHz: 2.0,
+  dampingRatio: 0.7,
+};
+
+/**
+ * A wheel joint. This joint provides two degrees of freedom: translation along
+ * an axis fixed in bodyA and rotation in the plane. In other words, it is a
+ * point to line constraint with a rotational motor and a linear spring/damper.
+ * This joint is designed for vehicle suspensions.
+ */
+export class WheelJoint extends Joint {
+  static TYPE = "wheel-joint" as const;
+
+  /** @internal */ m_type: "wheel-joint";
+  /** @internal */ m_localAnchorA: Vec2Value;
+  /** @internal */ m_localAnchorB: Vec2Value;
+  /** @internal */ m_localXAxisA: Vec2Value;
+  /** @internal */ m_localYAxisA: Vec2Value;
+
+  /** @internal */ m_mass: number;
+  /** @internal */ m_impulse: number;
+  /** @internal */ m_motorMass: number;
+  /** @internal */ m_motorImpulse: number;
+  /** @internal */ m_springMass: number;
+  /** @internal */ m_springImpulse: number;
+
+  /** @internal */ m_maxMotorTorque: number;
+  /** @internal */ m_motorSpeed: number;
+  /** @internal */ m_enableMotor: boolean;
+
+  /** @internal */ m_frequencyHz: number;
+  /** @internal */ m_dampingRatio: number;
+
+  /** @internal */ m_bias: number;
+  /** @internal */ m_gamma: number;
+
+  // Solver temp
+  /** @internal */ m_localCenterA: Vec2Value;
+  /** @internal */ m_localCenterB: Vec2Value;
+  /** @internal */ m_invMassA: number;
+  /** @internal */ m_invMassB: number;
+  /** @internal */ m_invIA: number;
+  /** @internal */ m_invIB: number;
+
+  /** @internal */ m_ax: Vec2Value;
+  /** @internal */ m_ay: Vec2Value;
+  /** @internal */ m_sAx: number;
+  /** @internal */ m_sBx: number;
+  /** @internal */ m_sAy: number;
+  /** @internal */ m_sBy: number;
+
+  constructor(def: WheelJointDef);
+  constructor(def: WheelJointOpt, bodyA: Body, bodyB: Body, anchor?: Vec2Value, axis?: Vec2Value);
+  constructor(def: WheelJointDef, bodyA?: Body, bodyB?: Body, anchor?: Vec2Value, axis?: Vec2Value) {
+    def = options(def, DEFAULTS);
+    super(def, bodyA, bodyB);
+    bodyA = this.m_bodyA;
+    bodyB = this.m_bodyB;
+
+    this.m_ax = geo.vec2(0, 0);
+    this.m_ay = geo.vec2(0, 0);
+
+    this.m_type = WheelJoint.TYPE;
+
+    this.m_localAnchorA = geo.vec2(0, 0);
+    this.m_localAnchorB = geo.vec2(0, 0);
+
+    if (anchor) {
+      geo.copyVec2(this.m_localAnchorA, bodyA.getLocalPoint(anchor));
+    } else if (geo.isVec2(def.localAnchorA)) {
+      geo.copyVec2(this.m_localAnchorA, def.localAnchorA);
+    }
+
+    if (anchor) {
+      geo.copyVec2(this.m_localAnchorB, bodyB.getLocalPoint(anchor));
+    } else if (geo.isVec2(def.localAnchorB)) {
+      geo.copyVec2(this.m_localAnchorB, def.localAnchorB);
+    }
+
+    this.m_localXAxisA = geo.vec2(1.0, 0.0);
+    if (geo.isVec2(axis)) {
+      geo.copyVec2(this.m_localXAxisA, bodyA.getLocalVector(axis));
+    } else if (geo.isVec2(def.localAxisA)) {
+      geo.copyVec2(this.m_localXAxisA, def.localAxisA);
+    } else if (geo.isVec2(def.localAxis)) {
+      // localAxis is renamed to localAxisA, this is for backward compatibility
+      geo.copyVec2(this.m_localXAxisA, def.localAxis);
+    }
+
+    this.m_localYAxisA = geo.vec2(0, 0);
+    geo.crossNumVec2(this.m_localYAxisA, 1.0, this.m_localXAxisA);
+
+    this.m_mass = 0.0;
+    this.m_impulse = 0.0;
+    this.m_motorMass = 0.0;
+    this.m_motorImpulse = 0.0;
+    this.m_springMass = 0.0;
+    this.m_springImpulse = 0.0;
+
+    this.m_maxMotorTorque = def.maxMotorTorque;
+    this.m_motorSpeed = def.motorSpeed;
+    this.m_enableMotor = def.enableMotor;
+
+    this.m_frequencyHz = def.frequencyHz;
+    this.m_dampingRatio = def.dampingRatio;
+
+    this.m_bias = 0.0;
+    this.m_gamma = 0.0;
+
+    // Linear constraint (point-to-line)
+    // d = pB - pA = xB + rB - xA - rA
+    // C = dot(ay, d)
+    // Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA,
+    // rA))
+    // = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB,
+    // ay), vB)
+    // J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
+
+    // Spring linear constraint
+    // C = dot(ax, d)
+    // Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) +
+    // dot(cross(rB, ax), vB)
+    // J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
+
+    // Motor rotational constraint
+    // Cdot = wB - wA
+    // J = [0 0 -1 0 0 1]
+  }
+
+  /** @hidden */
+  _serialize(): object {
+    return {
+      type: this.m_type,
+      bodyA: this.m_bodyA,
+      bodyB: this.m_bodyB,
+      collideConnected: this.m_collideConnected,
+
+      enableMotor: this.m_enableMotor,
+      maxMotorTorque: this.m_maxMotorTorque,
+      motorSpeed: this.m_motorSpeed,
+      frequencyHz: this.m_frequencyHz,
+      dampingRatio: this.m_dampingRatio,
+
+      localAnchorA: this.m_localAnchorA,
+      localAnchorB: this.m_localAnchorB,
+      localAxisA: this.m_localXAxisA,
+    };
+  }
+
+  /** @hidden */
+  static _deserialize(data: any, world: any, restore: any): WheelJoint {
+    data = { ...data };
+    data.bodyA = restore(Body, data.bodyA, world);
+    data.bodyB = restore(Body, data.bodyB, world);
+    const joint = new WheelJoint(data);
+    return joint;
+  }
+
+  /** @hidden */
+  _reset(def: Partial<WheelJointDef>): void {
+    if (def.anchorA) {
+      geo.copyVec2(this.m_localAnchorA, this.m_bodyA.getLocalPoint(def.anchorA));
+    } else if (geo.isVec2(def.localAnchorA)) {
+      geo.copyVec2(this.m_localAnchorA, def.localAnchorA);
+    }
+    if (def.anchorB) {
+      geo.copyVec2(this.m_localAnchorB, this.m_bodyB.getLocalPoint(def.anchorB));
+    } else if (geo.isVec2(def.localAnchorB)) {
+      geo.copyVec2(this.m_localAnchorB, def.localAnchorB);
+    }
+    if (def.localAxisA) {
+      geo.copyVec2(this.m_localXAxisA, def.localAxisA);
+      geo.crossNumVec2(this.m_localYAxisA, 1.0, this.m_localXAxisA);
+    }
+    if (def.enableMotor !== undefined) {
+      this.m_enableMotor = def.enableMotor;
+    }
+    if (Number.isFinite(def.maxMotorTorque)) {
+      this.m_maxMotorTorque = def.maxMotorTorque;
+    }
+    if (Number.isFinite(def.motorSpeed)) {
+      this.m_motorSpeed = def.motorSpeed;
+    }
+    if (Number.isFinite(def.frequencyHz)) {
+      this.m_frequencyHz = def.frequencyHz;
+    }
+    if (Number.isFinite(def.dampingRatio)) {
+      this.m_dampingRatio = def.dampingRatio;
+    }
+  }
+
+  /**
+   * The local anchor point relative to bodyA's origin.
+   */
+  getLocalAnchorA(): Vec2Value {
+    return this.m_localAnchorA;
+  }
+
+  /**
+   * The local anchor point relative to bodyB's origin.
+   */
+  getLocalAnchorB(): Vec2Value {
+    return this.m_localAnchorB;
+  }
+
+  /**
+   * The local joint axis relative to bodyA.
+   */
+  getLocalAxisA(): Vec2Value {
+    return this.m_localXAxisA;
+  }
+
+  /**
+   * Get the current joint translation, usually in meters.
+   */
+  getJointTranslation(): number {
+    const bA = this.m_bodyA;
+    const bB = this.m_bodyB;
+
+    const pA = bA.getWorldPoint(this.m_localAnchorA);
+    const pB = bB.getWorldPoint(this.m_localAnchorB);
+    const d = geo.vec2(0, 0);
+    geo.subVec2(d, pB, pA);
+    const axis = bA.getWorldVector(this.m_localXAxisA);
+
+    const translation = geo.dotVec2(d, axis);
+    return translation;
+  }
+
+  /**
+   * Get the current joint translation speed, usually in meters per second.
+   */
+  getJointSpeed(): number {
+    const wA = this.m_bodyA.m_angularVelocity;
+    const wB = this.m_bodyB.m_angularVelocity;
+    return wB - wA;
+  }
+
+  /**
+   * Is the joint motor enabled?
+   */
+  isMotorEnabled(): boolean {
+    return this.m_enableMotor;
+  }
+
+  /**
+   * Enable/disable the joint motor.
+   */
+  enableMotor(flag: boolean): void {
+    if (flag == this.m_enableMotor) return;
+    this.m_bodyA.setAwake(true);
+    this.m_bodyB.setAwake(true);
+    this.m_enableMotor = flag;
+  }
+
+  /**
+   * Set the motor speed, usually in radians per second.
+   */
+  setMotorSpeed(speed: number): void {
+    if (speed == this.m_motorSpeed) return;
+    this.m_bodyA.setAwake(true);
+    this.m_bodyB.setAwake(true);
+    this.m_motorSpeed = speed;
+  }
+
+  /**
+   * Get the motor speed, usually in radians per second.
+   */
+  getMotorSpeed(): number {
+    return this.m_motorSpeed;
+  }
+
+  /**
+   * Set/Get the maximum motor force, usually in N-m.
+   */
+  setMaxMotorTorque(torque: number): void {
+    if (torque == this.m_maxMotorTorque) return;
+    this.m_bodyA.setAwake(true);
+    this.m_bodyB.setAwake(true);
+    this.m_maxMotorTorque = torque;
+  }
+
+  getMaxMotorTorque(): number {
+    return this.m_maxMotorTorque;
+  }
+
+  /**
+   * Get the current motor torque given the inverse time step, usually in N-m.
+   */
+  getMotorTorque(inv_dt: number): number {
+    return inv_dt * this.m_motorImpulse;
+  }
+
+  /**
+   * Set/Get the spring frequency in hertz. Setting the frequency to zero disables
+   * the spring.
+   */
+  setSpringFrequencyHz(hz: number): void {
+    this.m_frequencyHz = hz;
+  }
+
+  getSpringFrequencyHz(): number {
+    return this.m_frequencyHz;
+  }
+
+  /**
+   * Set/Get the spring damping ratio
+   */
+  setSpringDampingRatio(ratio: number): void {
+    this.m_dampingRatio = ratio;
+  }
+
+  getSpringDampingRatio(): number {
+    return this.m_dampingRatio;
+  }
+
+  /**
+   * Get the anchor point on bodyA in world coordinates.
+   */
+  getAnchorA(): Vec2Value {
+    return this.m_bodyA.getWorldPoint(this.m_localAnchorA);
+  }
+
+  /**
+   * Get the anchor point on bodyB in world coordinates.
+   */
+  getAnchorB(): Vec2Value {
+    return this.m_bodyB.getWorldPoint(this.m_localAnchorB);
+  }
+
+  /**
+   * Get the reaction force on bodyB at the joint anchor in Newtons.
+   */
+  getReactionForce(inv_dt: number): Vec2Value {
+    const result = geo.vec2(0, 0);
+    geo.combine2Vec2(result, this.m_impulse, this.m_ay, this.m_springImpulse, this.m_ax);
+    geo.mulVec2(result, inv_dt);
+    return result;
+  }
+
+  /**
+   * Get the reaction torque on bodyB in N*m.
+   */
+  getReactionTorque(inv_dt: number): number {
+    return inv_dt * this.m_motorImpulse;
+  }
+
+  initVelocityConstraints(step: TimeStep): void {
+    this.m_localCenterA = this.m_bodyA.m_sweep.localCenter;
+    this.m_localCenterB = 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 mA = this.m_invMassA;
+    const mB = this.m_invMassB;
+    const iA = this.m_invIA;
+    const iB = this.m_invIB;
+
+    const cA = this.m_bodyA.c_position.c;
+    const aA = this.m_bodyA.c_position.a;
+    const vA = this.m_bodyA.c_velocity.v;
+    let wA = this.m_bodyA.c_velocity.w;
+
+    const cB = this.m_bodyB.c_position.c;
+    const aB = this.m_bodyB.c_position.a;
+    const vB = this.m_bodyB.c_velocity.v;
+    let wB = this.m_bodyB.c_velocity.w;
+
+    const qA = geo.rotation(aA);
+    const qB = geo.rotation(aB);
+
+    // Compute the effective masses.
+    const rA = geo.vec2(0, 0);
+    geo.rotSubVec2(rA, qA, this.m_localAnchorA, this.m_localCenterA);
+    const rB = geo.vec2(0, 0);
+    geo.rotSubVec2(rB, qB, this.m_localAnchorB, this.m_localCenterB);
+    const d = geo.vec2(0, 0);
+    geo.combine4Vec2(d, 1, cB, 1, rB, -1, cA, -1, rA);
+
+    // Point to line constraint
+    {
+      this.m_ay = geo.vec2(0, 0);
+      geo.rotVec2(this.m_ay, qA, this.m_localYAxisA);
+      this.m_sAy = geo.crossVec2Vec2(d, this.m_ay) + geo.crossVec2Vec2(rA, this.m_ay);
+      this.m_sBy = geo.crossVec2Vec2(rB, this.m_ay);
+
+      this.m_mass = mA + mB + iA * this.m_sAy * this.m_sAy + iB * this.m_sBy * this.m_sBy;
+
+      if (this.m_mass > 0.0) {
+        this.m_mass = 1.0 / this.m_mass;
+      }
+    }
+
+    // Spring constraint
+    this.m_springMass = 0.0;
+    this.m_bias = 0.0;
+    this.m_gamma = 0.0;
+    if (this.m_frequencyHz > 0.0) {
+      this.m_ax = geo.vec2(0, 0);
+      geo.rotVec2(this.m_ax, qA, this.m_localXAxisA);
+      this.m_sAx = geo.crossVec2Vec2(d, this.m_ax) + geo.crossVec2Vec2(rA, this.m_ax);
+      this.m_sBx = geo.crossVec2Vec2(rB, this.m_ax);
+
+      const invMass = mA + mB + iA * this.m_sAx * this.m_sAx + iB * this.m_sBx * this.m_sBx;
+
+      if (invMass > 0.0) {
+        this.m_springMass = 1.0 / invMass;
+
+        const C = geo.dotVec2(d, this.m_ax);
+
+        // Frequency
+        const omega = 2.0 * math_PI * this.m_frequencyHz;
+
+        // Damping coefficient
+        const damp = 2.0 * this.m_springMass * this.m_dampingRatio * omega;
+
+        // Spring stiffness
+        const k = this.m_springMass * omega * omega;
+
+        // magic formulas
+        const h = step.dt;
+        this.m_gamma = h * (damp + h * k);
+        if (this.m_gamma > 0.0) {
+          this.m_gamma = 1.0 / this.m_gamma;
+        }
+
+        this.m_bias = C * h * k * this.m_gamma;
+
+        this.m_springMass = invMass + this.m_gamma;
+        if (this.m_springMass > 0.0) {
+          this.m_springMass = 1.0 / this.m_springMass;
+        }
+      }
+    } else {
+      this.m_springImpulse = 0.0;
+    }
+
+    // Rotational motor
+    if (this.m_enableMotor) {
+      this.m_motorMass = iA + iB;
+      if (this.m_motorMass > 0.0) {
+        this.m_motorMass = 1.0 / this.m_motorMass;
+      }
+    } else {
+      this.m_motorMass = 0.0;
+      this.m_motorImpulse = 0.0;
+    }
+
+    if (step.warmStarting) {
+      // Account for variable time step.
+      this.m_impulse *= step.dtRatio;
+      this.m_springImpulse *= step.dtRatio;
+      this.m_motorImpulse *= step.dtRatio;
+
+      const P = geo.vec2(0, 0);
+      geo.combine2Vec2(P, this.m_impulse, this.m_ay, this.m_springImpulse, this.m_ax);
+      const LA = this.m_impulse * this.m_sAy + this.m_springImpulse * this.m_sAx + this.m_motorImpulse;
+      const LB = this.m_impulse * this.m_sBy + this.m_springImpulse * this.m_sBx + this.m_motorImpulse;
+
+      geo.minusScaleVec2(vA, this.m_invMassA, P);
+      wA -= this.m_invIA * LA;
+
+      geo.plusScaleVec2(vB, this.m_invMassB, P);
+      wB += this.m_invIB * LB;
+    } else {
+      this.m_impulse = 0.0;
+      this.m_springImpulse = 0.0;
+      this.m_motorImpulse = 0.0;
+    }
+
+    geo.copyVec2(this.m_bodyA.c_velocity.v, vA);
+    this.m_bodyA.c_velocity.w = wA;
+    geo.copyVec2(this.m_bodyB.c_velocity.v, vB);
+    this.m_bodyB.c_velocity.w = wB;
+  }
+
+  solveVelocityConstraints(step: TimeStep): void {
+    const mA = this.m_invMassA;
+    const mB = this.m_invMassB;
+    const iA = this.m_invIA;
+    const iB = this.m_invIB;
+
+    const vA = this.m_bodyA.c_velocity.v;
+    let wA = this.m_bodyA.c_velocity.w;
+    const vB = this.m_bodyB.c_velocity.v;
+    let wB = this.m_bodyB.c_velocity.w;
+
+    // Solve spring constraint
+    {
+      const Cdot = geo.dotVec2(this.m_ax, vB) - geo.dotVec2(this.m_ax, vA) + this.m_sBx * wB - this.m_sAx * wA;
+      const impulse = -this.m_springMass * (Cdot + this.m_bias + this.m_gamma * this.m_springImpulse);
+      this.m_springImpulse += impulse;
+
+      const P = geo.vec2(0, 0);
+      geo.scaleVec2(P, impulse, this.m_ax);
+      const LA = impulse * this.m_sAx;
+      const LB = impulse * this.m_sBx;
+
+      geo.minusScaleVec2(vA, mA, P);
+      wA -= iA * LA;
+
+      geo.plusScaleVec2(vB, mB, P);
+      wB += iB * LB;
+    }
+
+    // Solve rotational motor constraint
+    {
+      const Cdot = wB - wA - this.m_motorSpeed;
+      let impulse = -this.m_motorMass * Cdot;
+
+      const oldImpulse = this.m_motorImpulse;
+      const maxImpulse = step.dt * this.m_maxMotorTorque;
+      this.m_motorImpulse = clamp(this.m_motorImpulse + impulse, -maxImpulse, maxImpulse);
+      impulse = this.m_motorImpulse - oldImpulse;
+
+      wA -= iA * impulse;
+      wB += iB * impulse;
+    }
+
+    // Solve point to line constraint
+    {
+      const Cdot = geo.dotVec2(this.m_ay, vB) - geo.dotVec2(this.m_ay, vA) + this.m_sBy * wB - this.m_sAy * wA;
+      const impulse = -this.m_mass * Cdot;
+      this.m_impulse += impulse;
+
+      const P = geo.vec2(0, 0);
+      geo.scaleVec2(P, impulse, this.m_ay);
+      const LA = impulse * this.m_sAy;
+      const LB = impulse * this.m_sBy;
+
+      geo.minusScaleVec2(vA, mA, P);
+      wA -= iA * LA;
+
+      geo.plusScaleVec2(vB, mB, P);
+      wB += iB * LB;
+    }
+
+    geo.copyVec2(this.m_bodyA.c_velocity.v, vA);
+    this.m_bodyA.c_velocity.w = wA;
+    geo.copyVec2(this.m_bodyB.c_velocity.v, vB);
+    this.m_bodyB.c_velocity.w = wB;
+  }
+
+  /**
+   * This returns true if the position errors are within tolerance.
+   */
+  solvePositionConstraints(step: TimeStep): boolean {
+    const cA = this.m_bodyA.c_position.c;
+    let aA = this.m_bodyA.c_position.a;
+    const cB = this.m_bodyB.c_position.c;
+    let aB = this.m_bodyB.c_position.a;
+
+    const qA = geo.rotation(aA);
+    const qB = geo.rotation(aB);
+
+    const rA = geo.vec2(0, 0);
+    geo.rotSubVec2(rA, qA, this.m_localAnchorA, this.m_localCenterA);
+    const rB = geo.vec2(0, 0);
+    geo.rotSubVec2(rB, qB, this.m_localAnchorB, this.m_localCenterB);
+    const d = geo.vec2(0, 0);
+    geo.combine4Vec2(d, 1, cB, 1, rB, -1, cA, -1, rA);
+
+    const ay = geo.vec2(0, 0);
+    geo.rotVec2(ay, qA, this.m_localYAxisA);
+
+    const sAy = geo.crossVec2Vec2(rA, ay) + geo.crossVec2Vec2(rA, ay);
+    const sBy = geo.crossVec2Vec2(rB, ay);
+
+    const C = geo.dotVec2(d, ay);
+
+    const k =
+      this.m_invMassA +
+      this.m_invMassB +
+      this.m_invIA * this.m_sAy * this.m_sAy +
+      this.m_invIB * this.m_sBy * this.m_sBy;
+
+    const impulse = k != 0.0 ? -C / k : 0.0;
+
+    const P = geo.vec2(0, 0);
+    geo.scaleVec2(P, impulse, ay);
+    const LA = impulse * sAy;
+    const LB = impulse * sBy;
+
+    geo.minusScaleVec2(cA, this.m_invMassA, P);
+    aA -= this.m_invIA * LA;
+    geo.plusScaleVec2(cB, this.m_invMassB, P);
+    aB += this.m_invIB * LB;
+
+    geo.copyVec2(this.m_bodyA.c_position.c, cA);
+    this.m_bodyA.c_position.a = aA;
+    geo.copyVec2(this.m_bodyB.c_position.c, cB);
+    this.m_bodyB.c_position.a = aB;
+
+    return math_abs(C) <= Settings.linearSlop;
+  }
+}