@woosh/meep-engine 2.140.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/ecs/Joint.d.ts

@@ -0,0 +1,179 @@
+/**
+ * Sentinel for {@link Joint#_jointId} before the joint is linked into a
+ * {@link PhysicsSystem}.
+ * @type {number}
+ */
+export const JOINT_UNALLOCATED: number;
+/**
+ * `entityB` value meaning "the world" — body A is constrained to a fixed
+ * world anchor rather than to a second body.
+ * @type {number}
+ */
+export const JOINT_WORLD: number;
+/**
+ * A configurable 6-DOF constraint between two bodies (or a body and the
+ * world). Connects body A's anchor frame to body B's; each of the 6 relative
+ * degrees of freedom (3 linear, 3 angular) is independently
+ * locked / free / limited / springy / motorised ({@link DofMode}). One
+ * constraint type expresses the whole joint taxonomy — see DofMode.
+ *
+ * The default configuration is a **ball-socket** (point-to-point): the three
+ * linear DOFs LOCKED (the two anchor points are held coincident), the three
+ * angular DOFs FREE. That alone gives chains, ropes, and pendulums.
+ *
+ * Anchors live in body-local frames. When `entityB === JOINT_WORLD`,
+ * `localAnchorB` is interpreted as a fixed **world** point instead of a
+ * local anchor on a second body.
+ *
+ * Solver state ({@link dofImpulse}, the `_*` fields) is owned by the
+ * PhysicsSystem; user code must not write it.
+ *
+ * @author Alex Goldring
+ * @copyright Company Named Limited (c) 2026
+ */
+export class Joint {
+    /**
+     * Entity owning body A (the constraint's first body — always a real body).
+     * @type {number}
+     */
+    entityA: number;
+    /**
+     * Entity owning body B, or {@link JOINT_WORLD} to anchor A to the world.
+     * @type {number}
+     */
+    entityB: number;
+    /**
+     * Anchor point on body A, in A's local frame.
+     * @readonly
+     * @type {Vector3}
+     */
+    readonly localAnchorA: Vector3;
+    /**
+     * Anchor point on body B in B's local frame — or, when
+     * `entityB === JOINT_WORLD`, a fixed world-space anchor point.
+     * @readonly
+     * @type {Vector3}
+     */
+    readonly localAnchorB: Vector3;
+    /**
+     * Orientation of the joint's reference frame on body A, in A's local
+     * space (a unit quaternion; identity = the joint frame is the body
+     * frame). The 3 linear and 3 angular DOFs are expressed in this frame:
+     * locked/limited/etc. axes are the frame's axes, not world axes. For a
+     * hinge, the free angular axis is one of these frame axes — orient the
+     * frame so that axis is the hinge axis.
+     * @readonly
+     * @type {Quaternion}
+     */
+    readonly localBasisA: Quaternion;
+    /**
+     * Orientation of the joint's reference frame on body B, in B's local
+     * space. At rest a fully-locked joint holds frame B aligned to frame A.
+     * When `entityB === JOINT_WORLD`, the world frame is treated as identity
+     * (angular locks hold A's frame to world axes).
+     * @readonly
+     * @type {Quaternion}
+     */
+    readonly localBasisB: Quaternion;
+    /**
+     * Per-DOF mode, 6 entries: `[lin x, lin y, lin z, ang x, ang y, ang z]`.
+     * Default = ball-socket (linear locked, angular free).
+     * @readonly
+     * @type {Uint8Array}
+     */
+    readonly dofMode: Uint8Array;
+    /**
+     * Lower / upper bounds per DOF, used by {@link DofMode.LIMITED}. Linear in
+     * metres, angular in radians. (Reserved — consumed once the LIMITED mode
+     * lands.)
+     * @readonly
+     * @type {Float64Array}
+     */
+    readonly dofLowerLimit: Float64Array;
+    /** @readonly @type {Float64Array} */
+    readonly dofUpperLimit: Float64Array;
+    /**
+     * Spring stiffness / damping per DOF, used by {@link DofMode.SPRING}.
+     * (Reserved — consumed once the SPRING mode lands.)
+     * @readonly
+     * @type {Float64Array}
+     */
+    readonly dofStiffness: Float64Array;
+    /** @readonly @type {Float64Array} */
+    readonly dofDamping: Float64Array;
+    /**
+     * Motor target velocity / max force per DOF, used by
+     * {@link DofMode.MOTOR}. (Reserved — consumed once the MOTOR mode lands.)
+     * @readonly
+     * @type {Float64Array}
+     */
+    readonly dofMotorTargetVelocity: Float64Array;
+    /** @readonly @type {Float64Array} */
+    readonly dofMotorMaxForce: Float64Array;
+    /**
+     * Accumulated constraint impulse per DOF — warm-start state carried across
+     * frames. System-owned.
+     * @readonly
+     * @type {Float64Array}
+     */
+    readonly dofImpulse: Float64Array;
+    /**
+     * Resolved packed body id of A, set at link time. System-private.
+     * @type {number}
+     */
+    _bodyIdA: number;
+    /**
+     * Resolved packed body id of B, or {@link JOINT_WORLD}. System-private.
+     * @type {number}
+     */
+    _bodyIdB: number;
+    /**
+     * Packed joint handle assigned at link. System-private.
+     * @type {number}
+     */
+    _jointId: number;
+    /**
+     * Configure this joint as a ball-socket (point-to-point) between two
+     * anchors. Linear DOFs locked, angular free — the default, provided as a
+     * named helper for clarity at call sites.
+     * @returns {Joint} this
+     */
+    asBallSocket(): Joint;
+    /**
+     * Configure as a weld (fixed) joint: all 6 DOFs locked, holding the two
+     * bodies rigidly in their relative pose.
+     * @returns {Joint} this
+     */
+    asWeld(): Joint;
+    /**
+     * Configure as a hinge (revolute): 3 linear + 2 angular locked, free to
+     * rotate about a single frame axis. Orient {@link localBasisA} /
+     * {@link localBasisB} so that frame axis is the desired hinge axis.
+     *
+     * @param {number} [freeAngularAxis] which frame axis is free: 0 = x
+     *   (default), 1 = y, 2 = z.
+     * @returns {Joint} this
+     */
+    asHinge(freeAngularAxis?: number): Joint;
+    /**
+     * Configure as a prismatic (slider): 2 linear + 3 angular locked, free to
+     * translate along a single frame axis. Orient {@link localBasisA} /
+     * {@link localBasisB} so that frame axis is the desired slide direction.
+     *
+     * @param {number} [freeLinearAxis] which frame axis is free: 0 = x
+     *   (default), 1 = y, 2 = z.
+     * @returns {Joint} this
+     */
+    asPrismatic(freeLinearAxis?: number): Joint;
+    /**
+     * @readonly
+     * @type {boolean}
+     */
+    readonly isJoint: boolean;
+}
+export namespace Joint {
+    let typeName: string;
+}
+import Vector3 from "../../../core/geom/Vector3.js";
+import Quaternion from "../../../core/geom/Quaternion.js";
+//# sourceMappingURL=Joint.d.ts.map

