@woosh/meep-engine 2.138.20 → 2.140.0

package/src/engine/physics/solver/solve_contacts.js

@@ -0,0 +1,1016 @@
+import { BodyKind } from "../ecs/BodyKind.js";
+import { ColliderFlags } from "../ecs/ColliderFlags.js";
+import { CONTACT_STRIDE } from "../contact/ManifoldStore.js";
+import { RigidBodyFlags } from "../ecs/RigidBodyFlags.js";
+import { world_inverse_inertia_apply } from "../inertia/world_inverse_inertia.js";
+import { v3_quat3_apply } from "../../../core/geom/vec3/v3_quat3_apply.js";
+import { v3_quat3_apply_inverse } from "../../../core/geom/vec3/v3_quat3_apply_inverse.js";
+import { friction_cone_clamp } from "./friction_cone.js";
+
+/**
+ * # TGS split-impulse contact solver (staged)
+ *
+ * Temporal Gauss-Seidel with Catto-2018 split impulse. The solver runs as a
+ * sequence of stages driven by `PhysicsSystem.fixedUpdate`, which owns the
+ * substep loop (it has to — velocity/position integration spans every awake
+ * body, not just contacts):
+ *
+ *   prepare_contacts(manifolds, system, h)         // once per outer step
+ *   for each substep:
+ *       (system integrates gravity by h)
+ *       refresh_contacts(manifolds, system)        // re-derive geometry
+ *       warm_start_contacts(manifolds, system)     // replay impulse — per substep!
+ *       solve_velocity(manifolds, system, iters)   // non-penetration + friction
+ *       solve_position(manifolds, system, pos_iters)
+ *       (system integrates position by h, folding pseudo-velocity)
+ *   apply_restitution(manifolds, system)           // once, after the loop
+ *
+ * The three concerns are fully decoupled (Phases 1–2) and substepping
+ * (Phase 3) re-runs only the velocity + position solve per substep —
+ * narrowphase runs once per outer step. Each substep re-derives the
+ * current penetration analytically from the bodies' moved poses and the
+ * contact anchors captured at prepare time, so the position correction
+ * adapts as the stack settles (the mechanism behind TGS stack stability)
+ * without paying for narrowphase N times.
+ *
+ * Why staged module functions sharing module scratch, rather than one call:
+ * the substep loop interleaves solver stages with whole-body integration
+ * that lives in the system. The stages communicate through module-scoped
+ * scratch + `g_*` state set by `prepare_contacts`; they must be called in
+ * order, prepare first, within a single outer step. Single-threaded and
+ * deterministic, so the shared state is safe.
+ *
+ * Contacts from all islands are flattened into one scratch array. Islands
+ * are independent (they share no bodies), so a single flat Gauss-Seidel
+ * sweep gives the same result as per-island sweeps — the island partition
+ * is still built and used by the sleep test, just not needed here.
+ */
+
+/**
+ * A pair is "sensor-only" when either body OR either body's primary
+ * collider carries the IsSensor flag. The manifold still exists (so
+ * Begin/Stay/End events fire from the manifold-diff pass) but no impulse
+ * is applied.
+ *
+ * @param {RigidBody} rbA
+ * @param {Collider} colA
+ * @param {RigidBody} rbB
+ * @param {Collider} colB
+ * @returns {boolean}
+ */
+function pair_is_sensor(rbA, colA, rbB, colB) {
+    return (rbA.flags & RigidBodyFlags.IsSensor) !== 0
+        || (rbB.flags & RigidBodyFlags.IsSensor) !== 0
+        || (colA.flags & ColliderFlags.IsSensor) !== 0
+        || (colB.flags & ColliderFlags.IsSensor) !== 0;
+}
+
+/**
+ * Velocity-iteration count per substep. With substepping the per-substep
+ * count can be lower than a single-step PGS solver would need, because the
+ * outer loop revisits the contact set `substeps` times.
+ * @type {number}
+ */
+const DEFAULT_VELOCITY_ITERATIONS = 10;
+
+/**
+ * Position-iteration count per substep (split-impulse pseudo-velocity pass).
+ * @type {number}
+ */
+const DEFAULT_POSITION_ITERATIONS = 2;
+
+/**
+ * Penetration allowed without applying position correction. Eliminates
+ * micro-jitter at near-zero overlap.
+ * @type {number}
+ */
+const PENETRATION_SLOP = 0.005;
+
+/**
+ * SPOOK contact stiffness `k`. Effectively infinite for rigid-body
+ * contact: what the solver actually sees is the regularization
+ * `eps = 4 / (h² · k · (1 + 4d))`, negligible at `k = 1e12` but in place
+ * as a continuous compliance dial for future soft contacts. Lacoursière
+ * 2007; same formulation as cannon-es / AgX.
+ * @type {number}
+ */
+const CONTACT_STIFFNESS = 1e12;
+
+/**
+ * SPOOK contact relaxation `d`. Chosen so `a = 4 / (h(1 + 4d)) = 0.2 / h`,
+ * numerically matching the prior Baumgarte β = 0.2 gain. `4/(1+4d)=0.2` →
+ * `d = 4.75`. Note `h` here is the SUBSTEP size: position correction is
+ * applied per substep, so the gain is derived from the substep `dt`.
+ * @type {number}
+ */
+const CONTACT_RELAXATION = 4.75;
+
+/**
+ * Maximum magnitude of the position-correction velocity bias, in m/s.
+ * Belt-and-braces against inflated EPA depths (PLAN.md caveat) driving the
+ * bias to tens of m/s. Removed once closed-form triangle solvers land.
+ * @type {number}
+ */
+const MAX_POSITION_BIAS = 3;
+
+/**
+ * Velocity below which restitution is suppressed (no micro-bounce buzz on
+ * resting stacks).
+ * @type {number}
+ */
+const RESTITUTION_VELOCITY_THRESHOLD = 1.0;
+
+/**
+ * Per-contact pre-step scratch stride: 23 doubles.
+ *   0..2 : localWA  (A's contact witness, in A's LOCAL frame — constant)
+ *   3..5 : localWB  (B's contact witness, in B's LOCAL frame — constant)
+ *   6..8 : rA       (lever from A's COM to the contact midpoint — refreshed)
+ *   9..11: rB       (lever from B's COM to the contact midpoint — refreshed)
+ *   12..14: t1      (tangent 1, unit, world — constant)
+ *   15..17: t2      (tangent 2, unit, world — constant)
+ *   18   : m_eff_n
+ *   19   : m_eff_t1
+ *   20   : m_eff_t2
+ *   21   : rest_bias       (restitution: `e · vn_approach`, ≤ 0; 0 if not bouncing)
+ *   22   : bias_position   (depth-correction bias — refreshed each substep)
+ *
+ * `localWA` / `localWB` are the per-body contact witnesses expressed in body
+ * local frames at prepare time. Each substep, `refresh_contacts` rotates
+ * them back to world by the body's current pose to recover the moved contact
+ * points, re-derives the current penetration depth (anchored on the trusted
+ * prepare-time depth via a delta, so it's sign-robust), and rebuilds the
+ * impulse lever arms `rA` / `rB` and the position bias.
+ * @type {number}
+ */
+const PRE_STRIDE = 23;
+
+/** Per-contact index list — 4 uint32 per contact: slot, idx, idxA, idxB. */
+const INDEX_STRIDE = 4;
+
+/**
+ * Per-body pseudo-velocity stride: 3 linear + 3 angular doubles. Owned by
+ * PhysicsSystem (`system.__pseudo_velocity`); zeroed each substep before
+ * the position pass and folded into the pose by `integrate_position`.
+ * @type {number}
+ */
+const PSEUDO_STRIDE = 6;
+
+let scratch_pre = new Float64Array(64 * PRE_STRIDE);
+let scratch_idx = new Uint32Array(64 * INDEX_STRIDE);
+let scratch_mu = new Float64Array(64);
+let scratch_pos_jn = new Float64Array(64);
+
+/**
+ * Per-contact maximum normal impulse seen across the whole outer step's
+ * velocity solving (all substeps, all iterations). Reset in
+ * {@link prepare_contacts}, updated in {@link solve_velocity}, read by
+ * {@link apply_restitution}.
+ *
+ * Restitution must fire whenever a contact *was* compressive at some point
+ * in the step, even if its accumulated impulse later relaxes back to ~0 — a
+ * transient collision (ball bouncing, head-on hit) under per-substep
+ * warm-start ends the loop with `j_n ≈ 0` because there's no sustained load
+ * to hold the impulse up. Gating on this running max (Box2D-v3
+ * `maxNormalImpulse`) rather than the end-of-loop `j_n` is what makes
+ * bounces fire.
+ * @type {Float64Array}
+ */
+let scratch_max_jn = new Float64Array(64);
+
+/**
+ * Shared cross-stage state, set by {@link prepare_contacts} and read by the
+ * per-substep stages within the same outer step. Single-threaded, so plain
+ * module variables are safe.
+ */
+let g_contact_count = 0;
+let g_spook_a = 0;
+let g_spook_eps = 0;
+
+/** Scratch for re-deriving contact world points in {@link refresh_contacts}. */
+const scratch_cp = new Float64Array(6);
+
+function ensure_capacity(n) {
+    if (scratch_pre.length < n * PRE_STRIDE) {
+        scratch_pre = new Float64Array(n * PRE_STRIDE * 2);
+    }
+    if (scratch_idx.length < n * INDEX_STRIDE) {
+        scratch_idx = new Uint32Array(n * INDEX_STRIDE * 2);
+    }
+    if (scratch_mu.length < n) {
+        scratch_mu = new Float64Array(n * 2);
+    }
+    if (scratch_pos_jn.length < n) {
+        scratch_pos_jn = new Float64Array(n * 2);
+    }
+    if (scratch_max_jn.length < n) {
+        scratch_max_jn = new Float64Array(n * 2);
+    }
+}
+
+/**
+ * Build an orthonormal tangent basis perpendicular to a unit normal.
+ *
+ * @param {Float64Array} out  6 floats: t1.xyz then t2.xyz
+ * @param {number} off
+ * @param {number} nx
+ * @param {number} ny
+ * @param {number} nz
+ */
+function build_tangents(out, off, nx, ny, nz) {
+    const ax = nx < 0 ? -nx : nx;
+    const ay = ny < 0 ? -ny : ny;
+    const az = nz < 0 ? -nz : nz;
+
+    let rx, ry, rz;
+    if (ax <= ay && ax <= az) {
+        rx = 1; ry = 0; rz = 0;
+    } else if (ay <= az) {
+        rx = 0; ry = 1; rz = 0;
+    } else {
+        rx = 0; ry = 0; rz = 1;
+    }
+    let t1x = ny * rz - nz * ry;
+    let t1y = nz * rx - nx * rz;
+    let t1z = nx * ry - ny * rx;
+    const inv = 1 / Math.sqrt(t1x * t1x + t1y * t1y + t1z * t1z);
+    t1x *= inv; t1y *= inv; t1z *= inv;
+
+    const t2x = ny * t1z - nz * t1y;
+    const t2y = nz * t1x - nx * t1z;
+    const t2z = nx * t1y - ny * t1x;
+
+    out[off]     = t1x; out[off + 1] = t1y; out[off + 2] = t1z;
+    out[off + 3] = t2x; out[off + 4] = t2y; out[off + 5] = t2z;
+}
+
+/**
+ * @param {RigidBody} rb
+ * @returns {number}
+ */
+function inv_mass_of(rb) {
+    if (rb.kind !== BodyKind.Dynamic) return 0;
+    return rb.mass > 0 ? 1 / rb.mass : 0;
+}
+
+/**
+ * Combine two friction coefficients (geometric mean — Bullet / PhysX default).
+ * @param {number} a
+ * @param {number} b
+ * @returns {number}
+ */
+function combine_friction(a, b) {
+    return Math.sqrt(a * b);
+}
+
+/**
+ * Combine two restitution coefficients (maximum — Unity / common default).
+ * @param {number} a
+ * @param {number} b
+ * @returns {number}
+ */
+function combine_restitution(a, b) {
+    return a > b ? a : b;
+}
+
+const scratch_clamp = new Float64Array(2);
+const scratch_inertia_a = new Float64Array(3);
+const scratch_inertia_b = new Float64Array(3);
+
+/**
+ * Apply an impulse `P` at body-relative offset `r` to persistent velocity
+ * (Δv = P/m, Δω = I⁻¹·(r × P)). Direct typed-array writes bypass
+ * {@link Vector3#set}'s observer dispatch (dead weight in the solver loop).
+ *
+ * @param {RigidBody} rb
+ * @param {Transform} transform
+ * @param {number} invM
+ * @param {number} rx @param {number} ry @param {number} rz
+ * @param {number} Px @param {number} Py @param {number} Pz
+ * @param {number} sign +1 for body A, -1 for body B
+ * @param {Float64Array} scratch_inertia
+ */
+function apply_impulse_to_body(
+    rb, transform, invM,
+    rx, ry, rz,
+    Px, Py, Pz,
+    sign,
+    scratch_inertia
+) {
+    if (invM === 0) return;
+
+    const sPx = sign * Px;
+    const sPy = sign * Py;
+    const sPz = sign * Pz;
+
+    const lv = rb.linearVelocity;
+    lv[0] += sPx * invM;
+    lv[1] += sPy * invM;
+    lv[2] += sPz * invM;
+
+    const tx = ry * sPz - rz * sPy;
+    const ty = rz * sPx - rx * sPz;
+    const tz = rx * sPy - ry * sPx;
+
+    world_inverse_inertia_apply(scratch_inertia, 0, rb.inverseInertiaLocal, transform.rotation, tx, ty, tz);
+
+    const av = rb.angularVelocity;
+    av[0] += scratch_inertia[0];
+    av[1] += scratch_inertia[1];
+    av[2] += scratch_inertia[2];
+}
+
+/**
+ * Apply a position-pass impulse `P` at body-relative offset `r` to the
+ * body's pseudo-velocity (linear + angular) in the `pseudo_velocity` flat
+ * buffer. Mirrors {@link apply_impulse_to_body} but the result is consumed
+ * by `integrate_position` the same substep and never persists.
+ *
+ * @param {Float64Array} pseudo_velocity stride = {@link PSEUDO_STRIDE}
+ * @param {number} base offset = `body_index * PSEUDO_STRIDE`
+ * @param {RigidBody} rb @param {Transform} transform @param {number} invM
+ * @param {number} rx @param {number} ry @param {number} rz
+ * @param {number} Px @param {number} Py @param {number} Pz
+ * @param {number} sign +1 for body A, -1 for body B
+ * @param {Float64Array} scratch_inertia
+ */
+function apply_impulse_to_pseudo(
+    pseudo_velocity, base,
+    rb, transform, invM,
+    rx, ry, rz,
+    Px, Py, Pz,
+    sign,
+    scratch_inertia
+) {
+    if (invM === 0) return;
+
+    const sPx = sign * Px;
+    const sPy = sign * Py;
+    const sPz = sign * Pz;
+
+    pseudo_velocity[base]     += sPx * invM;
+    pseudo_velocity[base + 1] += sPy * invM;
+    pseudo_velocity[base + 2] += sPz * invM;
+
+    const tx = ry * sPz - rz * sPy;
+    const ty = rz * sPx - rx * sPz;
+    const tz = rx * sPy - ry * sPx;
+
+    world_inverse_inertia_apply(scratch_inertia, 0, rb.inverseInertiaLocal, transform.rotation, tx, ty, tz);
+
+    pseudo_velocity[base + 3] += scratch_inertia[0];
+    pseudo_velocity[base + 4] += scratch_inertia[1];
+    pseudo_velocity[base + 5] += scratch_inertia[2];
+}
+
+/**
+ * Quadratic-form contribution of one body to the constraint effective mass
+ * along a unit axis: `(r × axis)^T · I⁻¹_world · (r × axis)`.
+ *
+ * @param {RigidBody} rb @param {Transform} transform
+ * @param {number} rx @param {number} ry @param {number} rz
+ * @param {number} ax @param {number} ay @param {number} az
+ * @param {Float64Array} scratch_inertia
+ * @returns {number}
+ */
+function angular_jacobian_contribution(
+    rb, transform,
+    rx, ry, rz,
+    ax, ay, az,
+    scratch_inertia
+) {
+    if (rb.kind !== BodyKind.Dynamic) return 0;
+    const ii = rb.inverseInertiaLocal;
+    if (ii.x === 0 && ii.y === 0 && ii.z === 0) return 0;
+
+    const rxax = ry * az - rz * ay;
+    const rxay = rz * ax - rx * az;
+    const rxaz = rx * ay - ry * ax;
+
+    world_inverse_inertia_apply(scratch_inertia, 0, ii, transform.rotation, rxax, rxay, rxaz);
+
+    return rxax * scratch_inertia[0]
+         + rxay * scratch_inertia[1]
+         + rxaz * scratch_inertia[2];
+}
+
+/**
+ * Flat count of all touched contacts across every island, plus the slot
+ * list to iterate. Islands are concatenated densely in `contact_data`;
+ * `contact_offsets[island_count]` is the end of the last island.
+ *
+ * @param {PhysicsSystem} system
+ * @returns {{slot_list: Uint32Array, total_slots: number}|null}
+ */
+function island_slot_range(system) {
+    const islands = system.islands;
+    if (islands === undefined || islands === null) return null;
+    const island_count = islands.island_count;
+    if (island_count === 0) return null;
+    return {
+        slot_list: islands.contact_data,
+        total_slots: islands.contact_offsets[island_count],
+    };
+}
+
+/**
+ * Stage 1 — prepare the contact constraints for an outer step.
+ *
+ * Packs every touched, non-sensor contact into the flat scratch arrays:
+ * local-frame witness anchors, tangent basis, effective masses, friction,
+ * the restitution approach velocity, and the warm-start replay (applied
+ * once here, not per substep). Computes the SPOOK gains from the SUBSTEP
+ * size `dt_sub` because the position correction runs once per substep.
+ *
+ * @param {ManifoldStore} manifolds
+ * @param {PhysicsSystem} system
+ * @param {number} dt_sub substep size `dt / substeps`
+ * @returns {number} number of contacts prepared (also stored module-side)
+ */
+export function prepare_contacts(manifolds, system, dt_sub) {
+    g_contact_count = 0;
+    if (dt_sub <= 0) return 0;
+
+    const range = island_slot_range(system);
+    if (range === null) return 0;
+    const slot_list = range.slot_list;
+    const total_slots = range.total_slots;
+
+    let contact_total = 0;
+    for (let i = 0; i < total_slots; i++) {
+        contact_total += manifolds.contact_count(slot_list[i]);
+    }
+    if (contact_total === 0) return 0;
+    ensure_capacity(contact_total);
+
+    const denom = 1 + 4 * CONTACT_RELAXATION;
+    g_spook_a = 4 / (dt_sub * denom);
+    g_spook_eps = 4 / (dt_sub * dt_sub * CONTACT_STIFFNESS * denom);
+
+    const data = manifolds.data_buffer;
+    const pre = scratch_pre;
+    const idx = scratch_idx;
+    const mus = scratch_mu;
+    const pos_jn = scratch_pos_jn;
+
+    let c = 0;
+    for (let i = 0; i < total_slots; i++) {
+        const slot = slot_list[i];
+        const idxA = system.__index_of(manifolds.bodyA(slot));
+        const idxB = system.__index_of(manifolds.bodyB(slot));
+        const rbA = system.__bodies[idxA];
+        const rbB = system.__bodies[idxB];
+        const trA = system.__transforms[idxA];
+        const trB = system.__transforms[idxB];
+        const colA = system.__primary_collider(idxA);
+        const colB = system.__primary_collider(idxB);
+        if (colA === null || colB === null) continue;
+        if (pair_is_sensor(rbA, colA, rbB, colB)) continue;
+
+        const invMA = inv_mass_of(rbA);
+        const invMB = inv_mass_of(rbB);
+
+        const cc = manifolds.contact_count(slot);
+        const slot_off = manifolds.slot_data_offset(slot);
+        const restitution_combined = combine_restitution(colA.restitution, colB.restitution);
+        const friction_combined = combine_friction(colA.friction, colB.friction);
+
+        const pAx = trA.position.x, pAy = trA.position.y, pAz = trA.position.z;
+        const pBx = trB.position.x, pBy = trB.position.y, pBz = trB.position.z;
+        const qA = trA.rotation, qB = trB.rotation;
+
+        for (let k = 0; k < cc; k++) {
+            const off = slot_off + k * CONTACT_STRIDE;
+
+            const wax = data[off],     way = data[off + 1], waz = data[off + 2];
+            const wbx = data[off + 3], wby = data[off + 4], wbz = data[off + 5];
+            const nx  = data[off + 6], ny  = data[off + 7], nz  = data[off + 8];
+
+            const px = (wax + wbx) * 0.5;
+            const py = (way + wby) * 0.5;
+            const pz = (waz + wbz) * 0.5;
+
+            const rax = px - pAx, ray = py - pAy, raz = pz - pAz;
+            const rbx = px - pBx, rby = py - pBy, rbz = pz - pBz;
+
+            const pre_off = c * PRE_STRIDE;
+
+            // Per-body witness anchors in LOCAL frame (constant across the
+            // outer step). localW = R⁻¹ · (witness − COM).
+            v3_quat3_apply_inverse(pre, pre_off,     wax - pAx, way - pAy, waz - pAz, qA[0], qA[1], qA[2], qA[3]);
+            v3_quat3_apply_inverse(pre, pre_off + 3, wbx - pBx, wby - pBy, wbz - pBz, qB[0], qB[1], qB[2], qB[3]);
+
+            // Lever arms from COM to the contact midpoint (refreshed each
+            // substep; seeded here from prepare-time pose).
+            pre[pre_off + 6] = rax; pre[pre_off + 7] = ray; pre[pre_off + 8] = raz;
+            pre[pre_off + 9] = rbx; pre[pre_off + 10] = rby; pre[pre_off + 11] = rbz;
+
+            build_tangents(pre, pre_off + 12, nx, ny, nz);
+            const t1x = pre[pre_off + 12], t1y = pre[pre_off + 13], t1z = pre[pre_off + 14];
+            const t2x = pre[pre_off + 15], t2y = pre[pre_off + 16], t2z = pre[pre_off + 17];
+
+            const k_n = invMA + invMB
+                + angular_jacobian_contribution(rbA, trA, rax, ray, raz, nx, ny, nz, scratch_inertia_a)
+                + angular_jacobian_contribution(rbB, trB, rbx, rby, rbz, nx, ny, nz, scratch_inertia_b);
+            const k_t1 = invMA + invMB
+                + angular_jacobian_contribution(rbA, trA, rax, ray, raz, t1x, t1y, t1z, scratch_inertia_a)
+                + angular_jacobian_contribution(rbB, trB, rbx, rby, rbz, t1x, t1y, t1z, scratch_inertia_b);
+            const k_t2 = invMA + invMB
+                + angular_jacobian_contribution(rbA, trA, rax, ray, raz, t2x, t2y, t2z, scratch_inertia_a)
+                + angular_jacobian_contribution(rbB, trB, rbx, rby, rbz, t2x, t2y, t2z, scratch_inertia_b);
+
+            const k_n_eff = k_n + g_spook_eps;
+            pre[pre_off + 18] = k_n_eff > 0 ? 1 / k_n_eff : 0;
+            pre[pre_off + 19] = k_t1 > 0 ? 1 / k_t1 : 0;
+            pre[pre_off + 20] = k_t2 > 0 ? 1 / k_t2 : 0;
+
+            // Restitution approach velocity (captured once, this is the
+            // closing speed entering the step). n is B → A; vn < 0 closing.
+            const lvA = rbA.linearVelocity, avA = rbA.angularVelocity;
+            const lvB = rbB.linearVelocity, avB = rbB.angularVelocity;
+            const vAx_at = lvA[0] + avA[1] * raz - avA[2] * ray;
+            const vAy_at = lvA[1] + avA[2] * rax - avA[0] * raz;
+            const vAz_at = lvA[2] + avA[0] * ray - avA[1] * rax;
+            const vBx_at = lvB[0] + avB[1] * rbz - avB[2] * rby;
+            const vBy_at = lvB[1] + avB[2] * rbx - avB[0] * rbz;
+            const vBz_at = lvB[2] + avB[0] * rby - avB[1] * rbx;
+            const vn_pre = (vAx_at - vBx_at) * nx + (vAy_at - vBy_at) * ny + (vAz_at - vBz_at) * nz;
+
+            let rest_bias = 0;
+            if (vn_pre < -RESTITUTION_VELOCITY_THRESHOLD) {
+                rest_bias = restitution_combined * vn_pre;
+            }
+            pre[pre_off + 21] = rest_bias;
+            pre[pre_off + 22] = 0; // bias_position — filled by refresh_contacts
+
+            mus[c] = friction_combined;
+            pos_jn[c] = 0;
+            scratch_max_jn[c] = 0;
+
+            idx[c * INDEX_STRIDE]     = slot;
+            idx[c * INDEX_STRIDE + 1] = k;
+            idx[c * INDEX_STRIDE + 2] = idxA;
+            idx[c * INDEX_STRIDE + 3] = idxB;
+
+            // Warm-start is NOT applied here: under TGS it must be replayed
+            // *every substep* (see warm_start_contacts) so that, per substep,
+            // the cached impulse balances exactly one substep of gravity.
+            // Applying the cached impulse once against a full frame of
+            // gravity (the non-substepped pattern) over-pushes resting
+            // contacts and jitters / explodes deep stacks.
+
+            c++;
+        }
+    }
+
+    g_contact_count = c;
+    return c;
+}
+
+/**
+ * Stage 1b (per substep) — warm-start: replay the cached accumulated
+ * impulses `(j_n, j_t1, j_t2)` onto persistent velocity using the current
+ * (refreshed) lever arms and tangents. Run once per substep, after
+ * {@link refresh_contacts} and before {@link solve_velocity}.
+ *
+ * Per-substep warm-start is the crux of stable TGS: the stored impulse is a
+ * per-substep quantity (≈ the impulse to counter one substep of gravity), so
+ * replaying it each substep balances that substep's `integrate_velocity_gravity`
+ * and a resting contact holds at zero velocity. `solve_velocity` then only
+ * has to correct the residual, which converges in a few iterations even for
+ * deep chains because each substep carries just `h` of gravity.
+ *
+ * @param {ManifoldStore} manifolds
+ * @param {PhysicsSystem} system
+ */
+export function warm_start_contacts(manifolds, system) {
+    const count = g_contact_count;
+    if (count === 0) return;
+
+    const data = manifolds.data_buffer;
+    const pre = scratch_pre;
+    const idx = scratch_idx;
+
+    for (let ci = 0; ci < count; ci++) {
+        const slot = idx[ci * INDEX_STRIDE];
+        const cidx = idx[ci * INDEX_STRIDE + 1];
+        const idxA = idx[ci * INDEX_STRIDE + 2];
+        const idxB = idx[ci * INDEX_STRIDE + 3];
+        const rbA = system.__bodies[idxA];
+        const rbB = system.__bodies[idxB];
+        const trA = system.__transforms[idxA];
+        const trB = system.__transforms[idxB];
+        const invMA = inv_mass_of(rbA);
+        const invMB = inv_mass_of(rbB);
+
+        const pre_off = ci * PRE_STRIDE;
+        const rax = pre[pre_off + 6], ray = pre[pre_off + 7], raz = pre[pre_off + 8];
+        const rbx = pre[pre_off + 9], rby = pre[pre_off + 10], rbz = pre[pre_off + 11];
+        const t1x = pre[pre_off + 12], t1y = pre[pre_off + 13], t1z = pre[pre_off + 14];
+        const t2x = pre[pre_off + 15], t2y = pre[pre_off + 16], t2z = pre[pre_off + 17];
+
+        const slot_off = manifolds.slot_data_offset(slot);
+        const off = slot_off + cidx * CONTACT_STRIDE;
+        const nx = data[off + 6], ny = data[off + 7], nz = data[off + 8];
+
+        const j_n  = data[off + 10];
+        const j_t1 = data[off + 11];
+        const j_t2 = data[off + 12];
+        const Px = nx * j_n + t1x * j_t1 + t2x * j_t2;
+        const Py = ny * j_n + t1y * j_t1 + t2y * j_t2;
+        const Pz = nz * j_n + t1z * j_t1 + t2z * j_t2;
+        apply_impulse_to_body(rbA, trA, invMA, rax, ray, raz, Px, Py, Pz, +1, scratch_inertia_a);
+        apply_impulse_to_body(rbB, trB, invMB, rbx, rby, rbz, Px, Py, Pz, -1, scratch_inertia_b);
+    }
+}
+
+/**
+ * Stage 2 (per substep) — re-derive each contact's geometry from the
+ * bodies' current poses and the local witness anchors captured at prepare.
+ *
+ * For each contact:
+ *  - rotate the stored local witnesses back to world by the current pose to
+ *    get the moved contact points `cpA`, `cpB`;
+ *  - current penetration `depth_now = depth0 − Δseparation`, where
+ *    `Δseparation = (cpA − wA − (cpB − wB)) · n` is the change since prepare.
+ *    Anchoring on the trusted prepare-time depth makes the sign convention
+ *    irrelevant — only the analytic delta uses the anchors;
+ *  - rebuild the impulse levers `rA`/`rB` from the moved midpoint;
+ *  - recompute the position-correction bias from `depth_now`.
+ *
+ * The contact normal and tangents are held fixed for the outer step (valid
+ * for the small per-step rotation), so they are not recomputed here.
+ *
+ * @param {ManifoldStore} manifolds
+ * @param {PhysicsSystem} system
+ */
+export function refresh_contacts(manifolds, system) {
+    const count = g_contact_count;
+    if (count === 0) return;
+
+    const data = manifolds.data_buffer;
+    const pre = scratch_pre;
+    const idx = scratch_idx;
+    const cp = scratch_cp;
+    const spook_a = g_spook_a;
+
+    for (let ci = 0; ci < count; ci++) {
+        const slot = idx[ci * INDEX_STRIDE];
+        const cidx = idx[ci * INDEX_STRIDE + 1];
+        const idxA = idx[ci * INDEX_STRIDE + 2];
+        const idxB = idx[ci * INDEX_STRIDE + 3];
+        const trA = system.__transforms[idxA];
+        const trB = system.__transforms[idxB];
+
+        const slot_off = manifolds.slot_data_offset(slot);
+        const off = slot_off + cidx * CONTACT_STRIDE;
+
+        const wax = data[off],     way = data[off + 1], waz = data[off + 2];
+        const wbx = data[off + 3], wby = data[off + 4], wbz = data[off + 5];
+        const nx  = data[off + 6], ny  = data[off + 7], nz  = data[off + 8];
+        const depth0 = data[off + 9];
+
+        const pre_off = ci * PRE_STRIDE;
+
+        const pAx = trA.position.x, pAy = trA.position.y, pAz = trA.position.z;
+        const pBx = trB.position.x, pBy = trB.position.y, pBz = trB.position.z;
+        const qA = trA.rotation, qB = trB.rotation;
+
+        // Current world contact points: COM + R · localWitness.
+        v3_quat3_apply(cp, 0, pre[pre_off], pre[pre_off + 1], pre[pre_off + 2], qA[0], qA[1], qA[2], qA[3]);
+        v3_quat3_apply(cp, 3, pre[pre_off + 3], pre[pre_off + 4], pre[pre_off + 5], qB[0], qB[1], qB[2], qB[3]);
+        const cpAx = pAx + cp[0], cpAy = pAy + cp[1], cpAz = pAz + cp[2];
+        const cpBx = pBx + cp[3], cpBy = pBy + cp[4], cpBz = pBz + cp[5];
+
+        // Penetration re-derived as a delta from the trusted prepare depth.
+        // Δsep = ((cpA − wA) − (cpB − wB)) · n.
+        const dsep = ((cpAx - wax) - (cpBx - wbx)) * nx
+                   + ((cpAy - way) - (cpBy - wby)) * ny
+                   + ((cpAz - waz) - (cpBz - wbz)) * nz;
+        const depth_now = depth0 - dsep;
+
+        // Impulse levers from each COM to the current contact midpoint.
+        const mx = (cpAx + cpBx) * 0.5;
+        const my = (cpAy + cpBy) * 0.5;
+        const mz = (cpAz + cpBz) * 0.5;
+        pre[pre_off + 6] = mx - pAx; pre[pre_off + 7] = my - pAy; pre[pre_off + 8] = mz - pAz;
+        pre[pre_off + 9] = mx - pBx; pre[pre_off + 10] = my - pBy; pre[pre_off + 11] = mz - pBz;
+
+        let bias_position = 0;
+        if (depth_now > PENETRATION_SLOP) {
+            bias_position = -spook_a * (depth_now - PENETRATION_SLOP);
+            if (bias_position < -MAX_POSITION_BIAS) bias_position = -MAX_POSITION_BIAS;
+        }
+        pre[pre_off + 22] = bias_position;
+    }
+}
+
+/**
+ * Stage 3 (per substep) — velocity iterations enforcing pure
+ * non-penetration (`vn → 0`) plus Coulomb-disk friction. No bias: depth
+ * correction is the position pass, restitution is the one-shot pass.
+ *
+ * @param {ManifoldStore} manifolds
+ * @param {PhysicsSystem} system
+ * @param {number} iters
+ */
+export function solve_velocity(manifolds, system, iters) {
+    const contact_count = g_contact_count;
+    if (contact_count === 0) return;
+
+    const data = manifolds.data_buffer;
+    const pre = scratch_pre;
+    const idx = scratch_idx;
+    const mus = scratch_mu;
+
+    for (let iter = 0; iter < iters; iter++) {
+        for (let ci = 0; ci < contact_count; ci++) {
+            const slot = idx[ci * INDEX_STRIDE];
+            const cidx = idx[ci * INDEX_STRIDE + 1];
+            const idxA = idx[ci * INDEX_STRIDE + 2];
+            const idxB = idx[ci * INDEX_STRIDE + 3];
+            const rbA = system.__bodies[idxA];
+            const rbB = system.__bodies[idxB];
+            const trA = system.__transforms[idxA];
+            const trB = system.__transforms[idxB];
+            const invMA = inv_mass_of(rbA);
+            const invMB = inv_mass_of(rbB);
+
+            const pre_off = ci * PRE_STRIDE;
+            const rax = pre[pre_off + 6], ray = pre[pre_off + 7], raz = pre[pre_off + 8];
+            const rbx = pre[pre_off + 9], rby = pre[pre_off + 10], rbz = pre[pre_off + 11];
+            const t1x = pre[pre_off + 12], t1y = pre[pre_off + 13], t1z = pre[pre_off + 14];
+            const t2x = pre[pre_off + 15], t2y = pre[pre_off + 16], t2z = pre[pre_off + 17];
+            const m_eff_n  = pre[pre_off + 18];
+            const m_eff_t1 = pre[pre_off + 19];
+            const m_eff_t2 = pre[pre_off + 20];
+
+            const slot_off = manifolds.slot_data_offset(slot);
+            const off = slot_off + cidx * CONTACT_STRIDE;
+            const nx = data[off + 6], ny = data[off + 7], nz = data[off + 8];
+
+            const lvA = rbA.linearVelocity, avA = rbA.angularVelocity;
+            const lvB = rbB.linearVelocity, avB = rbB.angularVelocity;
+
+            const vAx_at = lvA[0] + avA[1] * raz - avA[2] * ray;
+            const vAy_at = lvA[1] + avA[2] * rax - avA[0] * raz;
+            const vAz_at = lvA[2] + avA[0] * ray - avA[1] * rax;
+            const vBx_at = lvB[0] + avB[1] * rbz - avB[2] * rby;
+            const vBy_at = lvB[1] + avB[2] * rbx - avB[0] * rbz;
+            const vBz_at = lvB[2] + avB[0] * rby - avB[1] * rbx;
+            const dvx = vAx_at - vBx_at;
+            const dvy = vAy_at - vBy_at;
+            const dvz = vAz_at - vBz_at;
+
+            // --- Normal impulse (non-penetration: drive vn → 0) ---
+            const vn = dvx * nx + dvy * ny + dvz * nz;
+            const j_n_accum = data[off + 10];
+            const lambda_n = -m_eff_n * vn;
+            const sum_n = j_n_accum + lambda_n;
+            const new_j_n = sum_n > 0 ? sum_n : 0;
+            const delta_j_n = new_j_n - j_n_accum;
+            data[off + 10] = new_j_n;
+            if (new_j_n > scratch_max_jn[ci]) scratch_max_jn[ci] = new_j_n;
+
+            if (delta_j_n !== 0) {
+                const Pnx = nx * delta_j_n;
+                const Pny = ny * delta_j_n;
+                const Pnz = nz * delta_j_n;
+                apply_impulse_to_body(rbA, trA, invMA, rax, ray, raz, Pnx, Pny, Pnz, +1, scratch_inertia_a);
+                apply_impulse_to_body(rbB, trB, invMB, rbx, rby, rbz, Pnx, Pny, Pnz, -1, scratch_inertia_b);
+            }
+
+            // --- Friction impulse (Coulomb disk in tangent plane) ---
+            const vAx2 = lvA[0] + avA[1] * raz - avA[2] * ray;
+            const vAy2 = lvA[1] + avA[2] * rax - avA[0] * raz;
+            const vAz2 = lvA[2] + avA[0] * ray - avA[1] * rax;
+            const vBx2 = lvB[0] + avB[1] * rbz - avB[2] * rby;
+            const vBy2 = lvB[1] + avB[2] * rbx - avB[0] * rbz;
+            const vBz2 = lvB[2] + avB[0] * rby - avB[1] * rbx;
+            const dvx2 = vAx2 - vBx2;
+            const dvy2 = vAy2 - vBy2;
+            const dvz2 = vAz2 - vBz2;
+
+            const vt1 = dvx2 * t1x + dvy2 * t1y + dvz2 * t1z;
+            const vt2 = dvx2 * t2x + dvy2 * t2y + dvz2 * t2z;
+
+            const j_t1_accum = data[off + 11];
+            const j_t2_accum = data[off + 12];
+            const lambda_t1 = -m_eff_t1 * vt1;
+            const lambda_t2 = -m_eff_t2 * vt2;
+
+            const want_t1 = j_t1_accum + lambda_t1;
+            const want_t2 = j_t2_accum + lambda_t2;
+            const max_friction = mus[ci] * new_j_n;
+            friction_cone_clamp(scratch_clamp, 0, want_t1, want_t2, max_friction);
+            const new_j_t1 = scratch_clamp[0];
+            const new_j_t2 = scratch_clamp[1];
+            const delta_t1 = new_j_t1 - j_t1_accum;
+            const delta_t2 = new_j_t2 - j_t2_accum;
+            data[off + 11] = new_j_t1;
+            data[off + 12] = new_j_t2;
+
+            if (delta_t1 !== 0 || delta_t2 !== 0) {
+                const Ptx = t1x * delta_t1 + t2x * delta_t2;
+                const Pty = t1y * delta_t1 + t2y * delta_t2;
+                const Ptz = t1z * delta_t1 + t2z * delta_t2;
+                apply_impulse_to_body(rbA, trA, invMA, rax, ray, raz, Ptx, Pty, Ptz, +1, scratch_inertia_a);
+                apply_impulse_to_body(rbB, trB, invMB, rbx, rby, rbz, Ptx, Pty, Ptz, -1, scratch_inertia_b);
+            }
+        }
+    }
+}
+
+/**
+ * Stage 4 (once, after the substep loop) — one-shot restitution
+ * (Box2D-v3 `b2ApplyRestitution`, Catto 2018). Drives `vn → -e · vn_approach`
+ * exactly once per closing contact, gated on (a) the contact having been
+ * closing faster than the threshold at prepare, and (b) a compressive
+ * normal impulse having formed during the velocity solve. The added impulse
+ * accumulates into the same normal-impulse slot so it composes with
+ * warm-start next frame.
+ *
+ * @param {ManifoldStore} manifolds
+ * @param {PhysicsSystem} system
+ */
+export function apply_restitution(manifolds, system) {
+    const contact_count = g_contact_count;
+    if (contact_count === 0) return;
+
+    const data = manifolds.data_buffer;
+    const pre = scratch_pre;
+    const idx = scratch_idx;
+
+    for (let ci = 0; ci < contact_count; ci++) {
+        const pre_off = ci * PRE_STRIDE;
+        const rest_bias = pre[pre_off + 21];
+        if (rest_bias === 0) continue;
+
+        const slot = idx[ci * INDEX_STRIDE];
+        const cidx = idx[ci * INDEX_STRIDE + 1];
+        // Gate on the running max normal impulse, not the end-of-loop value:
+        // a transient collision relaxes back to j_n ≈ 0 under per-substep
+        // warm-start, but it WAS compressive, so it should still bounce.
+        if (scratch_max_jn[ci] <= 0) continue;
+
+        const slot_off = manifolds.slot_data_offset(slot);
+        const off = slot_off + cidx * CONTACT_STRIDE;
+
+        const j_n_accum = data[off + 10];
+
+        const idxA = idx[ci * INDEX_STRIDE + 2];
+        const idxB = idx[ci * INDEX_STRIDE + 3];
+        const rbA = system.__bodies[idxA];
+        const rbB = system.__bodies[idxB];
+        const trA = system.__transforms[idxA];
+        const trB = system.__transforms[idxB];
+        const invMA = inv_mass_of(rbA);
+        const invMB = inv_mass_of(rbB);
+
+        const rax = pre[pre_off + 6], ray = pre[pre_off + 7], raz = pre[pre_off + 8];
+        const rbx = pre[pre_off + 9], rby = pre[pre_off + 10], rbz = pre[pre_off + 11];
+        const m_eff_n = pre[pre_off + 18];
+        const nx = data[off + 6], ny = data[off + 7], nz = data[off + 8];
+
+        const lvA = rbA.linearVelocity, avA = rbA.angularVelocity;
+        const lvB = rbB.linearVelocity, avB = rbB.angularVelocity;
+        const vAx_at = lvA[0] + avA[1] * raz - avA[2] * ray;
+        const vAy_at = lvA[1] + avA[2] * rax - avA[0] * raz;
+        const vAz_at = lvA[2] + avA[0] * ray - avA[1] * rax;
+        const vBx_at = lvB[0] + avB[1] * rbz - avB[2] * rby;
+        const vBy_at = lvB[1] + avB[2] * rbx - avB[0] * rbz;
+        const vBz_at = lvB[2] + avB[0] * rby - avB[1] * rbx;
+        const vn = (vAx_at - vBx_at) * nx + (vAy_at - vBy_at) * ny + (vAz_at - vBz_at) * nz;
+
+        const lambda = -m_eff_n * (vn + rest_bias);
+        const new_j_n = (j_n_accum + lambda) > 0 ? (j_n_accum + lambda) : 0;
+        const delta = new_j_n - j_n_accum;
+        data[off + 10] = new_j_n;
+
+        if (delta !== 0) {
+            const Px = nx * delta, Py = ny * delta, Pz = nz * delta;
+            apply_impulse_to_body(rbA, trA, invMA, rax, ray, raz, Px, Py, Pz, +1, scratch_inertia_a);
+            apply_impulse_to_body(rbB, trB, invMB, rbx, rby, rbz, Px, Py, Pz, -1, scratch_inertia_b);
+        }
+    }
+}
+
+/**
+ * Stage 5 (per substep) — split-impulse position correction. Normal-only
+ * (friction in the pseudo-velocity pass is ill-defined). Reads the per-body
+ * pseudo-velocity, applies a clamped normal impulse driven by the refreshed
+ * `bias_position`, and writes the increment back into the pseudo buffer. The
+ * pose integrator folds pseudo-velocity into `pos += v · dt` and discards it.
+ *
+ * The pseudo buffer must be zeroed by the caller before this stage each
+ * substep (it's a per-substep correction). The position accumulator
+ * `scratch_pos_jn` is likewise reset here per substep.
+ *
+ * @param {ManifoldStore} manifolds
+ * @param {PhysicsSystem} system
+ * @param {number} pos_iters
+ */
+export function solve_position(manifolds, system, pos_iters) {
+    const contact_count = g_contact_count;
+    if (contact_count === 0) return;
+
+    const data = manifolds.data_buffer;
+    const pre = scratch_pre;
+    const idx = scratch_idx;
+    const pos_jn = scratch_pos_jn;
+    const pseudoVel = system.__pseudo_velocity;
+
+    // Reset the position-impulse accumulator for this substep.
+    for (let ci = 0; ci < contact_count; ci++) pos_jn[ci] = 0;
+
+    for (let iter = 0; iter < pos_iters; iter++) {
+        for (let ci = 0; ci < contact_count; ci++) {
+            const pre_off = ci * PRE_STRIDE;
+            const bias_p = pre[pre_off + 22];
+            if (bias_p === 0) continue;
+
+            const slot = idx[ci * INDEX_STRIDE];
+            const cidx = idx[ci * INDEX_STRIDE + 1];
+            const idxA = idx[ci * INDEX_STRIDE + 2];
+            const idxB = idx[ci * INDEX_STRIDE + 3];
+            const rbA = system.__bodies[idxA];
+            const rbB = system.__bodies[idxB];
+            const trA = system.__transforms[idxA];
+            const trB = system.__transforms[idxB];
+            const invMA = inv_mass_of(rbA);
+            const invMB = inv_mass_of(rbB);
+
+            const rax = pre[pre_off + 6], ray = pre[pre_off + 7], raz = pre[pre_off + 8];
+            const rbx = pre[pre_off + 9], rby = pre[pre_off + 10], rbz = pre[pre_off + 11];
+            const m_eff_n = pre[pre_off + 18];
+
+            const slot_off = manifolds.slot_data_offset(slot);
+            const off = slot_off + cidx * CONTACT_STRIDE;
+            const nx = data[off + 6], ny = data[off + 7], nz = data[off + 8];
+
+            const baseA = idxA * PSEUDO_STRIDE;
+            const baseB = idxB * PSEUDO_STRIDE;
+
+            const pslA_x = pseudoVel[baseA],     pslA_y = pseudoVel[baseA + 1], pslA_z = pseudoVel[baseA + 2];
+            const psaA_x = pseudoVel[baseA + 3], psaA_y = pseudoVel[baseA + 4], psaA_z = pseudoVel[baseA + 5];
+            const pslB_x = pseudoVel[baseB],     pslB_y = pseudoVel[baseB + 1], pslB_z = pseudoVel[baseB + 2];
+            const psaB_x = pseudoVel[baseB + 3], psaB_y = pseudoVel[baseB + 4], psaB_z = pseudoVel[baseB + 5];
+
+            const psvAx_at = pslA_x + psaA_y * raz - psaA_z * ray;
+            const psvAy_at = pslA_y + psaA_z * rax - psaA_x * raz;
+            const psvAz_at = pslA_z + psaA_x * ray - psaA_y * rax;
+            const psvBx_at = pslB_x + psaB_y * rbz - psaB_z * rby;
+            const psvBy_at = pslB_y + psaB_z * rbx - psaB_x * rbz;
+            const psvBz_at = pslB_z + psaB_x * rby - psaB_y * rbx;
+
+            const psdvx = psvAx_at - psvBx_at;
+            const psdvy = psvAy_at - psvBy_at;
+            const psdvz = psvAz_at - psvBz_at;
+            const psvn = psdvx * nx + psdvy * ny + psdvz * nz;
+
+            const jn_accum = pos_jn[ci];
+            const lambda = -m_eff_n * (psvn + bias_p);
+            const sum = jn_accum + lambda;
+            const new_jn = sum > 0 ? sum : 0;
+            const delta = new_jn - jn_accum;
+            pos_jn[ci] = new_jn;
+
+            if (delta !== 0) {
+                const Px = nx * delta;
+                const Py = ny * delta;
+                const Pz = nz * delta;
+                apply_impulse_to_pseudo(pseudoVel, baseA, rbA, trA, invMA, rax, ray, raz, Px, Py, Pz, +1, scratch_inertia_a);
+                apply_impulse_to_pseudo(pseudoVel, baseB, rbB, trB, invMB, rbx, rby, rbz, Px, Py, Pz, -1, scratch_inertia_b);
+            }
+        }
+    }
+}
+
+/**
+ * Convenience single-step driver: prepare → refresh → velocity →
+ * restitution → position, all at the full `dt` (one substep). Equivalent to
+ * the Phase-2 solver. The substepped path in `PhysicsSystem.fixedUpdate`
+ * calls the stages directly; this entry point exists for callers/tests that
+ * want a one-shot solve.
+ *
+ * The position pass writes `system.__pseudo_velocity`; the caller must zero
+ * that buffer before this call and fold it into the pose afterwards.
+ *
+ * @param {ManifoldStore} manifolds
+ * @param {PhysicsSystem} system
+ * @param {number} dt
+ * @param {number} [iters]
+ * @param {number} [pos_iters]
+ */
+export function solve_contacts(manifolds, system, dt,
+                                iters = DEFAULT_VELOCITY_ITERATIONS,
+                                pos_iters = DEFAULT_POSITION_ITERATIONS) {
+    if (dt <= 0) return;
+    if (prepare_contacts(manifolds, system, dt) === 0) return;
+    refresh_contacts(manifolds, system);
+    warm_start_contacts(manifolds, system);
+    solve_velocity(manifolds, system, iters);
+    apply_restitution(manifolds, system);
+    solve_position(manifolds, system, pos_iters);
+}
+
+export { DEFAULT_VELOCITY_ITERATIONS, DEFAULT_POSITION_ITERATIONS };