@woosh/meep-engine 2.139.0 → 2.141.0

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

@@ -3,8 +3,51 @@ 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 { redetect_pair_geometry } from "../narrowphase/narrowphase_step.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)
+ *       redetect_concave_contacts(manifolds, system) // concave: fresh narrowphase
+ *       refresh_contacts(manifolds, system)        // convex: analytic re-derive
+ *       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
@@ -25,29 +68,18 @@ function pair_is_sensor(rbA, colA, rbB, colB) {
 }
 
 /**
- * Default velocity-iteration count per `solve_contacts` call. With TGS
- * substepping driven by the PhysicsSystem, each tick now produces multiple
- * solver calls (one per substep), so the per-call iter count is set lower
- * than the classic PGS default of 10. Total iterations per tick =
- * substeps × iters_per_substep; the recommended pairing is 4 × 2 = 8,
- * which matches Box2D's TGS defaults and outperforms 1 × 10 on tall
- * stacks (impulses get position-integrated between substeps, so info
- * propagation through a contact chain compounds across substeps rather
- * than fighting against accumulating position drift inside one solve).
- *
- * Direct callers that want classic PGS can still pass `iters = 10` and
- * skip substepping by calling once with the full tick `dt`.
+ * 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 = 2;
+const DEFAULT_VELOCITY_ITERATIONS = 10;
 
 /**
- * Baumgarte position-correction gain. Smaller values are "softer" (slower to
- * push bodies apart but more stable); larger values can introduce jitter.
- * 0.2 is the Catto / Box2D default.
+ * Position-iteration count per substep (split-impulse pseudo-velocity pass).
  * @type {number}
  */
