@woosh/meep-engine 2.141.0 → 2.142.0

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

@@ -18,20 +18,46 @@ import { quat3_multiply } from "../../../core/geom/3d/quaternion/quat3_multiply.
  *
  * 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.
+ * motorised. This module implements **LOCKED** and **LIMITED** DOFs (linear
+ * and angular). LOCKED + FREE express the ball-socket, hinge, weld, and
+ * prismatic joints; LIMITED adds slider end-stops and joint range-of-motion.
+ * Springs and motors slot into the same per-DOF row model 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.
+ * ## One row model, parameterised by mode
+ *
+ * Every DOF resolves to the same scalar velocity row, contact-shaped: 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)`  (linear)
+ *   `K = d·Iw_A·d + d·Iw_B·d`                                       (angular)
+ *   `vrel = (vA + ωA×rA − vB − ωB×rB) · d`   (linear, A−B)
+ *   `vrel = (ωB − ωA) · d`                   (angular, B−A)
+ *   `λ = clamp(λ_acc − (1/K)·(vrel + bias)) − λ_acc`,
+ * applied as `±λ·d`. The **mode** picks only two row parameters:
+ *   - LOCKED  → `bias = β/h · error`, impulse clamp `(−∞, +∞)` (bilateral).
+ *   - LIMITED → row active only when the DOF position is at/beyond a bound;
+ *     `bias = β/h · (position − bound)`, impulse clamp one-sided
+ *     (`[0, +∞)` at the lower stop, `(−∞, 0]` at the upper). The clamp makes
+ *     it a unilateral push-out-of-violation constraint, exactly like a contact
+ *     normal — the limit can only resist crossing the stop, never pull the DOF
+ *     back into range.
+ * The warm-start and the iteration loop are mode-agnostic; they read the
+ * per-DOF `(active, effMass, bias, clampLo, clampHi)` filled by the mode-aware
+ * setup. Geometry (anchors / lever arms / position error) is recomputed from
+ * the current pose every substep, so the joint tracks the bodies as they move.
+ *
+ * The "position" along a DOF is `C · axis` for linear (signed anchor offset in
+ * metres) and `e · axis` for angular, where `e = 2·sign(qD.w)·qD.xyz` is the
+ * small-angle rotation vector of the relative rotation `qD = conj(qA)⊗qB`,
+ * expressed in frame-A-local coordinates (so error component `k` pairs with
+ * frame axis `k`). The angular position is therefore a first-order (small
+ * angle) measure: accurate for the modest ranges of slider/hinge end-stops,
+ * but it under-reports large angles (`2·sin(θ/2)` vs `θ`). Accurate wide-cone
+ * range-of-motion (ragdoll shoulders) needs the swing-twist decomposition,
+ * tracked as the follow-up; angular LIMITED here is the moderate-range cut.
+ * The decomposition itself is already implemented — reuse
+ * `Quaternion.computeSwingAndTwist(axis, swing, twist)` /
+ * `computeTwistAngle(axis)` (`core/geom/Quaternion.js`) on the relative
+ * rotation `qD` rather than writing a new one.
  *
  * @author Alex Goldring
  * @copyright Company Named Limited (c) 2026
@@ -61,6 +87,24 @@ const scratch_anchor = new Float64Array(3);
 const scratch_frame_a = new Float64Array(9);
 /** Scratch quaternion for frame composition / relative rotation. */
 const scratch_q = new Float64Array(4);
+/** Angular position error `e` per frame axis (frame-A-local small-angle vector). */
+const scratch_ang_err = new Float64Array(3);
+
+// Per-DOF row parameters, filled per joint by the mode-aware setup and consumed
+// by the mode-agnostic warm-start + iteration. Indexed 0..2 (the three frame
+// axes); the linear set drives imp[0..2], the angular set drives imp[3..5].
+const lin_active = new Uint8Array(3);
+const lin_eff = new Float64Array(3);
+const lin_bias = new Float64Array(3);
+const lin_clo = new Float64Array(3);
+const lin_chi = new Float64Array(3);
+const lin_gamma = new Float64Array(3);
+const ang_active = new Uint8Array(3);
+const ang_eff = new Float64Array(3);
+const ang_bias = new Float64Array(3);
+const ang_clo = new Float64Array(3);
+const ang_chi = new Float64Array(3);
+const ang_gamma = new Float64Array(3);
 
 /**
  * Angular effective-mass contribution of one body about a unit world axis:
@@ -151,10 +195,227 @@ function inv_mass_of(rb) {
     return rb.mass > 0 ? 1 / rb.mass : 0;
 }
 
+/**
+ * Clamp the SPOOK position bias to ±{@link MAX_JOINT_BIAS}.
+ * @param {number} b @returns {number}
+ */
+function clamp_bias(b) {
+    if (b > MAX_JOINT_BIAS) return MAX_JOINT_BIAS;
+    if (b < -MAX_JOINT_BIAS) return -MAX_JOINT_BIAS;
+    return b;
+}
+
+/**
+ * Swing-twist decomposition of the relative rotation `qD = conj(qA)⊗qB`, giving
+ * the per-frame-axis angular positions used by wide-cone angular DOFs.
+ *
+ * Decomposes `qD = swing ⊗ twist`, with **twist** the rotation about frame
+ * axis X (the bone / twist axis) and **swing** the residual tilt (a rotation
+ * about an axis in the YZ plane that carries X to its new direction). Writes,
+ * into `out`:
+ *   - `out[0]` = signed twist angle about X, in `(−π, π]` — *exact*, so a twist
+ *     limit engages at the true angle, not the `2·sin(θ/2)` small-angle proxy;
+ *   - `out[1]`, `out[2]` = the swing angle distributed over Y and Z
+ *     (`φ·axis`, φ the true swing angle), so independent Y/Z swing limits form
+ *     an accurate (box-shaped) cone that holds at large tilt.
+ * Reduces continuously to the small-angle vector near identity. Pure scratch /
+ * locals — no allocation, suitable for the per-substep hot loop. (The Quaternion
+ * class has an object-based `computeSwingAndTwist`; this is the inlined,
+ * allocation-free form — see the bench that justifies it.)
+ *
+ * @param {number} dx @param {number} dy @param {number} dz @param {number} dw  qD
+ * @param {Float64Array} out  length-3 destination (frame-axis positions)
+ */
+export function swing_twist_error(dx, dy, dz, dw, out) {
+    // Shortest arc: work with the hemisphere where dw ≥ 0.
+    if (dw < 0) { dx = -dx; dy = -dy; dz = -dz; dw = -dw; }
+
+    const nxt = Math.sqrt(dx * dx + dw * dw); // |twist| (before normalising)
+    let sy, sz, sw;
+    if (nxt < 1e-12) {
+        // dw ≈ 0 and dx ≈ 0: a ~180° pure swing — twist is undefined, take 0.
+        out[0] = 0;
+        // Swing is qD itself; distribute its angle over Y/Z below.
+        sy = dy; sz = dz; sw = dw;
+    } else {
+        out[0] = 2 * Math.atan2(dx, dw); // exact signed twist about X
+        const tx = dx / nxt, tw = dw / nxt; // normalised twist (tx,0,0,tw)
+        // swing = qD ⊗ conj(twist); its X component is identically 0.
+        sy = dy * tw - dz * tx;
+        sz = dy * tx + dz * tw;
+        sw = dw * tw + dx * tx;
+    }
+
+    if (sw < 0) { sy = -sy; sz = -sz; sw = -sw; }
+    const syz = Math.sqrt(sy * sy + sz * sz);
+    if (syz < 1e-9) {
+        // Near-zero swing: small-angle vector (continuous with φ·axis as φ→0).
+        out[1] = 2 * sy;
+        out[2] = 2 * sz;
+    } else {
+        const phi = 2 * Math.atan2(syz, sw); // exact swing angle in [0, π]
+        const s = phi / syz;
+        out[1] = sy * s;
+        out[2] = sz * s;
+    }
+}
+
+/**
+ * Fill the row parameters for one DOF given its mode, signed position and
+ * signed velocity.
+ *
+ * Writes `active / eff / bias / clampLo / clampHi` at index `k` of the supplied
+ * arrays and, for LIMITED, resets the warm-start impulse when the active bound
+ * flipped (a stale opposite-side impulse must not warm-start).
+ *
+ * ## LIMITED is a speculative (β = 1) one-sided velocity constraint
+ *
+ * Rather than waiting for the DOF to cross a stop and then pushing back with a
+ * Baumgarte bias — which injects real velocity and makes the stop *bounce* when
+ * nothing else loads the joint — the limit row is always live and uses the full
+ * `(position − bound)/h` as its bias. With the impulse clamped to one side this
+ * removes exactly the approach velocity that would overshoot, so the DOF
+ * **lands on the bound** with zero relative velocity (an inelastic stop, no
+ * penetration to recover, no rebound). When the DOF is far from the bound the
+ * large same-signed bias drives the clamp to zero, so the row is a no-op — it
+ * self-gates on approach. At a stop held by a sustained load (gravity on a
+ * slider) the per-substep load and the per-substep warm-start cancel, so the
+ * DOF rests exactly on the bound, mirroring a resting contact.
+ *
+ * Only the *push-out* side of the bias is clamped (to {@link MAX_JOINT_BIAS}),
+ * so a body teleported deep past a stop is eased out rather than yanked; the
+ * gating side is left unbounded.
+ *
+ * The active bound is the violated one, else — within range — the one the DOF
+ * is moving toward (`vel` sign). LOCKED is unchanged: a bilateral row with a
+ * `β/h` Baumgarte bias and no clamp.
+ *
+ * ## MOTOR drives the relative velocity toward a target
+ *
+ * A motor is a velocity row biased to the target (`bias = −target`, so the
+ * solve drives the relative velocity to the target) with the impulse clamped to
+ * `±maxForce·h` — a force-limited servo. With an unbounded force budget it is a
+ * perfect velocity drive; with a finite one it pulls toward the target as hard
+ * as it can (a weak motor cannot overcome a heavier load). The target follows
+ * the row's sign convention: linear is A−B along the frame axis, angular is
+ * B−A about it.
+ *
+ * ## SPRING is a SPOOK soft (compliant) constraint
+ *
+ * A spring drives the DOF position toward zero with stiffness `k` (N/m or
+ * N·m/rad) and damping `c`, as a *regularised* row rather than a hard one. The
+ * implicit-Euler soft constraint (Catto, "Soft Constraints") gives, per substep
+ * `h`, with `denom = c + h·k`:
+ *   `γ = 1 / (h·denom)`         the compliance / regularisation (inverse mass),
+ *   `bias = (k/denom)·C`        the restoring velocity, and a softened effective
+ *   `effMass = 1/(K + γ)`       mass — so the row solves
+ *   `λ = −effMass·(vrel + bias + γ·λ_accum)`.
+ * The `γ·λ_accum` term is what makes it compliant (the constraint yields in
+ * proportion to the force it carries), so a spring under load settles at a
+ * deflection rather than snapping rigid. Stiffness-only (`c = 0`) oscillates
+ * undamped; damping-only (`k = 0`) is a pure damper; both zero is inert. The
+ * row is bilateral (no clamp). This is the suspension / bungee / soft-return
+ * element, and the soft basis for cone-twist later. The angular position uses
+ * the same small-angle measure as LIMITED (good for modest ranges).
+ *
+ * @param {Joint} joint   the joint (per-DOF config + warm-start impulse read here)
+ * @param {number} dofIndex  DOF 0..5 (selects mode / limits / motor / imp slot)
+ * @param {number} k      axis index 0..2 (output-array slot for this DOF group)
+ * @param {number} keff   raw effective-mass denominator `K` (0 ⇒ no row); the
+ *   row's effective mass is `1/K`, or `1/(K+γ)` softened for a spring.
+ * @param {number} pos    signed DOF position (`C·axis` linear / `e·axis` angular)
+ * @param {number} vel    signed DOF velocity along the same axis (selects the
+ *   approached bound for an in-range LIMITED DOF)
+ * @param {number} spook_locked  Baumgarte gain `β / dt_sub` (LOCKED)
+ * @param {number} spook_spec    speculative gain `1 / dt_sub` (LIMITED)
+ * @param {number} dt_sub  substep size (MOTOR impulse cap = `maxForce·dt_sub`;
+ *   SPRING compliance derives from it)
+ * @param {Uint8Array} active @param {Float64Array} effOut
+ * @param {Float64Array} biasOut @param {Float64Array} cloOut @param {Float64Array} chiOut
+ * @param {Float64Array} gammaOut  per-row regularisation (0 for non-spring rows)
+ */
+function fill_row(joint, dofIndex, k, keff, pos, vel, spook_locked, spook_spec, dt_sub,
+                  active, effOut, biasOut, cloOut, chiOut, gammaOut) {
+    const mode = joint.dofMode[dofIndex];
+    const imp = joint.dofImpulse;
+
+    if (mode === DofMode.LOCKED) {
+        if (keff === 0) { active[k] = 0; return; }
+        effOut[k] = 1 / keff;
+        biasOut[k] = clamp_bias(spook_locked * pos);
+        cloOut[k] = -Infinity;
+        chiOut[k] = Infinity;
+        gammaOut[k] = 0;
+        active[k] = 1;
+        return;
+    }
+    if (mode === DofMode.LIMITED) {
+        if (keff === 0) { imp[dofIndex] = 0; active[k] = 0; return; }
+        const lower = joint.dofLowerLimit[dofIndex];
+        const upper = joint.dofUpperLimit[dofIndex];
+        // Select the bound: the violated one, else the one being approached.
+        let bound, lower_side;
+        if (pos <= lower) { bound = lower; lower_side = true; }
+        else if (pos >= upper) { bound = upper; lower_side = false; }
+        else { lower_side = vel <= 0; bound = lower_side ? lower : upper; }
+
+        let b = spook_spec * (pos - bound);
+        if (lower_side) {
+            // Lower stop: impulse pushes the DOF up (+), clamp [0, +∞). The
+            // push-out side of the bias is the negative one — bound it.
+            cloOut[k] = 0; chiOut[k] = Infinity;
+            if (imp[dofIndex] < 0) imp[dofIndex] = 0; // drop stale upper-side impulse
+            if (b < -MAX_JOINT_BIAS) b = -MAX_JOINT_BIAS;
+        } else {
+            // Upper stop: impulse pushes the DOF down (−), clamp (−∞, 0]. The
+            // push-out side of the bias is the positive one — bound it.
+            cloOut[k] = -Infinity; chiOut[k] = 0;
+            if (imp[dofIndex] > 0) imp[dofIndex] = 0; // drop stale lower-side impulse
+            if (b > MAX_JOINT_BIAS) b = MAX_JOINT_BIAS;
+        }
+        effOut[k] = 1 / keff;
+        biasOut[k] = b;
+        gammaOut[k] = 0;
+        active[k] = 1;
+        return;
+    }
+    if (mode === DofMode.MOTOR) {
+        if (keff === 0) { imp[dofIndex] = 0; active[k] = 0; return; }
+        const maxImpulse = joint.dofMotorMaxForce[dofIndex] * dt_sub;
+        if (maxImpulse <= 0) { imp[dofIndex] = 0; active[k] = 0; return; }
+        effOut[k] = 1 / keff;
+        // Drive the relative velocity to the target: λ = −eff·(vrel − target).
+        biasOut[k] = -joint.dofMotorTargetVelocity[dofIndex];
+        cloOut[k] = -maxImpulse;
+        chiOut[k] = maxImpulse;
+        gammaOut[k] = 0;
+        active[k] = 1;
+        return;
+    }
+    if (mode === DofMode.SPRING) {
+        if (keff === 0) { imp[dofIndex] = 0; active[k] = 0; return; }
+        const ks = joint.dofStiffness[dofIndex];
+        const cs = joint.dofDamping[dofIndex];
+        const denom = cs + dt_sub * ks;
+        if (denom <= 0) { imp[dofIndex] = 0; active[k] = 0; return; } // inert (no k, no c)
+        const gamma = 1 / (dt_sub * denom);
+        effOut[k] = 1 / (keff + gamma);
+        biasOut[k] = (ks / denom) * pos; // restoring velocity toward pos = 0
+        cloOut[k] = -Infinity;
+        chiOut[k] = Infinity;
+        gammaOut[k] = gamma;
+        active[k] = 1;
+        return;
+    }
+    // FREE: no row this substep.
+    imp[dofIndex] = 0;
+    active[k] = 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.
+ * derive each DOF's row parameters from its mode, replay the per-substep
+ * warm-start, and run `iters` velocity iterations.
  *
  * Called once per substep from `PhysicsSystem.fixedUpdate`, after the contact
  * solve so the two share the substep / warm-start cadence.
@@ -169,7 +430,8 @@ 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 spook_locked = JOINT_BAUMGARTE / dt_sub;
+    const spook_spec = 1 / dt_sub;
 
     const storage = system.storage;
 
@@ -207,6 +469,11 @@ export function solve_joints(joints, system, dt_sub, iters) {
         const invMB = to_world ? 0 : inv_mass_of(rbB);
         if (invMA === 0 && invMB === 0) continue;
 
+        const mode = joint.dofMode;
+        const linConstrained = mode[0] !== DofMode.FREE || mode[1] !== DofMode.FREE || mode[2] !== DofMode.FREE;
+        const angConstrained = mode[3] !== DofMode.FREE || mode[4] !== DofMode.FREE || mode[5] !== DofMode.FREE;
+        if (!linConstrained && !angConstrained) 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,
@@ -232,81 +499,59 @@ export function solve_joints(joints, system, dt_sub, iters) {
             anchorBz = trB.position.z + rBz;
         }
 
-        // Position error C = anchorA − anchorB (drive to zero on locked axes).
+        // Position error C = anchorA − anchorB (its projection on a frame axis
+        // is the signed DOF offset; locked axes drive it to zero, limited axes
+        // bound it).
         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;
-
+        // Every DOF is expressed in the joint's frame on body A. Frame A =
+        // body-A rotation ⊗ localBasisA; the rows of its matrix are the frame
+        // axes in world (see quat3_to_matrix3).
         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 fa = scratch_frame_a;
+
+        // --- Linear DOF row setup (frame axes). Position along axis k is
+        //     `C · axis_k`; convention is A−B (impulse +to A / −to B). ---
+        if (linConstrained) {
+            for (let k = 0; k < 3; k++) {
+                lin_active[k] = 0;
+                const m = mode[k];
+                if (m === DofMode.FREE) { imp[k] = 0; continue; }
+                const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
+                const keff = axis_effective_mass(rbA, trA, invMA, rAx, rAy, rAz, ax, ay, az)
+                    + (to_world ? 0 : axis_effective_mass(rbB, trB, invMB, rBx, rBy, rBz, ax, ay, az));
+                const pos = Cx * ax + Cy * ay + Cz * az;
+                // Relative anchor velocity along the axis (A−B), for LIMITED
+                // bound selection. Cheap; only the projection is used.
+                let vel = 0;
+                if (m === DofMode.LIMITED) {
+                    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));
+                    vel = rvx * ax + rvy * ay + rvz * az;
+                }
+                fill_row(joint, k, k, keff, pos, vel, spook_locked, spook_spec, dt_sub,
+                    lin_active, lin_eff, lin_bias, lin_clo, lin_chi, lin_gamma);
+            }
+        } else {
+            lin_active[0] = lin_active[1] = lin_active[2] = 0;
         }
-        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) {
+
+        // --- Angular DOF row setup (frame axes). Position along axis k is the
+        //     k-th component of the frame-local small-angle error vector `e`;
+        //     convention is B−A (impulse +to B / −to A). ---
+        if (angConstrained) {
             // 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;
@@ -315,58 +560,68 @@ export function solve_joints(joints, system, dt_sub, iters) {
                 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).
+            // qD = conj(qA) ⊗ qB → the per-axis angular position in frame-A
+            // coords. Two measures: the cheap small-angle rotation vector
+            // (default — exact at the origin, fine for welds / hinges / modest
+            // limits) or, when the joint opts into `swingTwist`, the swing-twist
+            // decomposition (X = twist, Y/Z = swing) with *exact* angles, for
+            // wide cone-twist range-of-motion (ragdoll shoulders) where the
+            // small-angle measure under-reports.
             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 (joint.swingTwist) {
+                swing_twist_error(scratch_q[0], scratch_q[1], scratch_q[2], scratch_q[3], scratch_ang_err);
+            } else {
+                const es = scratch_q[3] < 0 ? -2 : 2;
+                scratch_ang_err[0] = es * scratch_q[0];
+                scratch_ang_err[1] = es * scratch_q[1];
+                scratch_ang_err[2] = es * scratch_q[2];
             }
-            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;
+
+            for (let k = 0; k < 3; k++) {
+                ang_active[k] = 0;
+                const m = mode[3 + k];
+                if (m === DofMode.FREE) { imp[3 + k] = 0; continue; }
+                const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
+                const keff = angular_axis_effective_mass(rbA, trA, ax, ay, az)
+                    + (to_world ? 0 : angular_axis_effective_mass(rbB, trB, ax, ay, az));
+                const pos = scratch_ang_err[k];
+                // Relative angular velocity about the axis (B−A), for LIMITED
+                // bound selection.
+                let vel = 0;
+                if (m === DofMode.LIMITED) {
+                    vel = -(avA[0] * ax + avA[1] * ay + avA[2] * az);
+                    if (!to_world) vel += avB[0] * ax + avB[1] * ay + avB[2] * az;
+                }
+                fill_row(joint, 3 + k, k, keff, pos, vel, spook_locked, spook_spec, dt_sub,
+                    ang_active, ang_eff, ang_bias, ang_clo, ang_chi, ang_gamma);
             }
+        } else {
+            ang_active[0] = ang_active[1] = ang_active[2] = 0;
         }
 
-        // --- Per-substep warm-start (frame axes): linear impulse `Σ imp[i]·aᵢ`
-        //     applied at the anchors, angular impulse `Σ imp[3+i]·aᵢ` about the
+        if (!lin_active[0] && !lin_active[1] && !lin_active[2]
+            && !ang_active[0] && !ang_active[1] && !ang_active[2]) continue;
+
+        // --- Per-substep warm-start (frame axes): linear impulse `Σ imp[k]·aₖ`
+        //     applied at the anchors, angular impulse `Σ imp[3+k]·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;
+            let Px = 0, Py = 0, Pz = 0;
+            for (let k = 0; k < 3; k++) {
+                if (!lin_active[k]) continue;
+                const w = imp[k];
+                Px += w * fa[3 * k]; Py += w * fa[3 * k + 1]; Pz += w * fa[3 * k + 2];
+            }
             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;