package/src/engine/physics/ecs/Joint.d.ts.map

@@ -0,0 +1 @@
+{"version":3,"file":"Joint.d.ts","sourceRoot":"","sources":["../../../../../src/engine/physics/ecs/Joint.js"],"names":[],"mappings":"AAIA;;;;GAIG;AACH,gCAFU,MAAM,CAEoB;AAEpC;;;;GAIG;AACH,0BAFU,MAAM,CAEc;AAE9B;;;;;;;;;;;;;;;;;;;;GAoBG;AACH;IAEI;;;OAGG;IACH,SAFU,MAAM,CAEH;IAEb;;;OAGG;IACH,SAFU,MAAM,CAEM;IAEtB;;;;OAIG;IACH,uBAFU,OAAO,CAEmB;IAEpC;;;;;OAKG;IACH,uBAFU,OAAO,CAEmB;IAEpC;;;;;;;;;OASG;IACH,sBAFU,UAAU,CAEW;IAE/B;;;;;;;OAOG;IACH,sBAFU,UAAU,CAEW;IAE/B;;;;;OAKG;IACH,kBAFU,UAAU,CAKjB;IAEH;;;;;;OAMG;IACH,wBAFU,YAAY,CAEc;IACpC,qCAAqC;IACrC,wBADqB,YAAY,CACG;IAEpC;;;;;OAKG;IACH,uBAFU,YAAY,CAEa;IACnC,qCAAqC;IACrC,qBADqB,YAAY,CACA;IAEjC;;;;;OAKG;IACH,iCAFU,YAAY,CAEuB;IAC7C,qCAAqC;IACrC,2BADqB,YAAY,CACM;IAEvC;;;;;OAKG;IACH,qBAFU,YAAY,CAEW;IAEjC;;;OAGG;IACH,UAFU,MAAM,CAEF;IACd;;;OAGG;IACH,UAFU,MAAM,CAEO;IACvB;;;OAGG;IACH,UAFU,MAAM,CAEa;IAE7B;;;;;OAKG;IACH,gBAFa,KAAK,CAUjB;IAED;;;;OAIG;IACH,UAFa,KAAK,CAKjB;IAED;;;;;;;;OAQG;IACH,0BAJW,MAAM,GAEJ,KAAK,CAWjB;IAED;;;;;;;;OAQG;IACH,6BAJW,MAAM,GAEJ,KAAK,CAWjB;IASL;;;OAGG;IACH,kBAFU,OAAO,CAEM;CAZtB;;kBAIS,MAAM;;oBAjOI,+BAA+B;uBAC5B,kCAAkC"}