@woosh/meep-engine 2.156.0 → 2.157.0

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

@@ -1,805 +1,830 @@
-import { quat3_multiply } from "../../../core/geom/3d/quaternion/quat3_multiply.js";
-import { quat3_to_matrix3 } from "../../../core/geom/3d/quaternion/quat3_to_matrix3.js";
-import { v3_quat3_apply } from "../../../core/geom/vec3/v3_quat3_apply.js";
-import { body_id_index } from "../body/BodyStorage.js";
-import {
-    SBS_AV_X,
-    SBS_AV_Y,
-    SBS_AV_Z,
-    SBS_INV_I_X,
-    SBS_INV_I_Y,
-    SBS_INV_I_Z,
-    SBS_INV_MASS,
-    SBS_LV_X,
-    SBS_LV_Y,
-    SBS_LV_Z,
-    SBS_QW,
-    SBS_QX,
-    SBS_QY,
-    SBS_QZ,
-    SBS_STRIDE,
-} from "../body/SolverBodyState.js";
-import { JOINT_WORLD } from "../ecs/Joint.js";
-import { SleepState } from "../ecs/SleepState.js";
-import { world_inverse_inertia_apply_raw } from "../inertia/world_inverse_inertia.js";
-import { DofMode } from "./DofMode.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** 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.
- *
- * ## 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
- */
-
-/**
- * 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 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);
-
-// A2 (block solve) scratch: the coupled 3×3 effective-mass of a fully-LOCKED
-// linear point constraint and its inverse, stored as the 6 symmetric entries
-// [00, 01, 02, 11, 12, 22]; plus the per-body cross products `r×aₖ` and their
-// inverse-inertia images `Iw⁻¹·(r×aₖ)`.
-const lin_K = new Float64Array(6);
-const lin_Kinv = new Float64Array(6);
-const block_c = new Float64Array(9);
-const block_w = new Float64Array(9);
-
-/**
- * Angular effective-mass contribution of one body about a unit world axis:
- * `a · Iw⁻¹ · a`. Zero for non-dynamic / rotation-locked bodies (whose
- * {@link SolverBodyState} inverse-inertia diagonal is zero).
- * @param {Float64Array} ss solver-body-state data array
- * @param {number} base `body_index * SBS_STRIDE`
- * @returns {number}
- */
-function angular_axis_effective_mass(ss, base, ax, ay, az) {
-    const iix = ss[base + SBS_INV_I_X], iiy = ss[base + SBS_INV_I_Y], iiz = ss[base + SBS_INV_I_Z];
-    if (iix === 0 && iiy === 0 && iiz === 0) return 0;
-    world_inverse_inertia_apply_raw(scratch_inertia, 0, iix, iiy, iiz,
-        ss[base + SBS_QX], ss[base + SBS_QY], ss[base + SBS_QZ], ss[base + SBS_QW], 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 {Float64Array} ss solver-body-state data array
- * @param {number} base `body_index * SBS_STRIDE`
- * @param {number} ax @param {number} ay @param {number} az
- * @param {number} lambda @param {number} sign
- */
-function apply_angular_impulse(ss, base, ax, ay, az, lambda, sign) {
-    const iix = ss[base + SBS_INV_I_X], iiy = ss[base + SBS_INV_I_Y], iiz = ss[base + SBS_INV_I_Z];
-    if (iix === 0 && iiy === 0 && iiz === 0) return;
-    const L = sign * lambda;
-    world_inverse_inertia_apply_raw(scratch_inertia, 0, iix, iiy, iiz,
-        ss[base + SBS_QX], ss[base + SBS_QY], ss[base + SBS_QZ], ss[base + SBS_QW], ax * L, ay * L, az * L);
-    ss[base + SBS_AV_X] += scratch_inertia[0];
-    ss[base + SBS_AV_Y] += scratch_inertia[1];
-    ss[base + SBS_AV_Z] += 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 {Float64Array} ss solver-body-state data array
- * @param {number} base `body_index * SBS_STRIDE`
- * @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(ss, base, invM, rx, ry, rz, dx, dy, dz) {
-    if (invM === 0) return 0;
-    let k = invM;
-    const iix = ss[base + SBS_INV_I_X], iiy = ss[base + SBS_INV_I_Y], iiz = ss[base + SBS_INV_I_Z];
-    if (iix !== 0 || iiy !== 0 || iiz !== 0) {
-        const cx = ry * dz - rz * dy;
-        const cy = rz * dx - rx * dz;
-        const cz = rx * dy - ry * dx;
-        world_inverse_inertia_apply_raw(scratch_inertia, 0, iix, iiy, iiz,
-            ss[base + SBS_QX], ss[base + SBS_QY], ss[base + SBS_QZ], ss[base + SBS_QW], 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).
- * Reads / writes the body's {@link SolverBodyState} span.
- *
- * @param {Float64Array} ss solver-body-state data array
- * @param {number} base `body_index * SBS_STRIDE`
- * @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(ss, base, invM, rx, ry, rz, Px, Py, Pz, sign) {
-    if (invM === 0) return;
-    const sPx = sign * Px, sPy = sign * Py, sPz = sign * Pz;
-    ss[base + SBS_LV_X] += sPx * invM;
-    ss[base + SBS_LV_Y] += sPy * invM;
-    ss[base + SBS_LV_Z] += sPz * invM;
-
-    const tx = ry * sPz - rz * sPy;
-    const ty = rz * sPx - rx * sPz;
-    const tz = rx * sPy - ry * sPx;
-    world_inverse_inertia_apply_raw(scratch_inertia, 0,
-        ss[base + SBS_INV_I_X], ss[base + SBS_INV_I_Y], ss[base + SBS_INV_I_Z],
-        ss[base + SBS_QX], ss[base + SBS_QY], ss[base + SBS_QZ], ss[base + SBS_QW], tx, ty, tz);
-    ss[base + SBS_AV_X] += scratch_inertia[0];
-    ss[base + SBS_AV_Y] += scratch_inertia[1];
-    ss[base + SBS_AV_Z] += scratch_inertia[2];
-}
-
-/**
- * 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;
-}
-
-/**
- * A2 — accumulate one body's angular contribution to the 3×3 point-constraint
- * effective-mass block `K` (frame axes): for each frame axis `aₖ`,
- * `cₖ = r×aₖ` and `wₖ = Iw⁻¹·cₖ`, then `K_kl += cₖ·w_l`. Symmetric, so only the
- * upper triangle is written. Rotation-locked / non-dynamic bodies (zero inverse
- * inertia) add nothing. The mass term `(invMA+invMB)·I` is seeded by the caller.
- *
- * @param {Float64Array} ss @param {number} base
- * @param {number} rx @param {number} ry @param {number} rz  lever arm
- * @param {Float64Array} fa  frame-A world basis (rows = axes)
- * @param {Float64Array} K   length-6 symmetric accumulator [00,01,02,11,12,22]
- */
-function accumulate_point_block(ss, base, rx, ry, rz, fa, K) {
-    const iix = ss[base + SBS_INV_I_X], iiy = ss[base + SBS_INV_I_Y], iiz = ss[base + SBS_INV_I_Z];
-    if (iix === 0 && iiy === 0 && iiz === 0) return;
-    const qx = ss[base + SBS_QX], qy = ss[base + SBS_QY], qz = ss[base + SBS_QZ], qw = ss[base + SBS_QW];
-    for (let k = 0; k < 3; k++) {
-        const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
-        const cx = ry * az - rz * ay, cy = rz * ax - rx * az, cz = rx * ay - ry * ax;
-        block_c[3 * k] = cx; block_c[3 * k + 1] = cy; block_c[3 * k + 2] = cz;
-        world_inverse_inertia_apply_raw(block_w, 3 * k, iix, iiy, iiz, qx, qy, qz, qw, cx, cy, cz);
-    }
-    K[0] += block_c[0] * block_w[0] + block_c[1] * block_w[1] + block_c[2] * block_w[2];
-    K[1] += block_c[0] * block_w[3] + block_c[1] * block_w[4] + block_c[2] * block_w[5];
-    K[2] += block_c[0] * block_w[6] + block_c[1] * block_w[7] + block_c[2] * block_w[8];
-    K[3] += block_c[3] * block_w[3] + block_c[4] * block_w[4] + block_c[5] * block_w[5];
-    K[4] += block_c[3] * block_w[6] + block_c[4] * block_w[7] + block_c[5] * block_w[8];
-    K[5] += block_c[6] * block_w[6] + block_c[7] * block_w[7] + block_c[8] * block_w[8];
-}
-
-/**
- * Invert a symmetric positive-definite 3×3 (6-entry form) in closed form,
- * writing the inverse (same 6-entry form) to `out`. Returns the determinant; a
- * near-singular block (|det| < 1e-12) returns 0 and leaves `out` untouched so
- * the caller falls back to the per-axis solve.
- *
- * @param {Float64Array} K  [00,01,02,11,12,22] @param {Float64Array} out
- * @returns {number} determinant
- */
-function invert_sym3(K, out) {
-    const a = K[0], b = K[1], c = K[2], d = K[3], e = K[4], f = K[5];
-    const A00 = d * f - e * e, A01 = c * e - b * f, A02 = b * e - c * d;
-    const A11 = a * f - c * c, A12 = b * c - a * e, A22 = a * d - b * b;
-    const det = a * A00 + b * A01 + c * A02;
-    if (det < 1e-12 && det > -1e-12) return 0;
-    const inv = 1 / det;
-    out[0] = A00 * inv; out[1] = A01 * inv; out[2] = A02 * inv;
-    out[3] = A11 * inv; out[4] = A12 * inv; out[5] = A22 * inv;
-    return det;
-}
-
-/**
- * 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,
- * 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.
- *
- * @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
- * @param {boolean} [reverse]  A2: traverse the joint array tip→root this call
- *   (the caller alternates it per substep for a symmetric Gauss-Seidel sweep, so
- *   chain impulses propagate both ways). Default false ⇒ original root→tip order.
- */
-export function solve_joints(joints, system, dt_sub, iters, reverse) {
-    const n = joints.length;
-    if (n === 0 || dt_sub <= 0) return;
-
-    const spook_locked = JOINT_BAUMGARTE / dt_sub;
-    const spook_spec = 1 / dt_sub;
-
-    const storage = system.storage;
-    const ss = system.__solver_state.data;
-
-    for (let jj = 0; jj < n; jj++) {
-        const ji = reverse ? (n - 1 - jj) : jj;
-        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 = body_id_index(joint._bodyIdA);
-        const baseA = idxA * SBS_STRIDE;
-        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, baseB = -1;
-        if (!to_world) {
-            if (!storage.is_valid(joint._bodyIdB)) continue;
-            idxB = body_id_index(joint._bodyIdB);
-            baseB = idxB * SBS_STRIDE;
-            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 = ss[baseA + SBS_INV_MASS];
-        const invMB = to_world ? 0 : ss[baseB + SBS_INV_MASS];
-        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,
-            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 (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 imp = joint.dofImpulse;
-
-        // === Frame A world axes — shared by the linear AND angular rows ===
-        // 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 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). ---
-        let useLinBlock = false;
-        if (linConstrained) {
-            // A2: a fully-LOCKED linear triple is a ball-socket point — solve it
-            // as one coupled 3×3 block (frame axes) instead of axis-by-axis,
-            // removing the intra-joint coupling error the sequential per-axis
-            // Gauss-Seidel leaves behind. Falls back to per-axis if the block is
-            // near-singular.
-            if (mode[0] === DofMode.LOCKED && mode[1] === DofMode.LOCKED && mode[2] === DofMode.LOCKED) {
-                lin_K[0] = lin_K[3] = lin_K[5] = invMA + invMB;
-                lin_K[1] = lin_K[2] = lin_K[4] = 0;
-                accumulate_point_block(ss, baseA, rAx, rAy, rAz, fa, lin_K);
-                if (!to_world) accumulate_point_block(ss, baseB, rBx, rBy, rBz, fa, lin_K);
-                if (invert_sym3(lin_K, lin_Kinv) !== 0) {
-                    for (let k = 0; k < 3; k++) {
-                        const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
-                        lin_bias[k] = clamp_bias(spook_locked * (Cx * ax + Cy * ay + Cz * az));
-                        lin_active[k] = 1;
-                    }
-                    useLinBlock = true;
-                }
-            }
-            if (!useLinBlock) {
-                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(ss, baseA, invMA, rAx, rAy, rAz, ax, ay, az)
-                        + (to_world ? 0 : axis_effective_mass(ss, baseB, 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 = (ss[baseA + SBS_LV_X] + ss[baseA + SBS_AV_Y] * rAz - ss[baseA + SBS_AV_Z] * rAy) - (to_world ? 0 : (ss[baseB + SBS_LV_X] + ss[baseB + SBS_AV_Y] * rBz - ss[baseB + SBS_AV_Z] * rBy));
-                        const rvy = (ss[baseA + SBS_LV_Y] + ss[baseA + SBS_AV_Z] * rAx - ss[baseA + SBS_AV_X] * rAz) - (to_world ? 0 : (ss[baseB + SBS_LV_Y] + ss[baseB + SBS_AV_Z] * rBx - ss[baseB + SBS_AV_X] * rBz));
-                        const rvz = (ss[baseA + SBS_LV_Z] + ss[baseA + SBS_AV_X] * rAy - ss[baseA + SBS_AV_Y] * rAx) - (to_world ? 0 : (ss[baseB + SBS_LV_Z] + ss[baseB + SBS_AV_X] * rBy - ss[baseB + SBS_AV_Y] * 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;
-        }
-
-        // --- 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;
-            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 → 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);
-            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];
-            }
-
-            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(ss, baseA, ax, ay, az)
-                    + (to_world ? 0 : angular_axis_effective_mass(ss, baseB, 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 = -(ss[baseA + SBS_AV_X] * ax + ss[baseA + SBS_AV_Y] * ay + ss[baseA + SBS_AV_Z] * az);
-                    if (!to_world) vel += ss[baseB + SBS_AV_X] * ax + ss[baseB + SBS_AV_Y] * ay + ss[baseB + SBS_AV_Z] * 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;
-        }
-
-        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). ---
-        {
-            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(ss, baseA, invMA, rAx, rAy, rAz, Px, Py, Pz, +1);
-                if (!to_world) apply_impulse(ss, baseB, invMB, rBx, rBy, rBz, Px, Py, Pz, -1);
-            }
-            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(ss, baseB, Lx, Ly, Lz, 1, +1);
-                apply_angular_impulse(ss, baseA, 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 (useLinBlock) {
-                // A2: coupled 3×3 point solve — one relative anchor velocity,
-                // projected onto the frame axes, solved against Kⁱⁿᵛ in a single
-                // Newton step (bilateral, no clamp). The point constraint is
-                // satisfied exactly each iteration; only its coupling with the
-                // angular rows remains to iterate.
-                const rvx = (ss[baseA + SBS_LV_X] + ss[baseA + SBS_AV_Y] * rAz - ss[baseA + SBS_AV_Z] * rAy) - (to_world ? 0 : (ss[baseB + SBS_LV_X] + ss[baseB + SBS_AV_Y] * rBz - ss[baseB + SBS_AV_Z] * rBy));
-                const rvy = (ss[baseA + SBS_LV_Y] + ss[baseA + SBS_AV_Z] * rAx - ss[baseA + SBS_AV_X] * rAz) - (to_world ? 0 : (ss[baseB + SBS_LV_Y] + ss[baseB + SBS_AV_Z] * rBx - ss[baseB + SBS_AV_X] * rBz));
-                const rvz = (ss[baseA + SBS_LV_Z] + ss[baseA + SBS_AV_X] * rAy - ss[baseA + SBS_AV_Y] * rAx) - (to_world ? 0 : (ss[baseB + SBS_LV_Z] + ss[baseB + SBS_AV_X] * rBy - ss[baseB + SBS_AV_Y] * rBx));
-                const a0x = fa[0], a0y = fa[1], a0z = fa[2];
-                const a1x = fa[3], a1y = fa[4], a1z = fa[5];
-                const a2x = fa[6], a2y = fa[7], a2z = fa[8];
-                const b0 = rvx * a0x + rvy * a0y + rvz * a0z + lin_bias[0];
-                const b1 = rvx * a1x + rvy * a1y + rvz * a1z + lin_bias[1];
-                const b2 = rvx * a2x + rvy * a2y + rvz * a2z + lin_bias[2];
-                const d0 = -(lin_Kinv[0] * b0 + lin_Kinv[1] * b1 + lin_Kinv[2] * b2);
-                const d1 = -(lin_Kinv[1] * b0 + lin_Kinv[3] * b1 + lin_Kinv[4] * b2);
-                const d2 = -(lin_Kinv[2] * b0 + lin_Kinv[4] * b1 + lin_Kinv[5] * b2);
-                imp[0] += d0; imp[1] += d1; imp[2] += d2;
-                const Px = d0 * a0x + d1 * a1x + d2 * a2x;
-                const Py = d0 * a0y + d1 * a1y + d2 * a2y;
-                const Pz = d0 * a0z + d1 * a1z + d2 * a2z;
-                if (Px !== 0 || Py !== 0 || Pz !== 0) {
-                    apply_impulse(ss, baseA, invMA, rAx, rAy, rAz, Px, Py, Pz, +1);
-                    if (!to_world) apply_impulse(ss, baseB, invMB, rBx, rBy, rBz, Px, Py, Pz, -1);
-                }
-            } else {
-                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 = (ss[baseA + SBS_LV_X] + ss[baseA + SBS_AV_Y] * rAz - ss[baseA + SBS_AV_Z] * rAy) - (to_world ? 0 : (ss[baseB + SBS_LV_X] + ss[baseB + SBS_AV_Y] * rBz - ss[baseB + SBS_AV_Z] * rBy));
-                    const rvy = (ss[baseA + SBS_LV_Y] + ss[baseA + SBS_AV_Z] * rAx - ss[baseA + SBS_AV_X] * rAz) - (to_world ? 0 : (ss[baseB + SBS_LV_Y] + ss[baseB + SBS_AV_Z] * rBx - ss[baseB + SBS_AV_X] * rBz));
-                    const rvz = (ss[baseA + SBS_LV_Z] + ss[baseA + SBS_AV_X] * rAy - ss[baseA + SBS_AV_Y] * rAx) - (to_world ? 0 : (ss[baseB + SBS_LV_Z] + ss[baseB + SBS_AV_X] * rBy - ss[baseB + SBS_AV_Y] * rBx));
-                    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(ss, baseA, invMA, rAx, rAy, rAz, d * ax, d * ay, d * az, +1);
-                        if (!to_world) apply_impulse(ss, baseB, invMB, rBx, rBy, rBz, d * ax, d * ay, d * az, -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 = -(ss[baseA + SBS_AV_X] * ax + ss[baseA + SBS_AV_Y] * ay + ss[baseA + SBS_AV_Z] * az);
-                if (!to_world) wrel += ss[baseB + SBS_AV_X] * ax + ss[baseB + SBS_AV_Y] * ay + ss[baseB + SBS_AV_Z] * 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(ss, baseB, ax, ay, az, d, +1);
-                    apply_angular_impulse(ss, baseA, ax, ay, az, d, -1);
-                }
-            }
-        }
-    }
-}
+import { quat3_multiply } from "../../../core/geom/3d/quaternion/quat3_multiply.js";
+import { quat3_to_matrix3 } from "../../../core/geom/3d/quaternion/quat3_to_matrix3.js";
+import { v3_quat3_apply } from "../../../core/geom/vec3/v3_quat3_apply.js";
+import { body_id_index } from "../body/BodyStorage.js";
+import {
+    SBS_AV_X,
+    SBS_AV_Y,
+    SBS_AV_Z,
+    SBS_INV_I_X,
+    SBS_INV_I_Y,
+    SBS_INV_I_Z,
+    SBS_INV_MASS,
+    SBS_LV_X,
+    SBS_LV_Y,
+    SBS_LV_Z,
+    SBS_QW,
+    SBS_QX,
+    SBS_QY,
+    SBS_QZ,
+    SBS_STRIDE,
+} from "../body/SolverBodyState.js";
+import { JOINT_WORLD } from "../ecs/Joint.js";
+import { SleepState } from "../ecs/SleepState.js";
+import { world_inverse_inertia_apply_raw } from "../inertia/world_inverse_inertia.js";
+import { DofMode } from "./DofMode.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** 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.
+ *
+ * ## 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
+ */
+
+/**
+ * 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 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);
+
+// A2 (block solve) scratch: the coupled 3×3 effective-mass of a fully-LOCKED
+// linear point constraint and its inverse, stored as the 6 symmetric entries
+// [00, 01, 02, 11, 12, 22]; plus the per-body cross products `r×aₖ` and their
+// inverse-inertia images `Iw⁻¹·(r×aₖ)`.
+const lin_K = new Float64Array(6);
+const lin_Kinv = new Float64Array(6);
+const block_c = new Float64Array(9);
+const block_w = new Float64Array(9);
+
+/**
+ * Angular effective-mass contribution of one body about a unit world axis:
+ * `a · Iw⁻¹ · a`. Zero for non-dynamic / rotation-locked bodies (whose
+ * {@link SolverBodyState} inverse-inertia diagonal is zero).
+ * @param {Float64Array} ss solver-body-state data array
+ * @param {number} base `body_index * SBS_STRIDE`
+ * @returns {number}
+ */
+function angular_axis_effective_mass(ss, base, ax, ay, az) {
+    const iix = ss[base + SBS_INV_I_X], iiy = ss[base + SBS_INV_I_Y], iiz = ss[base + SBS_INV_I_Z];
+    if (iix === 0 && iiy === 0 && iiz === 0) return 0;
+    world_inverse_inertia_apply_raw(scratch_inertia, 0, iix, iiy, iiz,
+        ss[base + SBS_QX], ss[base + SBS_QY], ss[base + SBS_QZ], ss[base + SBS_QW], 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 {Float64Array} ss solver-body-state data array
+ * @param {number} base `body_index * SBS_STRIDE`
+ * @param {number} ax @param {number} ay @param {number} az
+ * @param {number} lambda @param {number} sign
+ */
+function apply_angular_impulse(ss, base, ax, ay, az, lambda, sign) {
+    const iix = ss[base + SBS_INV_I_X], iiy = ss[base + SBS_INV_I_Y], iiz = ss[base + SBS_INV_I_Z];
+    if (iix === 0 && iiy === 0 && iiz === 0) return;
+    const L = sign * lambda;
+    world_inverse_inertia_apply_raw(scratch_inertia, 0, iix, iiy, iiz,
+        ss[base + SBS_QX], ss[base + SBS_QY], ss[base + SBS_QZ], ss[base + SBS_QW], ax * L, ay * L, az * L);
+    ss[base + SBS_AV_X] += scratch_inertia[0];
+    ss[base + SBS_AV_Y] += scratch_inertia[1];
+    ss[base + SBS_AV_Z] += 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 {Float64Array} ss solver-body-state data array
+ * @param {number} base `body_index * SBS_STRIDE`
+ * @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(ss, base, invM, rx, ry, rz, dx, dy, dz) {
+    if (invM === 0) return 0;
+    let k = invM;
+    const iix = ss[base + SBS_INV_I_X], iiy = ss[base + SBS_INV_I_Y], iiz = ss[base + SBS_INV_I_Z];
+    if (iix !== 0 || iiy !== 0 || iiz !== 0) {
+        const cx = ry * dz - rz * dy;
+        const cy = rz * dx - rx * dz;
+        const cz = rx * dy - ry * dx;
+        world_inverse_inertia_apply_raw(scratch_inertia, 0, iix, iiy, iiz,
+            ss[base + SBS_QX], ss[base + SBS_QY], ss[base + SBS_QZ], ss[base + SBS_QW], cx, cy, cz);
+        k += cx * scratch_inertia[0] + cy * scratch_inertia[1] + cz * scratch_inertia[2];
+    }
+    return k;
+}
+
+/**
+ * Relative velocity of the joint anchors, A − B: each side's anchor-point
+ * velocity `v + ω × r` read from its SolverBodyState span; the B term is
+ * zero for a world anchor. Writes 3 floats into `out`. Expressions are
+ * verbatim from the three solver sites that previously inlined this, so
+ * outputs are bit-identical. Re-evaluated before every row on purpose —
+ * Gauss-Seidel rows must see the prior rows' impulses.
+ *
+ * @param {Float64Array} out length >= 3
+ * @param {Float64Array} ss solver-body-state data
+ * @param {number} baseA
+ * @param {number} rAx
+ * @param {number} rAy
+ * @param {number} rAz anchor lever on A
+ * @param {number} baseB
+ * @param {number} rBx
+ * @param {number} rBy
+ * @param {number} rBz anchor lever on B
+ * @param {boolean} to_world B is a fixed world anchor (no B term)
+ */
+function relative_anchor_velocity(out, ss, baseA, rAx, rAy, rAz, baseB, rBx, rBy, rBz, to_world) {
+    out[0] = (ss[baseA + SBS_LV_X] + ss[baseA + SBS_AV_Y] * rAz - ss[baseA + SBS_AV_Z] * rAy) - (to_world ? 0 : (ss[baseB + SBS_LV_X] + ss[baseB + SBS_AV_Y] * rBz - ss[baseB + SBS_AV_Z] * rBy));
+    out[1] = (ss[baseA + SBS_LV_Y] + ss[baseA + SBS_AV_Z] * rAx - ss[baseA + SBS_AV_X] * rAz) - (to_world ? 0 : (ss[baseB + SBS_LV_Y] + ss[baseB + SBS_AV_Z] * rBx - ss[baseB + SBS_AV_X] * rBz));
+    out[2] = (ss[baseA + SBS_LV_Z] + ss[baseA + SBS_AV_X] * rAy - ss[baseA + SBS_AV_Y] * rAx) - (to_world ? 0 : (ss[baseB + SBS_LV_Z] + ss[baseB + SBS_AV_X] * rBy - ss[baseB + SBS_AV_Y] * rBx));
+}
+
+/** Destination for {@link relative_anchor_velocity}. */
+const scratch_rv = new Float64Array(3);
+
+/**
+ * Apply impulse `P` at body-relative offset `r`: Δv = P·invM, Δω = Iw⁻¹·(r×P).
+ * Reads / writes the body's {@link SolverBodyState} span.
+ *
+ * @param {Float64Array} ss solver-body-state data array
+ * @param {number} base `body_index * SBS_STRIDE`
+ * @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(ss, base, invM, rx, ry, rz, Px, Py, Pz, sign) {
+    if (invM === 0) return;
+    const sPx = sign * Px, sPy = sign * Py, sPz = sign * Pz;
+    ss[base + SBS_LV_X] += sPx * invM;
+    ss[base + SBS_LV_Y] += sPy * invM;
+    ss[base + SBS_LV_Z] += sPz * invM;
+
+    const tx = ry * sPz - rz * sPy;
+    const ty = rz * sPx - rx * sPz;
+    const tz = rx * sPy - ry * sPx;
+    world_inverse_inertia_apply_raw(scratch_inertia, 0,
+        ss[base + SBS_INV_I_X], ss[base + SBS_INV_I_Y], ss[base + SBS_INV_I_Z],
+        ss[base + SBS_QX], ss[base + SBS_QY], ss[base + SBS_QZ], ss[base + SBS_QW], tx, ty, tz);
+    ss[base + SBS_AV_X] += scratch_inertia[0];
+    ss[base + SBS_AV_Y] += scratch_inertia[1];
+    ss[base + SBS_AV_Z] += scratch_inertia[2];
+}
+
+/**
+ * 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;
+}
+
+/**
+ * A2 — accumulate one body's angular contribution to the 3×3 point-constraint
+ * effective-mass block `K` (frame axes): for each frame axis `aₖ`,
+ * `cₖ = r×aₖ` and `wₖ = Iw⁻¹·cₖ`, then `K_kl += cₖ·w_l`. Symmetric, so only the
+ * upper triangle is written. Rotation-locked / non-dynamic bodies (zero inverse
+ * inertia) add nothing. The mass term `(invMA+invMB)·I` is seeded by the caller.
+ *
+ * @param {Float64Array} ss @param {number} base
+ * @param {number} rx @param {number} ry @param {number} rz  lever arm
+ * @param {Float64Array} fa  frame-A world basis (rows = axes)
+ * @param {Float64Array} K   length-6 symmetric accumulator [00,01,02,11,12,22]
+ */
+function accumulate_point_block(ss, base, rx, ry, rz, fa, K) {
+    const iix = ss[base + SBS_INV_I_X], iiy = ss[base + SBS_INV_I_Y], iiz = ss[base + SBS_INV_I_Z];
+    if (iix === 0 && iiy === 0 && iiz === 0) return;
+    const qx = ss[base + SBS_QX], qy = ss[base + SBS_QY], qz = ss[base + SBS_QZ], qw = ss[base + SBS_QW];
+    for (let k = 0; k < 3; k++) {
+        const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
+        const cx = ry * az - rz * ay, cy = rz * ax - rx * az, cz = rx * ay - ry * ax;
+        block_c[3 * k] = cx; block_c[3 * k + 1] = cy; block_c[3 * k + 2] = cz;
+        world_inverse_inertia_apply_raw(block_w, 3 * k, iix, iiy, iiz, qx, qy, qz, qw, cx, cy, cz);
+    }
+    K[0] += block_c[0] * block_w[0] + block_c[1] * block_w[1] + block_c[2] * block_w[2];
+    K[1] += block_c[0] * block_w[3] + block_c[1] * block_w[4] + block_c[2] * block_w[5];
+    K[2] += block_c[0] * block_w[6] + block_c[1] * block_w[7] + block_c[2] * block_w[8];
+    K[3] += block_c[3] * block_w[3] + block_c[4] * block_w[4] + block_c[5] * block_w[5];
+    K[4] += block_c[3] * block_w[6] + block_c[4] * block_w[7] + block_c[5] * block_w[8];
+    K[5] += block_c[6] * block_w[6] + block_c[7] * block_w[7] + block_c[8] * block_w[8];
+}
+
+/**
+ * Invert a symmetric positive-definite 3×3 (6-entry form) in closed form,
+ * writing the inverse (same 6-entry form) to `out`. Returns the determinant; a
+ * near-singular block (|det| < 1e-12) returns 0 and leaves `out` untouched so
+ * the caller falls back to the per-axis solve.
+ *
+ * @param {Float64Array} K  [00,01,02,11,12,22] @param {Float64Array} out
+ * @returns {number} determinant
+ */
+function invert_sym3(K, out) {
+    const a = K[0], b = K[1], c = K[2], d = K[3], e = K[4], f = K[5];
+    const A00 = d * f - e * e, A01 = c * e - b * f, A02 = b * e - c * d;
+    const A11 = a * f - c * c, A12 = b * c - a * e, A22 = a * d - b * b;
+    const det = a * A00 + b * A01 + c * A02;
+    if (det < 1e-12 && det > -1e-12) return 0;
+    const inv = 1 / det;
+    out[0] = A00 * inv; out[1] = A01 * inv; out[2] = A02 * inv;
+    out[3] = A11 * inv; out[4] = A12 * inv; out[5] = A22 * inv;
+    return det;
+}
+
+/**
+ * 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,
+ * 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.
+ *
+ * @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
+ * @param {boolean} [reverse]  A2: traverse the joint array tip→root this call
+ *   (the caller alternates it per substep for a symmetric Gauss-Seidel sweep, so
+ *   chain impulses propagate both ways). Default false ⇒ original root→tip order.
+ */
+export function solve_joints(joints, system, dt_sub, iters, reverse) {
+    const n = joints.length;
+    if (n === 0 || dt_sub <= 0) return;
+
+    const spook_locked = JOINT_BAUMGARTE / dt_sub;
+    const spook_spec = 1 / dt_sub;
+
+    const storage = system.storage;
+    const ss = system.__solver_state.data;
+
+    for (let jj = 0; jj < n; jj++) {
+        const ji = reverse ? (n - 1 - jj) : jj;
+        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 = body_id_index(joint._bodyIdA);
+        const baseA = idxA * SBS_STRIDE;
+        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, baseB = -1;
+        if (!to_world) {
+            if (!storage.is_valid(joint._bodyIdB)) continue;
+            idxB = body_id_index(joint._bodyIdB);
+            baseB = idxB * SBS_STRIDE;
+            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 = ss[baseA + SBS_INV_MASS];
+        const invMB = to_world ? 0 : ss[baseB + SBS_INV_MASS];
+        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,
+            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 (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 imp = joint.dofImpulse;
+
+        // === Frame A world axes — shared by the linear AND angular rows ===
+        // 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 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). ---
+        let useLinBlock = false;
+        if (linConstrained) {
+            // A2: a fully-LOCKED linear triple is a ball-socket point — solve it
+            // as one coupled 3×3 block (frame axes) instead of axis-by-axis,
+            // removing the intra-joint coupling error the sequential per-axis
+            // Gauss-Seidel leaves behind. Falls back to per-axis if the block is
+            // near-singular.
+            if (mode[0] === DofMode.LOCKED && mode[1] === DofMode.LOCKED && mode[2] === DofMode.LOCKED) {
+                lin_K[0] = lin_K[3] = lin_K[5] = invMA + invMB;
+                lin_K[1] = lin_K[2] = lin_K[4] = 0;
+                accumulate_point_block(ss, baseA, rAx, rAy, rAz, fa, lin_K);
+                if (!to_world) accumulate_point_block(ss, baseB, rBx, rBy, rBz, fa, lin_K);
+                if (invert_sym3(lin_K, lin_Kinv) !== 0) {
+                    for (let k = 0; k < 3; k++) {
+                        const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
+                        lin_bias[k] = clamp_bias(spook_locked * (Cx * ax + Cy * ay + Cz * az));
+                        lin_active[k] = 1;
+                    }
+                    useLinBlock = true;
+                }
+            }
+            if (!useLinBlock) {
+                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(ss, baseA, invMA, rAx, rAy, rAz, ax, ay, az)
+                        + (to_world ? 0 : axis_effective_mass(ss, baseB, 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) {
+                        relative_anchor_velocity(scratch_rv, ss, baseA, rAx, rAy, rAz, baseB, rBx, rBy, rBz, to_world);
+                        vel = scratch_rv[0] * ax + scratch_rv[1] * ay + scratch_rv[2] * 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;
+        }
+
+        // --- 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;
+            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 → 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);
+            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];
+            }
+
+            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(ss, baseA, ax, ay, az)
+                    + (to_world ? 0 : angular_axis_effective_mass(ss, baseB, 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 = -(ss[baseA + SBS_AV_X] * ax + ss[baseA + SBS_AV_Y] * ay + ss[baseA + SBS_AV_Z] * az);
+                    if (!to_world) vel += ss[baseB + SBS_AV_X] * ax + ss[baseB + SBS_AV_Y] * ay + ss[baseB + SBS_AV_Z] * 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;
+        }
+
+        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). ---
+        {
+            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(ss, baseA, invMA, rAx, rAy, rAz, Px, Py, Pz, +1);
+                if (!to_world) apply_impulse(ss, baseB, invMB, rBx, rBy, rBz, Px, Py, Pz, -1);
+            }
+            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(ss, baseB, Lx, Ly, Lz, 1, +1);
+                apply_angular_impulse(ss, baseA, 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 (useLinBlock) {
+                // A2: coupled 3×3 point solve — one relative anchor velocity,
+                // projected onto the frame axes, solved against Kⁱⁿᵛ in a single
+                // Newton step (bilateral, no clamp). The point constraint is
+                // satisfied exactly each iteration; only its coupling with the
+                // angular rows remains to iterate.
+                relative_anchor_velocity(scratch_rv, ss, baseA, rAx, rAy, rAz, baseB, rBx, rBy, rBz, to_world);
+                const rvx = scratch_rv[0], rvy = scratch_rv[1], rvz = scratch_rv[2];
+                const a0x = fa[0], a0y = fa[1], a0z = fa[2];
+                const a1x = fa[3], a1y = fa[4], a1z = fa[5];
+                const a2x = fa[6], a2y = fa[7], a2z = fa[8];
+                const b0 = rvx * a0x + rvy * a0y + rvz * a0z + lin_bias[0];
+                const b1 = rvx * a1x + rvy * a1y + rvz * a1z + lin_bias[1];
+                const b2 = rvx * a2x + rvy * a2y + rvz * a2z + lin_bias[2];
+                const d0 = -(lin_Kinv[0] * b0 + lin_Kinv[1] * b1 + lin_Kinv[2] * b2);
+                const d1 = -(lin_Kinv[1] * b0 + lin_Kinv[3] * b1 + lin_Kinv[4] * b2);
+                const d2 = -(lin_Kinv[2] * b0 + lin_Kinv[4] * b1 + lin_Kinv[5] * b2);
+                imp[0] += d0; imp[1] += d1; imp[2] += d2;
+                const Px = d0 * a0x + d1 * a1x + d2 * a2x;
+                const Py = d0 * a0y + d1 * a1y + d2 * a2y;
+                const Pz = d0 * a0z + d1 * a1z + d2 * a2z;
+                if (Px !== 0 || Py !== 0 || Pz !== 0) {
+                    apply_impulse(ss, baseA, invMA, rAx, rAy, rAz, Px, Py, Pz, +1);
+                    if (!to_world) apply_impulse(ss, baseB, invMB, rBx, rBy, rBz, Px, Py, Pz, -1);
+                }
+            } else {
+                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];
+                    relative_anchor_velocity(scratch_rv, ss, baseA, rAx, rAy, rAz, baseB, rBx, rBy, rBz, to_world);
+                    const rvx = scratch_rv[0], rvy = scratch_rv[1], rvz = scratch_rv[2];
+                    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(ss, baseA, invMA, rAx, rAy, rAz, d * ax, d * ay, d * az, +1);
+                        if (!to_world) apply_impulse(ss, baseB, invMB, rBx, rBy, rBz, d * ax, d * ay, d * az, -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 = -(ss[baseA + SBS_AV_X] * ax + ss[baseA + SBS_AV_Y] * ay + ss[baseA + SBS_AV_Z] * az);
+                if (!to_world) wrel += ss[baseB + SBS_AV_X] * ax + ss[baseB + SBS_AV_Y] * ay + ss[baseB + SBS_AV_Z] * 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(ss, baseB, ax, ay, az, d, +1);
+                    apply_angular_impulse(ss, baseA, ax, ay, az, d, -1);
+                }
+            }
+        }
+    }
+}