+            let Lx = 0, Ly = 0, Lz = 0;
+            for (let k = 0; k < 3; k++) {
+                if (!ang_active[k]) continue;
+                const w = imp[3 + k];
+                Lx += w * fa[3 * k]; Ly += w * fa[3 * k + 1]; Lz += w * fa[3 * k + 2];
+            }
             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);
@@ -377,59 +632,41 @@ export function solve_joints(joints, system, dt_sub, iters) {
             // 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) {
+            for (let k = 0; k < 3; k++) {
+                if (!lin_active[k]) continue;
+                const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
                 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);
+                const vrel = rvx * ax + rvy * ay + rvz * az;
+                const old = imp[k];
+                // `+ γ·old` regularises a spring row (γ = 0 otherwise → exact).
+                let nv = old - lin_eff[k] * (vrel + lin_bias[k] + lin_gamma[k] * old);
+                if (nv < lin_clo[k]) nv = lin_clo[k]; else if (nv > lin_chi[k]) nv = lin_chi[k];
+                imp[k] = nv;
+                const d = nv - old;
+                if (d !== 0) {
+                    apply_impulse(rbA, trA, invMA, rAx, rAy, rAz, d * ax, d * ay, d * az, +1);
+                    if (!to_world) apply_impulse(rbB, trB, invMB, rBx, rBy, rBz, d * ax, d * ay, d * az, -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);
+            // Angular rows (frame axes): drive ωB−ωA about each axis toward the
+            // bias target, clamped per the row's bound.
+            for (let k = 0; k < 3; k++) {
+                if (!ang_active[k]) continue;
+                const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
+                let wrel = -(avA[0] * ax + avA[1] * ay + avA[2] * az);
+                if (!to_world) wrel += avB[0] * ax + avB[1] * ay + avB[2] * az;
+                const old = imp[3 + k];
+                let nv = old - ang_eff[k] * (wrel + ang_bias[k] + ang_gamma[k] * old);
+                if (nv < ang_clo[k]) nv = ang_clo[k]; else if (nv > ang_chi[k]) nv = ang_chi[k];
+                imp[3 + k] = nv;
+                const d = nv - old;
+                if (d !== 0) {
+                    if (!to_world) apply_angular_impulse(rbB, trB, ax, ay, az, d, +1);
+                    apply_angular_impulse(rbA, trA, ax, ay, az, d, -1);
+                }
             }
         }
     }