@woosh/meep-engine 2.139.0 → 2.141.0

package/src/engine/physics/constraint/solve_constraints.js

@@ -0,0 +1,436 @@
+import { BodyKind } from "../ecs/BodyKind.js";
+import { SleepState } from "../ecs/SleepState.js";
+import { JOINT_WORLD } from "../ecs/Joint.js";
+import { DofMode } from "./DofMode.js";
+import { world_inverse_inertia_apply } from "../inertia/world_inverse_inertia.js";
+import { v3_quat3_apply } from "../../../core/geom/vec3/v3_quat3_apply.js";
+import { quat3_to_matrix3 } from "../../../core/geom/3d/quaternion/quat3_to_matrix3.js";
+import { quat3_multiply } from "../../../core/geom/3d/quaternion/quat3_multiply.js";
+
+/**
+ * # 6-DOF constraint (joint) solver
+ *
+ * Runs alongside the contact solver inside the same TGS substep loop: each
+ * substep the joints are warm-started and their velocity rows solved, sharing
+ * the contact solver's per-substep / warm-start / SPOOK-bias model so the two
+ * constraint families converge together (a body touched by both a contact and
+ * a joint sees a single coupled Gauss-Seidel sweep across the substep loop).
+ *
+ * A joint is a configurable 6-DOF constraint ({@link Joint}); each relative
+ * degree of freedom is independently locked / free / limited / springy /
+ * motorised. This module implements **LOCKED linear** DOFs first — i.e. the
+ * ball-socket (point-to-point) joint, which alone covers chains, ropes, and
+ * pendulums. Angular locks, limits, springs and motors slot into the same
+ * per-DOF loop as they land.
+ *
+ * Row math is identical in shape to a contact's normal row: for a unit axis
+ * `d` and lever arms `rA = anchorA − comA`, `rB = anchorB − comB`,
+ *   `K = invMA + invMB + (rA×d)·Iw_A·(rA×d) + (rB×d)·Iw_B·(rB×d)`,
+ *   `vrel = (vA + ωA×rA − vB − ωB×rB) · d`,
+ *   `λ = −(1/K)·(vrel + bias)`,
+ * applied as `±λ·d` at the anchors. Locked DOFs are bilateral (no clamp); the
+ * position error is corrected by a SPOOK velocity bias. Geometry (anchors /
+ * lever arms / position error) is recomputed from the current pose every
+ * substep, so the joint tracks the bodies as they move.
+ *
+ * @author Alex Goldring
+ * @copyright Company Named Limited (c) 2026
+ */
+
+/**
+ * Maximum SPOOK position-correction bias for a joint row, m/s — belt-and-braces
+ * against a wildly-violated constraint (e.g. a freshly-teleported body)
+ * yanking the solve. Equality joints sit near zero error at steady state, so
+ * this only clamps transients.
+ * @type {number}
+ */
+const MAX_JOINT_BIAS = 4;
+
+/**
+ * Baumgarte / SPOOK position-correction gain `β`. The per-substep gain is
+ * `β / dt_sub`; with `β = 0.2` this matches the contact solver's position
+ * stiffness. Joints are bilateral equality constraints, so this only nudges
+ * residual position error toward zero (no clamp interplay to destabilise).
+ * @type {number}
+ */
+const JOINT_BAUMGARTE = 0.2;
+
+const scratch_inertia = new Float64Array(3);
+const scratch_anchor = new Float64Array(3);
+/** Frame-A world basis (rows = A's frame axes in world), from quat3_to_matrix3. */
+const scratch_frame_a = new Float64Array(9);
+/** Scratch quaternion for frame composition / relative rotation. */
+const scratch_q = new Float64Array(4);
+
+/**
+ * Angular effective-mass contribution of one body about a unit world axis:
+ * `a · Iw⁻¹ · a`. Zero for non-dynamic / rotation-locked bodies.
+ * @returns {number}
+ */
+function angular_axis_effective_mass(rb, transform, ax, ay, az) {
+    if (rb.kind !== BodyKind.Dynamic) return 0;
+    const ii = rb.inverseInertiaLocal;
+    if (ii.x === 0 && ii.y === 0 && ii.z === 0) return 0;
+    world_inverse_inertia_apply(scratch_inertia, 0, ii, transform.rotation, ax, ay, az);
+    return ax * scratch_inertia[0] + ay * scratch_inertia[1] + az * scratch_inertia[2];
+}
+
+/**
+ * Apply a pure angular impulse `sign·λ·axis`: Δω = Iw⁻¹·(sign·λ·axis).
+ * @param {RigidBody} rb @param {Transform} transform
+ * @param {number} ax @param {number} ay @param {number} az
+ * @param {number} lambda @param {number} sign
+ */
+function apply_angular_impulse(rb, transform, ax, ay, az, lambda, sign) {
+    if (rb.kind !== BodyKind.Dynamic) return;
+    const ii = rb.inverseInertiaLocal;
+    if (ii.x === 0 && ii.y === 0 && ii.z === 0) return;
+    const L = sign * lambda;
+    world_inverse_inertia_apply(scratch_inertia, 0, ii, transform.rotation, ax * L, ay * L, az * L);
+    const av = rb.angularVelocity;
+    av[0] += scratch_inertia[0];
+    av[1] += scratch_inertia[1];
+    av[2] += scratch_inertia[2];
+}
+
+/**
+ * Effective-mass denominator contribution of one body along a unit axis:
+ * `invM + (r×d)·Iw⁻¹·(r×d)`. Static / kinematic bodies (invM === 0) contribute
+ * nothing.
+ *
+ * @param {RigidBody} rb
+ * @param {Transform} transform
+ * @param {number} invM
+ * @param {number} rx @param {number} ry @param {number} rz
+ * @param {number} dx @param {number} dy @param {number} dz
+ * @returns {number}
+ */
+function axis_effective_mass(rb, transform, invM, rx, ry, rz, dx, dy, dz) {
+    if (invM === 0) return 0;
+    let k = invM;
+    const ii = rb.inverseInertiaLocal;
+    if (ii.x !== 0 || ii.y !== 0 || ii.z !== 0) {
+        const cx = ry * dz - rz * dy;
+        const cy = rz * dx - rx * dz;
+        const cz = rx * dy - ry * dx;
+        world_inverse_inertia_apply(scratch_inertia, 0, ii, transform.rotation, cx, cy, cz);
+        k += cx * scratch_inertia[0] + cy * scratch_inertia[1] + cz * scratch_inertia[2];
+    }
+    return k;
+}
+
+/**
+ * Apply impulse `P` at body-relative offset `r`: Δv = P·invM, Δω = Iw⁻¹·(r×P).
+ * Direct typed-array writes (bypass Vector3#set observers — dead weight here).
+ *
+ * @param {RigidBody} rb @param {Transform} transform @param {number} invM
+ * @param {number} rx @param {number} ry @param {number} rz
+ * @param {number} Px @param {number} Py @param {number} Pz
+ * @param {number} sign +1 / −1
+ */
+function apply_impulse(rb, transform, invM, rx, ry, rz, Px, Py, Pz, sign) {
+    if (invM === 0) return;
+    const sPx = sign * Px, sPy = sign * Py, sPz = sign * Pz;
+    const lv = rb.linearVelocity;
+    lv[0] += sPx * invM; lv[1] += sPy * invM; lv[2] += sPz * invM;
+
+    const tx = ry * sPz - rz * sPy;
+    const ty = rz * sPx - rx * sPz;
+    const tz = rx * sPy - ry * sPx;
+    world_inverse_inertia_apply(scratch_inertia, 0, rb.inverseInertiaLocal, transform.rotation, tx, ty, tz);
+    const av = rb.angularVelocity;
+    av[0] += scratch_inertia[0]; av[1] += scratch_inertia[1]; av[2] += scratch_inertia[2];
+}
+
+/**
+ * @param {RigidBody} rb
+ * @returns {number}
+ */
+function inv_mass_of(rb) {
+    if (rb.kind !== BodyKind.Dynamic) return 0;
+    return rb.mass > 0 ? 1 / rb.mass : 0;
+}
+
+/**
+ * Solve every joint for one substep: recompute geometry at the current poses,
+ * replay the per-substep warm-start, and run `iters` velocity iterations of
+ * the locked linear DOFs.
+ *
+ * Called once per substep from `PhysicsSystem.fixedUpdate`, after the contact
+ * solve so the two share the substep / warm-start cadence.
+ *
+ * @param {Joint[]} joints  live joints (sparse array; holes skipped)
+ * @param {PhysicsSystem} system  reads `__bodies` / `__transforms` / index map
+ * @param {number} dt_sub  substep size in seconds (the SPOOK gain is derived
+ *   from it, matching the contact solver's per-substep position stiffness)
+ * @param {number} iters  velocity iterations
+ */
+export function solve_joints(joints, system, dt_sub, iters) {
+    const n = joints.length;
+    if (n === 0 || dt_sub <= 0) return;
+
+    const spook_a = JOINT_BAUMGARTE / dt_sub;
+
+    const storage = system.storage;
+
+    for (let ji = 0; ji < n; ji++) {
+        const joint = joints[ji];
+        if (joint === undefined || joint === null) continue;
+
+        // Generation-checked validity: if A's body was unlinked (and its slot
+        // possibly reused by a different body), the stored packed id no longer
+        // validates — skip the stale joint rather than attach to the wrong body.
+        if (!storage.is_valid(joint._bodyIdA)) continue;
+        const idxA = system.__index_of(joint._bodyIdA);
+        const rbA = system.__bodies[idxA];
+        if (rbA === undefined) continue;
+        const trA = system.__transforms[idxA];
+
+        const to_world = joint._bodyIdB === JOINT_WORLD;
+        let rbB = null, trB = null, idxB = -1;
+        if (!to_world) {
+            if (!storage.is_valid(joint._bodyIdB)) continue;
+            idxB = system.__index_of(joint._bodyIdB);
+            rbB = system.__bodies[idxB];
+            if (rbB === undefined) continue;
+            trB = system.__transforms[idxB];
+        }
+
+        // Skip while a participating dynamic body sleeps — its velocity must
+        // stay zero until woken. (Island-aware joint sleep is a follow-up; for
+        // now jointed bodies that should stay coupled use DisableSleep.)
+        if (rbA.sleepState === SleepState.Sleeping) continue;
+        if (!to_world && rbB.sleepState === SleepState.Sleeping) continue;
+
+        // Both bodies static/kinematic → nothing to solve.
+        const invMA = inv_mass_of(rbA);
+        const invMB = to_world ? 0 : inv_mass_of(rbB);
+        if (invMA === 0 && invMB === 0) continue;
+
+        // World anchor A and its lever arm rA = R_A · localAnchorA.
+        const la = joint.localAnchorA;
+        v3_quat3_apply(scratch_anchor, 0, la.x, la.y, la.z,
+            trA.rotation[0], trA.rotation[1], trA.rotation[2], trA.rotation[3]);
+        const rAx = scratch_anchor[0], rAy = scratch_anchor[1], rAz = scratch_anchor[2];
+        const anchorAx = trA.position.x + rAx;
+        const anchorAy = trA.position.y + rAy;
+        const anchorAz = trA.position.z + rAz;
+
+        // World anchor B + lever arm rB. For a world anchor, localAnchorB is a
+        // fixed world point and there is no body B lever.
+        let rBx = 0, rBy = 0, rBz = 0;
+        let anchorBx, anchorBy, anchorBz;
+        const lb = joint.localAnchorB;
+        if (to_world) {
+            anchorBx = lb.x; anchorBy = lb.y; anchorBz = lb.z;
+        } else {
+            v3_quat3_apply(scratch_anchor, 0, lb.x, lb.y, lb.z,
+                trB.rotation[0], trB.rotation[1], trB.rotation[2], trB.rotation[3]);
+            rBx = scratch_anchor[0]; rBy = scratch_anchor[1]; rBz = scratch_anchor[2];
+            anchorBx = trB.position.x + rBx;
+            anchorBy = trB.position.y + rBy;
+            anchorBz = trB.position.z + rBz;
+        }
+
+        // Position error C = anchorA − anchorB (drive to zero on locked axes).
+        const Cx = anchorAx - anchorBx;
+        const Cy = anchorAy - anchorBy;
+        const Cz = anchorAz - anchorBz;
+
+        const mode = joint.dofMode;
+        const imp = joint.dofImpulse;
+        const lvA = rbA.linearVelocity, avA = rbA.angularVelocity;
+        const lvB = to_world ? null : rbB.linearVelocity;
+        const avB = to_world ? null : rbB.angularVelocity;
+
+        // === Frame A world axes — shared by the linear AND angular rows ===
+        // Every DOF is expressed in the joint's frame on body A. A locked
+        // linear DOF pins the anchor offset along a frame axis (all three →
+        // ball-socket); a FREE linear DOF is a prismatic slide. A locked
+        // angular DOF pins relative rotation about a frame axis; a FREE one is
+        // a hinge axis. Frame A = body-A rotation ⊗ localBasisA; the rows of
+        // its matrix are the frame axes in world (see quat3_to_matrix3).
+        const linLockX = mode[0] === DofMode.LOCKED;
+        const linLockY = mode[1] === DofMode.LOCKED;
+        const linLockZ = mode[2] === DofMode.LOCKED;
+        const angLockX = mode[3] === DofMode.LOCKED;
+        const angLockY = mode[4] === DofMode.LOCKED;
+        const angLockZ = mode[5] === DofMode.LOCKED;
+        if (!(linLockX || linLockY || linLockZ || angLockX || angLockY || angLockZ)) continue;
+
+        const lba = joint.localBasisA;
+        quat3_multiply(scratch_q, 0, trA.rotation[0], trA.rotation[1], trA.rotation[2], trA.rotation[3], lba[0], lba[1], lba[2], lba[3]);
+        const qAx = scratch_q[0], qAy = scratch_q[1], qAz = scratch_q[2], qAw = scratch_q[3];
+        quat3_to_matrix3(scratch_frame_a, 0, qAx, qAy, qAz, qAw);
+        const aXx = scratch_frame_a[0], aXy = scratch_frame_a[1], aXz = scratch_frame_a[2];
+        const aYx = scratch_frame_a[3], aYy = scratch_frame_a[4], aYz = scratch_frame_a[5];
+        const aZx = scratch_frame_a[6], aZy = scratch_frame_a[7], aZz = scratch_frame_a[8];
+
+        // --- Linear rows: effective mass + SPOOK bias along each locked frame
+        //     axis. Error along an axis is `C · axis` (signed anchor offset).
+        //     Convention is A−B (relative anchor velocity vA−vB, impulse +to A
+        //     / −to B), matching contacts. ---
+        let m_eff_lx = 0, m_eff_ly = 0, m_eff_lz = 0;
+        let biasLx = 0, biasLy = 0, biasLz = 0;
+        if (linLockX) {
+            const k = axis_effective_mass(rbA, trA, invMA, rAx, rAy, rAz, aXx, aXy, aXz)
+                    + (to_world ? 0 : axis_effective_mass(rbB, trB, invMB, rBx, rBy, rBz, aXx, aXy, aXz));
+            m_eff_lx = k > 0 ? 1 / k : 0;
+            let b = spook_a * (Cx * aXx + Cy * aXy + Cz * aXz);
+            if (b > MAX_JOINT_BIAS) b = MAX_JOINT_BIAS; else if (b < -MAX_JOINT_BIAS) b = -MAX_JOINT_BIAS;
+            biasLx = b;
+        }
+        if (linLockY) {
+            const k = axis_effective_mass(rbA, trA, invMA, rAx, rAy, rAz, aYx, aYy, aYz)
+                    + (to_world ? 0 : axis_effective_mass(rbB, trB, invMB, rBx, rBy, rBz, aYx, aYy, aYz));
+            m_eff_ly = k > 0 ? 1 / k : 0;
+            let b = spook_a * (Cx * aYx + Cy * aYy + Cz * aYz);
+            if (b > MAX_JOINT_BIAS) b = MAX_JOINT_BIAS; else if (b < -MAX_JOINT_BIAS) b = -MAX_JOINT_BIAS;
+            biasLy = b;
+        }
+        if (linLockZ) {
+            const k = axis_effective_mass(rbA, trA, invMA, rAx, rAy, rAz, aZx, aZy, aZz)
+                    + (to_world ? 0 : axis_effective_mass(rbB, trB, invMB, rBx, rBy, rBz, aZx, aZy, aZz));
+            m_eff_lz = k > 0 ? 1 / k : 0;
+            let b = spook_a * (Cx * aZx + Cy * aZy + Cz * aZz);
+            if (b > MAX_JOINT_BIAS) b = MAX_JOINT_BIAS; else if (b < -MAX_JOINT_BIAS) b = -MAX_JOINT_BIAS;
+            biasLz = b;
+        }
+        const doLinX = linLockX && m_eff_lx !== 0;
+        const doLinY = linLockY && m_eff_ly !== 0;
+        const doLinZ = linLockZ && m_eff_lz !== 0;
+
+        // --- Angular rows: relative-rotation error + effective mass about each
+        //     locked frame axis. Convention is B−A (relative angular velocity
+        //     ωB−ωA, impulse +to B / −to A). ---
+        let m_eff_ax = 0, m_eff_ay = 0, m_eff_az = 0;
+        let biasAx = 0, biasAy = 0, biasAz = 0;
+        let lockAX = false, lockAY = false, lockAZ = false;
+        if (angLockX || angLockY || angLockZ) {
+            // Frame B world rotation (identity for a world anchor → locks A's
+            // frame to world axes).
+            let qBx = 0, qBy = 0, qBz = 0, qBw = 1;
+            if (!to_world) {
+                const lbb = joint.localBasisB;
+                quat3_multiply(scratch_q, 0, trB.rotation[0], trB.rotation[1], trB.rotation[2], trB.rotation[3], lbb[0], lbb[1], lbb[2], lbb[3]);
+                qBx = scratch_q[0]; qBy = scratch_q[1]; qBz = scratch_q[2]; qBw = scratch_q[3];
+            }
+            // qD = conj(qA) ⊗ qB; small-angle rotation vector (shortest path via
+            // sign(w)) is the per-axis error in frame-A-local coords — pairs
+            // with the world frame axes above (axis i ↔ error component i).
+            quat3_multiply(scratch_q, 0, -qAx, -qAy, -qAz, qAw, qBx, qBy, qBz, qBw);
+            const es = scratch_q[3] < 0 ? -2 : 2;
+            const eX = es * scratch_q[0];
+            const eY = es * scratch_q[1];
+            const eZ = es * scratch_q[2];
+
+            if (angLockX) {
+                const k = angular_axis_effective_mass(rbA, trA, aXx, aXy, aXz)
+                        + (to_world ? 0 : angular_axis_effective_mass(rbB, trB, aXx, aXy, aXz));
+                m_eff_ax = k > 0 ? 1 / k : 0;
+                let b = spook_a * eX;
+                if (b > MAX_JOINT_BIAS) b = MAX_JOINT_BIAS; else if (b < -MAX_JOINT_BIAS) b = -MAX_JOINT_BIAS;
+                biasAx = b;
+                lockAX = m_eff_ax !== 0;
+            }
+            if (angLockY) {
+                const k = angular_axis_effective_mass(rbA, trA, aYx, aYy, aYz)
+                        + (to_world ? 0 : angular_axis_effective_mass(rbB, trB, aYx, aYy, aYz));
+                m_eff_ay = k > 0 ? 1 / k : 0;
+                let b = spook_a * eY;
+                if (b > MAX_JOINT_BIAS) b = MAX_JOINT_BIAS; else if (b < -MAX_JOINT_BIAS) b = -MAX_JOINT_BIAS;
+                biasAy = b;
+                lockAY = m_eff_ay !== 0;
+            }
+            if (angLockZ) {
+                const k = angular_axis_effective_mass(rbA, trA, aZx, aZy, aZz)
+                        + (to_world ? 0 : angular_axis_effective_mass(rbB, trB, aZx, aZy, aZz));
+                m_eff_az = k > 0 ? 1 / k : 0;
+                let b = spook_a * eZ;
+                if (b > MAX_JOINT_BIAS) b = MAX_JOINT_BIAS; else if (b < -MAX_JOINT_BIAS) b = -MAX_JOINT_BIAS;
+                biasAz = b;
+                lockAZ = m_eff_az !== 0;
+            }
+        }
+
+        // --- Per-substep warm-start (frame axes): linear impulse `Σ imp[i]·aᵢ`
+        //     applied at the anchors, angular impulse `Σ imp[3+i]·aᵢ` about the
+        //     frame axes (+to B / −to A). ---
+        {
+            const Px = (doLinX ? imp[0] : 0) * aXx + (doLinY ? imp[1] : 0) * aYx + (doLinZ ? imp[2] : 0) * aZx;
+            const Py = (doLinX ? imp[0] : 0) * aXy + (doLinY ? imp[1] : 0) * aYy + (doLinZ ? imp[2] : 0) * aZy;
+            const Pz = (doLinX ? imp[0] : 0) * aXz + (doLinY ? imp[1] : 0) * aYz + (doLinZ ? imp[2] : 0) * aZz;
+            if (Px !== 0 || Py !== 0 || Pz !== 0) {
+                apply_impulse(rbA, trA, invMA, rAx, rAy, rAz, Px, Py, Pz, +1);
+                if (!to_world) apply_impulse(rbB, trB, invMB, rBx, rBy, rBz, Px, Py, Pz, -1);
+            }
+            const Lx = (lockAX ? imp[3] : 0) * aXx + (lockAY ? imp[4] : 0) * aYx + (lockAZ ? imp[5] : 0) * aZx;
+            const Ly = (lockAX ? imp[3] : 0) * aXy + (lockAY ? imp[4] : 0) * aYy + (lockAZ ? imp[5] : 0) * aZy;
+            const Lz = (lockAX ? imp[3] : 0) * aXz + (lockAY ? imp[4] : 0) * aYz + (lockAZ ? imp[5] : 0) * aZz;
+            if (Lx !== 0 || Ly !== 0 || Lz !== 0) {
+                if (!to_world) apply_angular_impulse(rbB, trB, Lx, Ly, Lz, 1, +1);
+                apply_angular_impulse(rbA, trA, Lx, Ly, Lz, 1, -1);
+            }
+        }
+
+        for (let it = 0; it < iters; it++) {
+            // Linear rows (frame axes). The relative anchor velocity is
+            // recomputed before each row so successive rows see the prior
+            // impulses (Gauss-Seidel coupling).
+            if (doLinX) {
+                const rvx = (lvA[0] + avA[1] * rAz - avA[2] * rAy) - (to_world ? 0 : (lvB[0] + avB[1] * rBz - avB[2] * rBy));
+                const rvy = (lvA[1] + avA[2] * rAx - avA[0] * rAz) - (to_world ? 0 : (lvB[1] + avB[2] * rBx - avB[0] * rBz));
+                const rvz = (lvA[2] + avA[0] * rAy - avA[1] * rAx) - (to_world ? 0 : (lvB[2] + avB[0] * rBy - avB[1] * rBx));
+                const lambda = -m_eff_lx * ((rvx * aXx + rvy * aXy + rvz * aXz) + biasLx);
+                imp[0] += lambda;
+                apply_impulse(rbA, trA, invMA, rAx, rAy, rAz, lambda * aXx, lambda * aXy, lambda * aXz, +1);
+                if (!to_world) apply_impulse(rbB, trB, invMB, rBx, rBy, rBz, lambda * aXx, lambda * aXy, lambda * aXz, -1);
+            }
+            if (doLinY) {
+                const rvx = (lvA[0] + avA[1] * rAz - avA[2] * rAy) - (to_world ? 0 : (lvB[0] + avB[1] * rBz - avB[2] * rBy));
+                const rvy = (lvA[1] + avA[2] * rAx - avA[0] * rAz) - (to_world ? 0 : (lvB[1] + avB[2] * rBx - avB[0] * rBz));
+                const rvz = (lvA[2] + avA[0] * rAy - avA[1] * rAx) - (to_world ? 0 : (lvB[2] + avB[0] * rBy - avB[1] * rBx));
+                const lambda = -m_eff_ly * ((rvx * aYx + rvy * aYy + rvz * aYz) + biasLy);
+                imp[1] += lambda;
+                apply_impulse(rbA, trA, invMA, rAx, rAy, rAz, lambda * aYx, lambda * aYy, lambda * aYz, +1);
+                if (!to_world) apply_impulse(rbB, trB, invMB, rBx, rBy, rBz, lambda * aYx, lambda * aYy, lambda * aYz, -1);
+            }
+            if (doLinZ) {
+                const rvx = (lvA[0] + avA[1] * rAz - avA[2] * rAy) - (to_world ? 0 : (lvB[0] + avB[1] * rBz - avB[2] * rBy));
+                const rvy = (lvA[1] + avA[2] * rAx - avA[0] * rAz) - (to_world ? 0 : (lvB[1] + avB[2] * rBx - avB[0] * rBz));
+                const rvz = (lvA[2] + avA[0] * rAy - avA[1] * rAx) - (to_world ? 0 : (lvB[2] + avB[0] * rBy - avB[1] * rBx));
+                const lambda = -m_eff_lz * ((rvx * aZx + rvy * aZy + rvz * aZz) + biasLz);
+                imp[2] += lambda;
+                apply_impulse(rbA, trA, invMA, rAx, rAy, rAz, lambda * aZx, lambda * aZy, lambda * aZz, +1);
+                if (!to_world) apply_impulse(rbB, trB, invMB, rBx, rBy, rBz, lambda * aZx, lambda * aZy, lambda * aZz, -1);
+            }
+
+            // Angular rows (frame axes): drive ωB−ωA about each locked axis to
+            // zero with the SPOOK bias.
+            if (lockAX) {
+                let wrel = -(avA[0] * aXx + avA[1] * aXy + avA[2] * aXz);
+                if (!to_world) wrel += avB[0] * aXx + avB[1] * aXy + avB[2] * aXz;
+                const lambda = -m_eff_ax * (wrel + biasAx);
+                imp[3] += lambda;
+                if (!to_world) apply_angular_impulse(rbB, trB, aXx, aXy, aXz, lambda, +1);
+                apply_angular_impulse(rbA, trA, aXx, aXy, aXz, lambda, -1);
+            }
+            if (lockAY) {
+                let wrel = -(avA[0] * aYx + avA[1] * aYy + avA[2] * aYz);
+                if (!to_world) wrel += avB[0] * aYx + avB[1] * aYy + avB[2] * aYz;
+                const lambda = -m_eff_ay * (wrel + biasAy);
+                imp[4] += lambda;
+                if (!to_world) apply_angular_impulse(rbB, trB, aYx, aYy, aYz, lambda, +1);
+                apply_angular_impulse(rbA, trA, aYx, aYy, aYz, lambda, -1);
+            }
+            if (lockAZ) {
+                let wrel = -(avA[0] * aZx + avA[1] * aZy + avA[2] * aZz);
+                if (!to_world) wrel += avB[0] * aZx + avB[1] * aZy + avB[2] * aZz;
+                const lambda = -m_eff_az * (wrel + biasAz);
+                imp[5] += lambda;
+                if (!to_world) apply_angular_impulse(rbB, trB, aZx, aZy, aZz, lambda, +1);
+                apply_angular_impulse(rbA, trA, aZx, aZy, aZz, lambda, -1);
+            }
+        }
+    }
+}