-const BAUMGARTE_BETA = 0.2;
+const DEFAULT_POSITION_ITERATIONS = 2;
 
 /**
  * Penetration allowed without applying position correction. Eliminates
@@ -57,32 +89,128 @@ const BAUMGARTE_BETA = 0.2;
 const PENETRATION_SLOP = 0.005;
 
 /**
- * Velocity below which restitution is suppressed (no "buzz" of micro-bounces
- * when bodies are nearly resting on each other).
+ * 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;
 
 /**
- * Pre-step scratch stride per contact: 16 doubles
- *   0..2 : rA  (contact point − bodyA position)
- *   3..5 : rB  (contact point − bodyB position)
- *   6..8 : t1  (tangent 1, unit, world)
- *   9..11: t2  (tangent 2, unit, world)
- *   12   : m_eff_n
- *   13   : m_eff_t1
- *   14   : m_eff_t2
- *   15   : bias_n  (combined Baumgarte + restitution target velocity)
+ * 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 = 16;
+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);
+
+/**
+ * Per-contact flag: 1 if the contact's pair involves a concave body, else 0.
+ * Concave contacts take the per-substep re-detection path
+ * ({@link redetect_concave_contacts}) — their feature genuinely changes as
+ * the body rocks, so the analytic refresh that freezes it would pump energy.
+ * Convex contacts (flag 0) take the cheap analytic {@link refresh_contacts}.
+ * @type {Uint8Array}
+ */
+let scratch_concave = new Uint8Array(64);
+
+/**
+ * Distinct concave-involved manifold slots touched this step, with the body
+ * indices needed to fetch their collider lists for re-detection. Parallel
+ * arrays, `g_concave_slot_count` valid entries, filled by
+ * {@link prepare_contacts}.
+ */
+let concave_slot = new Uint32Array(32);
+let concave_slot_idxA = new Uint32Array(32);
+let concave_slot_idxB = new Uint32Array(32);
+let g_concave_slot_count = 0;
+
+/**
+ * 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) {
@@ -94,12 +222,27 @@ function ensure_capacity(n) {
     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);
+    }
+    if (scratch_concave.length < n) {
+        scratch_concave = new Uint8Array(n * 2);
+    }
+}
+
+function ensure_concave_slot_capacity(n) {
+    if (concave_slot.length < n) {
+        concave_slot = new Uint32Array(n * 2);
+        concave_slot_idxA = new Uint32Array(n * 2);
+        concave_slot_idxB = new Uint32Array(n * 2);
+    }
 }
 
 /**
- * Build an orthonormal tangent basis perpendicular to a unit normal. Standard
- * "least-aligned axis" trick: pick the world axis least aligned with `n`,
- * cross with `n`, then cross again for the second tangent.
+ * Build an orthonormal tangent basis perpendicular to a unit normal.
  *
  * @param {Float64Array} out  6 floats: t1.xyz then t2.xyz
  * @param {number} off
@@ -108,7 +251,6 @@ function ensure_capacity(n) {
  * @param {number} nz
  */
 function build_tangents(out, off, nx, ny, nz) {
-    // Pick the world axis least aligned with n.
     const ax = nx < 0 ? -nx : nx;
     const ay = ny < 0 ? -ny : ny;
     const az = nz < 0 ? -nz : nz;
@@ -121,14 +263,12 @@ function build_tangents(out, off, nx, ny, nz) {
     } else {
         rx = 0; ry = 0; rz = 1;
     }
-    // t1 = normalize(n × r)
     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;
 
-    // t2 = n × t1  (already unit since n and t1 are orthogonal unit vectors)
     const t2x = ny * t1z - nz * t1y;
     const t2y = nz * t1x - nx * t1z;
     const t2z = nx * t1y - ny * t1x;
@@ -147,8 +287,7 @@ function inv_mass_of(rb) {
 }
 
 /**
- * Combine two friction coefficients. Geometric mean — matches the Bullet /
- * PhysX default and never goes beyond the smaller of the two.
+ * Combine two friction coefficients (geometric mean — Bullet / PhysX default).
  * @param {number} a
  * @param {number} b
  * @returns {number}
@@ -158,8 +297,7 @@ function combine_friction(a, b) {
 }
 
 /**
- * Combine two restitution coefficients. Maximum — visually bounciest pair
- * wins; matches Unity / common game-engine default.
+ * Combine two restitution coefficients (maximum — Unity / common default).
  * @param {number} a
  * @param {number} b
  * @returns {number}
@@ -173,28 +311,17 @@ const scratch_inertia_a = new Float64Array(3);
 const scratch_inertia_b = new Float64Array(3);
 
 /**
- * Apply an impulse `P` at body-relative offset `r`, advancing both the linear
- * velocity (Δv = P/m) and angular velocity (Δω = I⁻¹·(r × P)).
- *
- * Writes go directly to the typed-array backing of `linearVelocity` and
- * `angularVelocity` — bypassing {@link Vector3#set} on purpose. Each `set()`
- * runs a hasHandlers / sendN dispatch path which is dead weight in the
- * solver inner loop (no observer is subscribed to a body's velocity in
- * normal use, and even when one is, fanning out per-iteration writes is
- * useless — callers want the post-step value).
+ * 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 (the contact normal points
- *                       from B → A, so A receives +P and B receives −P).
- * @param {Float64Array} scratch_inertia 3-vector scratch for the angular term
+ * @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,
@@ -214,12 +341,11 @@ function apply_impulse_to_body(
     lv[1] += sPy * invM;
     lv[2] += sPz * invM;
 
-    // r × P (already-signed)
     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, transform, tx, ty, tz);
+    world_inverse_inertia_apply(scratch_inertia, 0, rb.inverseInertiaLocal, transform.rotation, tx, ty, tz);
 
     const av = rb.angularVelocity;
     av[0] += scratch_inertia[0];
@@ -227,18 +353,56 @@ function apply_impulse_to_body(
     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. Computes `(r × axis)^T · I⁻¹_world · (r × axis)`.
+ * 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 axis x
- * @param {number} ay
- * @param {number} az
+ * @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}
  */
@@ -256,7 +420,7 @@ function angular_jacobian_contribution(
     const rxay = rz * ax - rx * az;
     const rxaz = rx * ay - ry * ax;
 
-    world_inverse_inertia_apply(scratch_inertia, 0, rb, transform, rxax, rxay, rxaz);
+    world_inverse_inertia_apply(scratch_inertia, 0, ii, transform.rotation, rxax, rxay, rxaz);
 
     return rxax * scratch_inertia[0]
          + rxay * scratch_inertia[1]
@@ -264,97 +428,68 @@ function angular_jacobian_contribution(
 }
 
 /**
- * Sequential-impulse contact solver. Partitions the touched contact set into
- * islands (via {@link IslandBuilder}, owned by the PhysicsSystem and rebuilt
- * upstream in the pipeline), then iterates each island independently.
- *
- * Per-island iteration matters for two reasons:
- *  1. Impulse propagation converges inside an island without waiting for
- *     unrelated bodies' Gauss-Seidel updates from previous outer loops.
- *  2. Disconnected awake bodies don't pay each other's solver cost — adding
- *     an unrelated active body to the scene scales O(island_size) rather than
- *     O(global_active).
+ * 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.
  *
- * Coulomb friction is applied as a disk clamp in the contact tangent plane.
- * Position correction is folded into the velocity solve via Baumgarte bias.
- *
- * The `apply_warm_start` flag gates the warm-start impulse application.
- * In a classic PGS call (one solve per tick), pass `true` (default) — the
- * cached impulses from the previous tick get replayed onto the velocity,
- * seeding the iterations near the converged answer. In TGS substepping,
- * the PhysicsSystem calls this once per substep; only substep 0 should
- * apply warm-start, because subsequent substeps continue from the solver
- * state left by the previous substep (the impulses in `data` already
- * reflect the current velocity, so re-applying them would double-count).
- *
- * Inertial: angular response uses the full world-frame inverse inertia
- * `R · diag(I⁻¹_local) · R^T` via {@link world_inverse_inertia_apply},
- * correct for arbitrary rotations + diagonal local inertia.
- *
- * @param {ManifoldStore} manifolds
- * @param {PhysicsSystem} system  PhysicsSystem; reads `system.islands`.
- * @param {number} dt step duration (full tick `dt` for PGS, sub-tick for TGS)
- * @param {number} [iters]
- * @param {boolean} [apply_warm_start]
+ * @param {PhysicsSystem} system
+ * @returns {{slot_list: Uint32Array, total_slots: number}|null}
  */
-export function solve_contacts(manifolds, system, dt, iters = DEFAULT_VELOCITY_ITERATIONS, apply_warm_start = true) {
-    if (dt <= 0) return;
-
+function island_slot_range(system) {
     const islands = system.islands;
-    if (islands === undefined || islands === null) return;
-
+    if (islands === undefined || islands === null) return null;
     const island_count = islands.island_count;
-    if (island_count === 0) return;
-
-    const slot_list = islands.contact_data;
-    const slot_offsets = islands.contact_offsets;
-
-    for (let i = 0; i < island_count; i++) {
-        const start = slot_offsets[i];
-        const end = slot_offsets[i + 1];
-        if (end === start) continue;
-        solve_island(manifolds, system, dt, iters, slot_list, start, end, apply_warm_start);
-    }
+    if (island_count === 0) return null;
+    return {
+        slot_list: islands.contact_data,
+        total_slots: islands.contact_offsets[island_count],
+    };
 }
 
 /**
- * Solve a single island's contact constraints. Operates on the slice
- * `slot_list[slot_start..slot_end)` of manifold slot ids. Module-scoped
- * scratch buffers are reused across islands (resized by doubling when an
- * island has more contacts than seen before in the same step).
+ * 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
- * @param {number} iters
- * @param {Uint32Array} slot_list
- * @param {number} slot_start
- * @param {number} slot_end
- * @param {boolean} apply_warm_start whether to replay cached impulses (see
- *   {@link solve_contacts})
+ * @param {number} dt_sub substep size `dt / substeps`
+ * @returns {number} number of contacts prepared (also stored module-side)
  */
-function solve_island(manifolds, system, dt, iters, slot_list, slot_start, slot_end, apply_warm_start) {
-    // 1. Count contacts in this island so we can size scratch.
-    // Stale-but-cached slots (the island builder skips them upstream) and
-    // sensor-only slots contribute zero impulse but we still want exact
-    // sizing — sensor slots are filtered at the per-pair check below and
-    // contribute zero to `contact_total`.
+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 = slot_start; i < slot_end; i++) {
-        const slot = slot_list[i];
-        contact_total += manifolds.contact_count(slot);
+    for (let i = 0; i < total_slots; i++) {
+        contact_total += manifolds.contact_count(slot_list[i]);
     }
-    if (contact_total === 0) return;
+    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;
+
+    g_concave_slot_count = 0;
 
-    // 2. Pre-step + warm-start.
     let c = 0;
-    for (let i = slot_start; i < slot_end; i++) {
+    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));
@@ -362,18 +497,24 @@ function solve_island(manifolds, system, dt, iters, slot_list, slot_start, slot_
         const rbB = system.__bodies[idxB];
         const trA = system.__transforms[idxA];
         const trB = system.__transforms[idxB];
-        // Material parameters come from the "primary" collider of each body
-        // — for single-collider bodies this is the only one; for compound
-        // bodies it's a v1 approximation (per-contact source-collider
-        // tracking is a follow-up). Fall back to defaults if a body has no
-        // attached colliders.
         const colA = system.__primary_collider(idxA);
         const colB = system.__primary_collider(idxB);
         if (colA === null || colB === null) continue;
-        // Sensor pairs: contacts persist in the manifold (for events) but
-        // produce no impulse. Skip the solver work for this pair entirely.
         if (pair_is_sensor(rbA, colA, rbB, colB)) continue;
 
+        // A pair is "concave" when either side's shape is non-convex. Those
+        // contacts take the per-substep re-detection path (the contact
+        // feature moves as the body rocks); convex pairs keep the cheap
+        // analytic refresh. Recorded once per slot for redetect_concave_contacts.
+        const slot_concave = (colA.shape.is_convex === false) || (colB.shape.is_convex === false);
+        if (slot_concave) {
+            ensure_concave_slot_capacity(g_concave_slot_count + 1);
+            concave_slot[g_concave_slot_count] = slot;
+            concave_slot_idxA[g_concave_slot_count] = idxA;
+            concave_slot_idxB[g_concave_slot_count] = idxB;
+            g_concave_slot_count++;
+        }
+
         const invMA = inv_mass_of(rbA);
         const invMB = inv_mass_of(rbB);
 
@@ -382,35 +523,40 @@ function solve_island(manifolds, system, dt, iters, slot_list, slot_start, 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 depth = data[off + 9];
 
-            // Application point: midpoint of the two surface witnesses.
             const px = (wax + wbx) * 0.5;
             const py = (way + wby) * 0.5;
             const pz = (waz + wbz) * 0.5;
 
-            const rax = px - trA.position.x;
-            const ray = py - trA.position.y;
-            const raz = pz - trA.position.z;
-            const rbx = px - trB.position.x;
-            const rby = py - trB.position.y;
-            const rbz = pz - trB.position.z;
+            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;
-            pre[pre_off]     = rax; pre[pre_off + 1] = ray; pre[pre_off + 2] = raz;
-            pre[pre_off + 3] = rbx; pre[pre_off + 4] = rby; pre[pre_off + 5] = rbz;
-            build_tangents(pre, pre_off + 6, nx, ny, nz);
 
-            const t1x = pre[pre_off + 6], t1y = pre[pre_off + 7], t1z = pre[pre_off + 8];
-            const t2x = pre[pre_off + 9], t2y = pre[pre_off + 10], t2z = pre[pre_off + 11];
+            // 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];
 
-            // K = invMA + invMB + (rA×axis)^T·I_w⁻¹_A·(rA×axis) + (rB×axis)^T·I_w⁻¹_B·(rB×axis)
             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);
@@ -421,15 +567,13 @@ function solve_island(manifolds, system, dt, iters, slot_list, slot_start, slot_
                 + 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);
 
-            pre[pre_off + 12] = k_n  > 0 ? 1 / k_n  : 0;
-            pre[pre_off + 13] = k_t1 > 0 ? 1 / k_t1 : 0;
-            pre[pre_off + 14] = k_t2 > 0 ? 1 / k_t2 : 0;
+            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;
 
-            // Pre-impulse relative velocity at the contact point, including
-            // angular contribution. Convention: n is from B toward A;
-            // dv = vA_at_contact − vB_at_contact; vn = dv · n.
-            // Cache the typed-array views once — Vector3 extends Float64Array,
-            // so `lv[0]` is a direct memory read, no signal observer chain.
+            // 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;
@@ -438,62 +582,313 @@ function solve_island(manifolds, system, dt, iters, slot_list, slot_start, slot_
             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;
-            const vn_pre = dvx * nx + dvy * ny + dvz * nz;
-
-            // Baumgarte position correction: gentle separation push when depth
-            // exceeds the slop tolerance.
-            let bias = 0;
-            if (depth > PENETRATION_SLOP) {
-                bias = -BAUMGARTE_BETA / dt * (depth - PENETRATION_SLOP);
-                // negative because the solver formula is `lambda = m_eff * (-vn + bias_pos)`
-                // → bias_pos = beta/dt * pen → we'll flip below by using `lambda = -m_eff * (vn + bias)`
-                // For clarity: target vn_new = -bias_pos (i.e. positive separation velocity).
-                // Stored sign convention: lambda = -m_eff * (vn + bias) with bias < 0 to push apart.
-            }
-            // Restitution: if closing fast, push the post-impulse vn to -e * vn_pre.
+            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) {
-                bias += restitution_combined * vn_pre;
-                // restitution_combined > 0, vn_pre < 0 → contribution is negative,
-                // reinforcing separation.
+                rest_bias = restitution_combined * vn_pre;
             }
-            pre[pre_off + 15] = bias;
+            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;
+            scratch_concave[c] = slot_concave ? 1 : 0;
 
             idx[c * INDEX_STRIDE]     = slot;
             idx[c * INDEX_STRIDE + 1] = k;
             idx[c * INDEX_STRIDE + 2] = idxA;
             idx[c * INDEX_STRIDE + 3] = idxB;
 
-            if (apply_warm_start) {
-                // Warm-start: replay cached impulses immediately so the
-                // iteration loop starts close to the answer. With TGS
-                // substepping, this only fires on substep 0 — later
-                // substeps continue from the impulses left by the
-                // previous substep's iterations (those are already
-                // reflected in the current velocity, re-applying would
-                // double-count).
-                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);
-            }
+            // 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++;
         }
     }
 
-    const contact_count = 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++) {
+        // Concave contacts are refreshed by redetect_concave_contacts (fresh
+        // narrowphase geometry each substep), not by the analytic rotation of
+        // frozen anchors — their feature moves as the body rocks.
+        if (scratch_concave[ci] === 1) 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 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 2b (per substep) — the concave counterpart of {@link refresh_contacts}.
+ *
+ * For each concave-involved slot, re-runs the narrowphase geometry at the
+ * current substep pose ({@link redetect_pair_geometry}, which rewrites the
+ * manifold's witness / normal / depth in place), then re-derives the solver
+ * scratch (lever arms, tangent basis, effective masses, position bias) for
+ * that slot's contacts from the fresh geometry. This is the whole point of
+ * the concave path: a body rocking on a mesh changes which triangle (and
+ * normal) it rests on within a single outer step, and freezing that — as the
+ * convex analytic refresh does — pumps energy in. Re-detecting gives a
+ * correct per-substep normal so the body settles.
+ *
+ * Cost is ~one narrowphase dispatch per concave slot per substep — acceptable
+ * because concave-involved pairs are rare (the common concave case, a convex
+ * body on static terrain, is convex on the moving side and never lands here).
+ * The contact count is held fixed (redetect updates geometry only), so the
+ * scratch stays aligned with prepare.
+ *
+ * Must run before {@link refresh_contacts} / {@link warm_start_contacts} each
+ * substep. `rest_bias` (captured at prepare) and the `scratch_max_jn`
+ * restitution gate are preserved.
+ *
+ * @param {ManifoldStore} manifolds
+ * @param {PhysicsSystem} system  reads `__body_collider_lists`, `__bodies`,
+ *   `__transforms`.
+ */
+export function redetect_concave_contacts(manifolds, system) {
+    const ns = g_concave_slot_count;
+    if (ns === 0) return;
+
+    const lists = system.__body_collider_lists;
+
+    // 1. Re-detect fresh geometry into the manifold for each concave slot.
+    for (let s = 0; s < ns; s++) {
+        redetect_pair_geometry(manifolds, concave_slot[s], lists[concave_slot_idxA[s]], lists[concave_slot_idxB[s]]);
+    }
+
+    // 2. Re-derive the solver scratch for every concave contact from the
+    //    fresh manifold geometry (lever arms, tangents, effective masses,
+    //    position bias). Mirrors prepare's per-contact setup but reads the
+    //    just-updated witness / normal / depth instead of the prepare-time
+    //    values; no local-frame anchors are needed since we re-detect rather
+    //    than rotate frozen anchors.
+    const count = g_contact_count;
+    const data = manifolds.data_buffer;
+    const pre = scratch_pre;
+    const idx = scratch_idx;
+    const spook_a = g_spook_a;
+    const spook_eps = g_spook_eps;
+
+    for (let ci = 0; ci < count; ci++) {
+        if (scratch_concave[ci] === 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 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 depth = data[off + 9];
+
+        const px = (wax + wbx) * 0.5;
+        const py = (way + wby) * 0.5;
+        const pz = (waz + wbz) * 0.5;
+
+        const rax = px - trA.position.x, ray = py - trA.position.y, raz = pz - trA.position.z;
+        const rbx = px - trB.position.x, rby = py - trB.position.y, rbz = pz - trB.position.z;
+
+        const pre_off = ci * PRE_STRIDE;
+        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 + 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;
+
+        let bias_position = 0;
+        if (depth > PENETRATION_SLOP) {
+            bias_position = -spook_a * (depth - 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;
 
-    // 3. Velocity iterations.
     for (let iter = 0; iter < iters; iter++) {
         for (let ci = 0; ci < contact_count; ci++) {
             const slot = idx[ci * INDEX_STRIDE];
@@ -508,22 +903,18 @@ function solve_island(manifolds, system, dt, iters, slot_list, slot_start, slot_
             const invMB = inv_mass_of(rbB);
 
             const pre_off = ci * PRE_STRIDE;
-            const rax = pre[pre_off],     ray = pre[pre_off + 1], raz = pre[pre_off + 2];
-            const rbx = pre[pre_off + 3], rby = pre[pre_off + 4], rbz = pre[pre_off + 5];
-            const t1x = pre[pre_off + 6], t1y = pre[pre_off + 7], t1z = pre[pre_off + 8];
-            const t2x = pre[pre_off + 9], t2y = pre[pre_off + 10], t2z = pre[pre_off + 11];
-            const m_eff_n  = pre[pre_off + 12];
-            const m_eff_t1 = pre[pre_off + 13];
-            const m_eff_t2 = pre[pre_off + 14];
-            const bias_n   = pre[pre_off + 15];
+            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];
 
-            // --- Current relative velocity at contact (linear + angular) ---
-            // Cache typed-array views once; direct indexing avoids the
-            // accessor chain on Vector3.x/y/z.
             const lvA = rbA.linearVelocity, avA = rbA.angularVelocity;
             const lvB = rbB.linearVelocity, avB = rbB.angularVelocity;
 
@@ -537,14 +928,15 @@ function solve_island(manifolds, system, dt, iters, slot_list, slot_start, slot_
             const dvy = vAy_at - vBy_at;
             const dvz = vAz_at - vBz_at;
 
-            // --- Normal impulse ---
+            // --- 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 + bias_n);
+            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;
@@ -555,8 +947,6 @@ function solve_island(manifolds, system, dt, iters, slot_list, slot_start, slot_
             }
 
             // --- Friction impulse (Coulomb disk in tangent plane) ---
-            // Recompute relative velocity at contact after the normal impulse.
-            // lvA/avA/lvB/avB still alias the same arrays, so we just re-read.
             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;
@@ -596,3 +986,199 @@ function solve_island(manifolds, system, dt, iters, slot_list, slot_start, slot_
         }
     }
 }
+
+/**
+ * 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;
+    redetect_concave_contacts(manifolds, system);
+    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 };