package/src/engine/physics/contact/ManifoldStore.d.ts

@@ -9,21 +9,30 @@ export const MAX_CONTACTS_PER_MANIFOLD: number;
  * Per-contact field stride in the data Float64Array.
  *
  * Layout per contact (all world-space):
- *   0..2  : world_a   (contact point on body A's surface)
- *   3..5  : world_b   (contact point on body B's surface)
- *   6..8  : normal_w  (from B toward A)
- *   9     : depth     (positive = penetration, negative = speculative gap)
- *   10    : j_n       (accumulated normal impulse, warm-start)
- *   11    : j_t1      (accumulated tangent impulse, axis 1)
- *   12    : j_t2      (accumulated tangent impulse, axis 2)
+ *   0..2  : world_a    (contact point on body A's surface)
+ *   3..5  : world_b    (contact point on body B's surface)
+ *   6..8  : normal_w   (from B toward A)
+ *   9     : depth      (positive = penetration, negative = speculative gap)
+ *   10    : j_n        (accumulated normal impulse, warm-start)
+ *   11    : j_t1       (accumulated tangent impulse, axis 1)
+ *   12    : j_t2       (accumulated tangent impulse, axis 2)
+ *   13    : feature_id (uint32 packed as f64 — stable cross-frame ID of the
+ *                       geometric feature pair that produced this contact;
+ *                       0 means "no info, fall back to position matching")
  *
  * Solver uses `(world_a + world_b) * 0.5` as the application point; storing
- * both surface points keeps the door open for richer warm-start matching
- * across frames in a follow-up slice.
+ * both surface points enables the per-frame warm-start matcher in the
+ * narrowphase to compare contact-point positions when feature_id is 0.
  *
  * @type {number}
  */
 export const CONTACT_STRIDE: number;
+/**
+ * Per-contact feature_id offset within {@link CONTACT_STRIDE}.
+ * Exposed for the narrowphase matcher; solver code does not read this.
+ * @type {number}
+ */
+export const CONTACT_FEATURE_ID_OFFSET: number;
 /**
  * Per-slot Float64 stride: room for {@link MAX_CONTACTS_PER_MANIFOLD} contacts.
  * @type {number}
@@ -65,6 +74,7 @@ export class ManifoldStore {
     __high_water: number;
     __data: Float64Array;
     __meta: Uint32Array;
+    __slot_axis: Float64Array;
     __pair_index: PairUint32Map;
     __live_slots: Uint32Array;
     __live_pos: Int32Array;
@@ -137,10 +147,31 @@ export class ManifoldStore {
      * @param {number} slot
      */
     clear_contacts(slot: number): void;
+    /**
+     * Reset the contact count to zero so the next `set_contact(slot, 0, …)`
+     * lands at index 0, but DO NOT zero the data slab. Warm-start impulses
+     * (`j_n`, `j_t1`, `j_t2`) at each contact slot survive — the subsequent
+     * `set_contact` calls only overwrite the first 10 floats (position,
+     * normal, depth) per contact.
+     *
+     * Use this when narrowphase determines the pair still has overlap and
+     * is about to refill the slot with the new frame's contacts. Use
+     * {@link clear_contacts} instead when the pair has genuinely separated
+     * and the cached impulses should be evicted.
+     *
+     * @param {number} slot
+     */
+    begin_refill(slot: number): void;
     /**
      * Write a contact point into a slot. Increments contact_count if `idx`
      * exceeds the current count.
      *
+     * Writes geometry (offsets 0..9) and the feature_id at offset 13.
+     * Deliberately does NOT touch the warm-start impulse fields (offsets
+     * 10..12) — the narrowphase match-and-merge pass uses this to refill
+     * a slot while preserving the cached impulses that the solver's
+     * warm-start needs.
+     *
      * @param {number} slot
      * @param {number} idx contact index in `[0, MAX_CONTACTS_PER_MANIFOLD)`
      * @param {number} lax
@@ -153,8 +184,35 @@ export class ManifoldStore {
      * @param {number} ny
      * @param {number} nz
      * @param {number} depth
+     * @param {number} feature_id stable cross-frame identifier of the
+     *   geometric feature pair that produced this contact; 0 means
+     *   "no info, fall back to position matching"
      */
-    set_contact(slot: number, idx: number, lax: number, lay: number, laz: number, lbx: number, lby: number, lbz: number, nx: number, ny: number, nz: number, depth: number): void;
+    set_contact(slot: number, idx: number, lax: number, lay: number, laz: number, lbx: number, lby: number, lbz: number, nx: number, ny: number, nz: number, depth: number, feature_id?: number): void;
+    /**
+     * Zero the warm-start impulse fields (j_n, j_t1, j_t2) for one contact
+     * index inside a slot. Called by the narrowphase match-and-merge pass
+     * after writing a candidate that did NOT match any prior-frame contact:
+     * the geometry at this index is "new" and inheriting impulses from
+     * whatever happened to be cached at this index last frame would
+     * destabilise the solver.
+     *
+     * Distinct from {@link clear_contacts} (which wipes the entire slot's
+     * data slab including geometry) — `clear_impulses` only touches the
+     * three impulse fields at one contact index, leaving the geometric
+     * fields and feature_id intact.
+     *
+     * @param {number} slot
+     * @param {number} idx
+     */
+    clear_impulses(slot: number, idx: number): void;
+    /**
+     * Read the feature_id stored at one contact within a slot.
+     * @param {number} slot
+     * @param {number} idx
+     * @returns {number}
+     */
+    feature_id_of(slot: number, idx: number): number;
     /**
      * Read a contact point. Writes 10 floats: lax, lay, laz, lbx, lby, lbz, nx, ny, nz, depth.
      * @param {number} slot
@@ -184,6 +242,21 @@ export class ManifoldStore {
      * @returns {Float64Array}
      */
     get data_buffer(): Float64Array;
+    /**
+     * Per-slot cached GJK separating axis buffer. Hot-path GJK callers
+     * pass this together with {@link slot_axis_offset} to
+     * `gjk_with_axis(...)` so the cached axis seeds the next iteration.
+     * Buffer identity is stable until grow.
+     * @returns {Float64Array}
+     */
+    get slot_axis_buffer(): Float64Array;
+    /**
+     * Float offset into {@link slot_axis_buffer} where this slot's
+     * cached axis lives (3 floats: x, y, z).
+     * @param {number} slot
+     * @returns {number}
+     */
+    slot_axis_offset(slot: number): number;
     /**
      * Word offset into {@link data_buffer} where the first contact of `slot`
      * begins. Subsequent contacts within the slot are at

package/src/engine/physics/contact/ManifoldStore.d.ts.map

@@ -1 +1 @@
-{"version":3,"file":"ManifoldStore.d.ts","sourceRoot":"","sources":["../../../../../src/engine/physics/contact/ManifoldStore.js"],"names":[],"mappings":"AAGA;;;;;GAKG;AACH,wCAFU,MAAM,CAE2B;AAE3C;;;;;;;;;;;;;;;;;GAiBG;AACH,6BAFU,MAAM,CAEiB;AAEjC;;;GAGG;AACH,+BAFU,MAAM,CAE2D;AAyB3E;;;GAGG;AACH,mCAFU,MAAM,CAEuB;AAIvC;;;;;;;;;;;;;;;;;;;;;GAqBG;AACH;IAEI;;OAEG;IACH,+BAFW,MAAM,EAqBhB;IAjBG,mBAA+C;IAC/C,qBAAqB;IAErB,qBAAkE;IAClE,oBAAiE;IAEjE,4BAA0D;IAG1D,0BAAoD;IACpD,uBAAiD;IAEjD,qBAAqB;IAGrB,yBAAmD;IACnD,qBAAqB;IAGzB;;OAEG;IACH,oBAEC;IAED;;OAEG;IACH,uBAEC;IAED;;;;;;;OAOG;IACH,UAJW,MAAM,OACN,MAAM,GACJ,MAAM,CAKlB;IAED;;;;;;;;OAQG;IACH,aAJW,MAAM,OACN,MAAM,GACJ,MAAM,CAwBlB;IAED;;;OAGG;IACH,YAHW,MAAM,GACJ,MAAM,CAIlB;IAED;;;OAGG;IACH,YAHW,MAAM,GACJ,MAAM,CAIlB;IAED;;;OAGG;IACH,oBAHW,MAAM,GACJ,MAAM,CAIlB;IAED;;;OAGG;IACH,iBAHW,MAAM,GACJ,OAAO,CAInB;IAED;;;OAGG;IACH,uBAHW,MAAM,GACJ,OAAO,CAInB;IAED;;;;;OAKG;IACH,WAHW,MAAM,GACJ,MAAM,CAIlB;IAED;;;;;OAKG;IACH,qBAFW,MAAM,QAQhB;IAED;;;;;;;;;;;;;;;;OAgBG;IACH,kBAbW,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,SACN,MAAM,QAqBhB;IAED;;;;;;OAMG;IACH,kBALW,MAAM,OACN,MAAM,OACN,MAAM,EAAE,GAAC,YAAY,cACrB,MAAM,QAOhB;IAED;;;;;;OAMG;IACH,uBAJW,MAAM,OACN,MAAM,GACJ,MAAM,CAIlB;IAED,iEAAiE;IACjE,uBADY,MAAM,OAAe,MAAM,GAAgB,MAAM,CAG5D;IAED,iEAAiE;IACjE,uBADY,MAAM,OAAe,MAAM,GAAgB,MAAM,CAG5D;IAED,iEAAiE;IACjE,oBADY,MAAM,OAAe,MAAM,GAAgB,MAAM,CAG5D;IAED;;;;;OAKG;IACH,gCAEC;IAED;;;;;;OAMG;IACH,uBAHW,MAAM,GACJ,MAAM,CAIlB;IAED;;;OAGG;IACH,sBA6BC;IAED;;;OAGG;IACH,wBAcC;IAED;;;OAGG;IACH,uBAkBC;IAED;;OAEG;IACH,eA8BC;IAED;;;OAGG;IACH,oBAaC;IAED;;;OAGG;IACH,mBAqBC;CACJ;8BAjfqD,2CAA2C"}
+{"version":3,"file":"ManifoldStore.d.ts","sourceRoot":"","sources":["../../../../../src/engine/physics/contact/ManifoldStore.js"],"names":[],"mappings":"AAGA;;;;;GAKG;AACH,wCAFU,MAAM,CAE2B;AAE3C;;;;;;;;;;;;;;;;;;;;GAoBG;AACH,6BAFU,MAAM,CAEiB;AAEjC;;;;GAIG;AACH,wCAFU,MAAM,CAE4B;AAE5C;;;GAGG;AACH,+BAFU,MAAM,CAE2D;AAyB3E;;;GAGG;AACH,mCAFU,MAAM,CAEuB;AAIvC;;;;;;;;;;;;;;;;;;;;;GAqBG;AACH;IAEI;;OAEG;IACH,+BAFW,MAAM,EA6BhB;IAzBG,mBAA+C;IAC/C,qBAAqB;IAErB,qBAAkE;IAClE,oBAAiE;IAQjE,0BAAwD;IAExD,4BAA0D;IAG1D,0BAAoD;IACpD,uBAAiD;IAEjD,qBAAqB;IAGrB,yBAAmD;IACnD,qBAAqB;IAGzB;;OAEG;IACH,oBAEC;IAED;;OAEG;IACH,uBAEC;IAED;;;;;;;OAOG;IACH,UAJW,MAAM,OACN,MAAM,GACJ,MAAM,CAKlB;IAED;;;;;;;;OAQG;IACH,aAJW,MAAM,OACN,MAAM,GACJ,MAAM,CA+BlB;IAED;;;OAGG;IACH,YAHW,MAAM,GACJ,MAAM,CAIlB;IAED;;;OAGG;IACH,YAHW,MAAM,GACJ,MAAM,CAIlB;IAED;;;OAGG;IACH,oBAHW,MAAM,GACJ,MAAM,CAIlB;IAED;;;OAGG;IACH,iBAHW,MAAM,GACJ,OAAO,CAInB;IAED;;;OAGG;IACH,uBAHW,MAAM,GACJ,OAAO,CAInB;IAED;;;;;OAKG;IACH,WAHW,MAAM,GACJ,MAAM,CAIlB;IAED;;;;;OAKG;IACH,qBAFW,MAAM,QAQhB;IAED;;;;;;;;;;;;;OAaG;IACH,mBAFW,MAAM,QAKhB;IAED;;;;;;;;;;;;;;;;;;;;;;;;;OAyBG;IACH,kBAhBW,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,SACN,MAAM,eACN,MAAM,QAwBhB;IAED;;;;;;;;;;;;;;;OAeG;IACH,qBAHW,MAAM,OACN,MAAM,QAOhB;IAED;;;;;OAKG;IACH,oBAJW,MAAM,OACN,MAAM,GACJ,MAAM,CAIlB;IAED;;;;;;OAMG;IACH,kBALW,MAAM,OACN,MAAM,OACN,MAAM,EAAE,GAAC,YAAY,cACrB,MAAM,QAOhB;IAED;;;;;;OAMG;IACH,uBAJW,MAAM,OACN,MAAM,GACJ,MAAM,CAIlB;IAED,iEAAiE;IACjE,uBADY,MAAM,OAAe,MAAM,GAAgB,MAAM,CAG5D;IAED,iEAAiE;IACjE,uBADY,MAAM,OAAe,MAAM,GAAgB,MAAM,CAG5D;IAED,iEAAiE;IACjE,oBADY,MAAM,OAAe,MAAM,GAAgB,MAAM,CAG5D;IAED;;;;;OAKG;IACH,gCAEC;IAED;;;;;;OAMG;IACH,qCAEC;IAED;;;;;OAKG;IACH,uBAHW,MAAM,GACJ,MAAM,CAIlB;IAED;;;;;;OAMG;IACH,uBAHW,MAAM,GACJ,MAAM,CAIlB;IAED;;;OAGG;IACH,sBA6BC;IAED;;;OAGG;IACH,wBAcC;IAED;;;OAGG;IACH,uBAkBC;IAED;;OAEG;IACH,eA+BC;IAED;;;OAGG;IACH,oBAaC;IAED;;;OAGG;IACH,mBAqBC;CACJ;8BA9lBqD,2CAA2C"}