@woosh/meep-engine 2.140.0 → 2.142.0

package/src/engine/physics/queries/raycast.js

@@ -8,42 +8,67 @@ import {
 } from "../../../core/bvh2/bvh3/BVH.js";
 import { returnTrue } from "../../../core/function/returnTrue.js";
 import { aabb3_near_distance_to_intersection_ray_segment } from "../../../core/geom/3d/aabb/aabb3_near_distance_to_intersection_ray_segment.js";
+import { refine_ray_hit, RAY_REFINE_UNSUPPORTED } from "../narrowphase/refine_ray_hit.js";
 
 const stack = SCRATCH_UINT32_TRAVERSAL_STACK;
 
 /**
- * Walk a single BVH along a ray and update the running nearest hit.
+ * Reusable nearest-hit accumulator + scratch. Module-scoped so {@link raycast}
+ * doesn't allocate per call; the physics step is single-threaded so contention
+ * isn't a concern.
+ * @type {{best_t:number, best_body:number}}
+ */
+const acc = { best_t: Infinity, best_body: 0 };
+const best_normal = new Float64Array(3); // winning hit's world normal
+const cand_normal = new Float64Array(3); // per-leaf candidate normal
+
+/**
+ * Outward AABB-face normal at a hit point — the fallback normal for a leaf
+ * whose shape has no exact ray test (composite convex). The hit point is
+ * projected into the AABB's normalised local space (centre = 0, faces = ±1);
+ * the dominant component's axis is the entry face and its sign the outward
+ * normal.
+ */
+function aabb_face_normal(out, min_x, min_y, min_z, max_x, max_y, max_z, hx, hy, hz) {
+    const cx = (min_x + max_x) * 0.5;
+    const cy = (min_y + max_y) * 0.5;
+    const cz = (min_z + max_z) * 0.5;
+    const px = (hx - cx) * 2 / (max_x - min_x);
+    const py = (hy - cy) * 2 / (max_y - min_y);
+    const pz = (hz - cz) * 2 / (max_z - min_z);
+    const apx = px < 0 ? -px : px;
+    const apy = py < 0 ? -py : py;
+    const apz = pz < 0 ? -pz : pz;
+    if (apx >= apy && apx >= apz) { out[0] = px >= 0 ? 1 : -1; out[1] = 0; out[2] = 0; }
+    else if (apy >= apz) { out[0] = 0; out[1] = py >= 0 ? 1 : -1; out[2] = 0; }
+    else { out[0] = 0; out[1] = 0; out[2] = pz >= 0 ? 1 : -1; }
+}
+
+/**
+ * Walk a single BVH along a ray, refining each crossing leaf against its true
+ * shape geometry and keeping the nearest confirmed hit.
  *
- * Hit refinement is broadphase-only: the returned `t` is the distance to
- * the inflated leaf AABB. Narrowphase shape refinement is a follow-up.
+ * Pruning stays on the *inflated leaf AABB* entry distance `t_near`: a shape
+ * hit is always at or beyond its tight AABB entry, which is at or beyond the
+ * inflated-AABB entry, so a subtree whose AABB entry is past the current best
+ * cannot contain a closer hit. A leaf whose ray crosses the fat AABB but misses
+ * the true shape contributes nothing — the correctness gain over broadphase.
  *
- * @param {BVH} bvh
- * @param {number} root
- * @param {number} ox
- * @param {number} oy
- * @param {number} oz
- * @param {number} dx
- * @param {number} dy
- * @param {number} dz
- * @param {number} inv_dx
- * @param {number} inv_dy
- * @param {number} inv_dz
+ * @param {BVH} bvh @param {number} root
+ * @param {PhysicsSystem} system
+ * @param {number} ox @param {number} oy @param {number} oz  ray origin
+ * @param {number} dx @param {number} dy @param {number} dz  ray dir (unit)
+ * @param {number} inv_dx @param {number} inv_dy @param {number} inv_dz
  * @param {number} max_distance
- * @param {{best_t:number, best_body:number, best_node:number, best_bvh:BVH|null}} acc
- * @param {(body_id:number)=>boolean} test  per-leaf filter
+ * @param {(entity:number, collider:Collider)=>boolean} filter
  */
 function bvh_raycast_nearest(
-    bvh, root,
-    ox, oy, oz,
-    dx, dy, dz,
+    bvh, root, system,
+    ox, oy, oz, dx, dy, dz,
     inv_dx, inv_dy, inv_dz,
-    max_distance,
-    acc,
-    test
+    max_distance, filter
 ) {
-    if (root === NULL_NODE) {
-        return;
-    }
+    if (root === NULL_NODE) return;
 
     const float32 = bvh.__data_float32;
     const uint32 = bvh.__data_uint32;
@@ -66,181 +91,105 @@ function bvh_raycast_nearest(
         );
 
         // No intersection or this subtree can't beat the best known hit — prune.
-        if (t_near >= acc.best_t) {
-            continue;
-        }
+        if (t_near >= acc.best_t) continue;
 
         const child_1 = uint32[address + COLUMN_CHILD_1];
         if (child_1 !== NULL_NODE) {
             stack[pointer++] = uint32[address + COLUMN_CHILD_2];
             stack[pointer++] = child_1;
-        } else {
-            const body_id = uint32[address + COLUMN_USER_DATA];
-            if (test(body_id)) {
+            continue;
+        }
+
+        // Leaf — refine against the true shape.
+        const body_id = uint32[address + COLUMN_USER_DATA];
+        const entity = system.entityOf(body_id);
+        if (entity < 0) continue; // unlinked concurrently
+        const idx = system.__index_of(body_id);
+        const collider = system.__primary_collider(idx);
+        if (collider === null) continue;
+        if (!filter(entity, collider)) continue;
+
+        const tr = system.__transforms[idx];
+        const refined = refine_ray_hit(
+            collider.shape, tr.position, tr.rotation,
+            ox, oy, oz, dx, dy, dz, acc.best_t, cand_normal
+        );
+
+        if (refined === RAY_REFINE_UNSUPPORTED) {
+            // No exact ray test for this shape — keep the broadphase AABB hit.
+            if (t_near < acc.best_t) {
                 acc.best_t = t_near;
                 acc.best_body = body_id;
-                acc.best_node = node;
-                acc.best_bvh = bvh;
+                aabb_face_normal(best_normal,
+                    float32[address], float32[address + 1], float32[address + 2],
+                    float32[address + 3], float32[address + 4], float32[address + 5],
+                    ox + dx * t_near, oy + dy * t_near, oz + dz * t_near);
             }
+        } else if (refined < acc.best_t) { // a refined miss is Infinity → never wins
+            acc.best_t = refined;
+            acc.best_body = body_id;
+            best_normal[0] = cand_normal[0];
+            best_normal[1] = cand_normal[1];
+            best_normal[2] = cand_normal[2];
         }
     }
 
     stack.pointer = stack_top;
 }
 
-/**
- * Reusable nearest-hit accumulator. Module-scoped so {@link raycast} doesn't
- * allocate per call. The physics step is single-threaded so contention isn't
- * a concern.
- * @type {{best_t:number, best_body:number, best_node:number, best_bvh:BVH|null}}
- */
-const acc = { best_t: Infinity, best_body: 0, best_node: 0, best_bvh: null };
-
-/**
- * Reusable closure bound to the current system + user filter so we don't
- * allocate a new function object per call. Re-wired at the top of each
- * {@link raycast} invocation. The BVH traversal sees a stable identity
- * which helps V8 keep the call site monomorphic.
- *
- * Carries state via the module-scoped `bound_*` slots below.
- * @type {(body_id:number)=>boolean}
- */
-const bound_test = (body_id) => {
-    const system = bound_system;
-    const entity = system.entityOf(body_id);
-    if (entity < 0) return false;
-    // v1 limitation: when a multi-collider body's BVH leaf is hit, we pass
-    // the body's primary (first-attached) collider rather than the specific
-    // collider the ray actually crossed. Sufficient for entity-level
-    // filtering; per-collider filtering needs the BVH user_data scheme to
-    // encode collider index too — future work.
-    const collider = system.__primary_collider(system.__index_of(body_id));
-    return bound_filter(entity, collider);
-};
-
-let bound_system = null;
-let bound_filter = returnTrue;
-
 /**
  * Raycast against both broadphase trees (static + dynamic) of a
- * {@link PhysicsSystem}, filling `result` with the nearest hit and returning
- * `true` on hit, `false` on miss.
+ * {@link PhysicsSystem}, refined against each candidate's true shape geometry.
+ * Fills `result` with the nearest hit and returns `true` on hit, `false` on
+ * miss. `result.t` is the exact surface distance and `result.normal` the true
+ * surface normal for sphere / box / capsule / mesh / heightmap colliders;
+ * composite convex shapes (no exact ray test yet) fall back to the broadphase
+ * AABB hit + AABB-face normal.
  *
- * Hit normal is the AABB face normal — exact for AABB-shaped colliders,
- * a stable approximation for general convex shapes (an upcoming narrowphase
- * refinement pass will replace this with the true shape normal at the same
- * call site, no API change).
+ * Multi-collider bodies resolve their primary (first-attached) collider — the
+ * BVH leaf encodes only `body_id`; per-collider rays need the leaf user-data to
+ * carry the collider index (future work).
  *
  * @param {PhysicsSystem} system
  * @param {Ray3} ray  origin + unit direction + `tMax`
  * @param {PhysicsSurfacePoint} result  populated on hit; untouched on miss
- * @param {(entity:number, collider:Collider)=>boolean} [filter] mandatory in
- *   contract; defaults to {@link returnTrue} (accept everything). Called once
- *   per BVH leaf that crosses the ray.
+ * @param {(entity:number, collider:Collider)=>boolean} [filter] called once per
+ *   crossing leaf; defaults to {@link returnTrue}.
  * @returns {boolean} true on hit, false on miss
  */
 export function raycast(system, ray, result, filter = returnTrue) {
-    const ox = ray.origin_x;
-    const oy = ray.origin_y;
-    const oz = ray.origin_z;
-    const dx = ray.direction_x;
-    const dy = ray.direction_y;
-    const dz = ray.direction_z;
+    const ox = ray.origin_x, oy = ray.origin_y, oz = ray.origin_z;
+    const dx = ray.direction_x, dy = ray.direction_y, dz = ray.direction_z;
     const max_distance = ray.tMax;
 
     acc.best_t = max_distance;
     acc.best_body = 0;
-    acc.best_node = 0;
-    acc.best_bvh = null;
+    best_normal[0] = 0; best_normal[1] = 0; best_normal[2] = 0;
 
-    bound_system = system;
-    bound_filter = filter;
-
-    const inv_dx = 1 / dx;
-    const inv_dy = 1 / dy;
-    const inv_dz = 1 / dz;
+    const inv_dx = 1 / dx, inv_dy = 1 / dy, inv_dz = 1 / dz;
 
     bvh_raycast_nearest(
-        system.staticBvh, system.staticBvh.root,
-        ox, oy, oz,
-        dx, dy, dz,
-        inv_dx, inv_dy, inv_dz,
-        max_distance,
-        acc, bound_test
+        system.staticBvh, system.staticBvh.root, system,
+        ox, oy, oz, dx, dy, dz, inv_dx, inv_dy, inv_dz, max_distance, filter
     );
-
     bvh_raycast_nearest(
-        system.dynamicBvh, system.dynamicBvh.root,
-        ox, oy, oz,
-        dx, dy, dz,
-        inv_dx, inv_dy, inv_dz,
-        max_distance,
-        acc, bound_test
+        system.dynamicBvh, system.dynamicBvh.root, system,
+        ox, oy, oz, dx, dy, dz, inv_dx, inv_dy, inv_dz, max_distance, filter
     );
 
-    if (acc.best_bvh === null || acc.best_t >= max_distance) {
-        return false;
-    }
+    // Any hit updates best_t to strictly below max_distance (both the refined
+    // and AABB-fallback paths require t < current best); a miss leaves it at
+    // max_distance. This is body_id-agnostic — the first body packs to id 0.
+    if (acc.best_t >= max_distance) return false;
 
     const entity = system.entityOf(acc.best_body);
-    if (entity < 0) {
-        // Body was unlinked concurrently; treat as a miss.
-        return false;
-    }
+    if (entity < 0) return false; // body unlinked concurrently → treat as miss
 
-    // Hit position in world space.
     const t = acc.best_t;
-    const hx = ox + dx * t;
-    const hy = oy + dy * t;
-    const hz = oz + dz * t;
-
-    // AABB face normal at the hit point. Read the best leaf's AABB back from
-    // its BVH; project the hit point into the AABB's normalised local space
-    // (centre = 0, faces = ±1); the dominant component is the entry-face
-    // axis, its sign gives the outward normal.
-    const bvh = acc.best_bvh;
-    const node_address = acc.best_node * ELEMENT_WORD_COUNT;
-    const f32 = bvh.__data_float32;
-    const aabb_min_x = f32[node_address];
-    const aabb_min_y = f32[node_address + 1];
-    const aabb_min_z = f32[node_address + 2];
-    const aabb_max_x = f32[node_address + 3];
-    const aabb_max_y = f32[node_address + 4];
-    const aabb_max_z = f32[node_address + 5];
-
-    const cx = (aabb_min_x + aabb_max_x) * 0.5;
-    const cy = (aabb_min_y + aabb_max_y) * 0.5;
-    const cz = (aabb_min_z + aabb_max_z) * 0.5;
-    // Multiplicative inverse of the half-extent saves one divide per axis.
-    // A degenerate AABB (zero extent on an axis) is impossible for live BVH
-    // leaves — physics shapes always have non-zero bounding extent.
-    const inv_half_x = 2 / (aabb_max_x - aabb_min_x);
-    const inv_half_y = 2 / (aabb_max_y - aabb_min_y);
-    const inv_half_z = 2 / (aabb_max_z - aabb_min_z);
-
-    const px = (hx - cx) * inv_half_x;
-    const py = (hy - cy) * inv_half_y;
-    const pz = (hz - cz) * inv_half_z;
-
-    const apx = px < 0 ? -px : px;
-    const apy = py < 0 ? -py : py;
-    const apz = pz < 0 ? -pz : pz;
-
-    let nx, ny, nz;
-    if (apx >= apy && apx >= apz) {
-        nx = px >= 0 ? 1 : -1; ny = 0; nz = 0;
-    } else if (apy >= apz) {
-        nx = 0; ny = py >= 0 ? 1 : -1; nz = 0;
-    } else {
-        nx = 0; ny = 0; nz = pz >= 0 ? 1 : -1;
-    }
-
-    // Fill result. Direct typed-array writes — Vector3 extends Float32Array,
-    // and there are no observers subscribed to a query-result vector.
     const rp = result.position;
     const rn = result.normal;
-    rp[0] = hx; rp[1] = hy; rp[2] = hz;
-    rn[0] = nx; rn[1] = ny; rn[2] = nz;
+    rp[0] = ox + dx * t; rp[1] = oy + dy * t; rp[2] = oz + dz * t;
+    rn[0] = best_normal[0]; rn[1] = best_normal[1]; rn[2] = best_normal[2];
     result.t = t;
     result.entity = entity;
     result.body_id = acc.best_body;

package/src/engine/physics/solver/solve_contacts.d.ts

@@ -51,6 +51,34 @@ export function warm_start_contacts(manifolds: ManifoldStore, system: PhysicsSys
  * @param {PhysicsSystem} system
  */
 export function refresh_contacts(manifolds: ManifoldStore, system: PhysicsSystem): void;
+/**
+ * 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: ManifoldStore, system: PhysicsSystem): void;
 /**
  * Stage 3 (per substep) — velocity iterations enforcing pure
  * non-penetration (`vn → 0`) plus Coulomb-disk friction. No bias: depth

package/src/engine/physics/solver/solve_contacts.d.ts.map

@@ -1 +1 @@
-{"version":3,"file":"solve_contacts.d.ts","sourceRoot":"","sources":["../../../../../src/engine/physics/solver/solve_contacts.js"],"names":[],"mappings":"AA8ZA;;;;;;;;;;;;;GAaG;AACH,0FAHW,MAAM,GACJ,MAAM,CA4IlB;AAED;;;;;;;;;;;;;;;GAeG;AACH,2FAuCC;AAED;;;;;;;;;;;;;;;;;;;GAmBG;AACH,wFA2DC;AAED;;;;;;;;GAQG;AACH,uFAFW,MAAM,QA2GhB;AAED;;;;;;;;;;;GAWG;AACH,yFA4DC;AAED;;;;;;;;;;;;;;GAcG;AACH,2FAFW,MAAM,QA4EhB;AAED;;;;;;;;;;;;;;;GAeG;AACH,oFAJW,MAAM,UACN,MAAM,cACN,MAAM,QAYhB;AAl7BD;;;;;GAKG;AACH,0CAFU,MAAM,CAEuB;AAEvC;;;GAGG;AACH,0CAFU,MAAM,CAEsB"}
+{"version":3,"file":"solve_contacts.d.ts","sourceRoot":"","sources":["../../../../../src/engine/physics/solver/solve_contacts.js"],"names":[],"mappings":"AAgcA;;;;;;;;;;;;;GAaG;AACH,0FAHW,MAAM,GACJ,MAAM,CA4JlB;AAED;;;;;;;;;;;;;;;GAeG;AACH,2FAuCC;AAED;;;;;;;;;;;;;;;;;;;GAmBG;AACH,wFAgEC;AAED;;;;;;;;;;;;;;;;;;;;;;;;;;GA0BG;AACH,iGAmFC;AAED;;;;;;;;GAQG;AACH,uFAFW,MAAM,QA2GhB;AAED;;;;;;;;;;;GAWG;AACH,yFA4DC;AAED;;;;;;;;;;;;;;GAcG;AACH,2FAFW,MAAM,QA4EhB;AAED;;;;;;;;;;;;;;;GAeG;AACH,oFAJW,MAAM,UACN,MAAM,cACN,MAAM,QAahB;AAxlCD;;;;;GAKG;AACH,0CAFU,MAAM,CAEuB;AAEvC;;;GAGG;AACH,0CAFU,MAAM,CAEsB"}

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

@@ -5,6 +5,7 @@ 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";
 
 /**
@@ -18,7 +19,8 @@ import { friction_cone_clamp } from "./friction_cone.js";
  *   prepare_contacts(manifolds, system, h)         // once per outer step
  *   for each substep:
  *       (system integrates gravity by h)
- *       refresh_contacts(manifolds, system)        // re-derive geometry
+ *       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)
@@ -177,6 +179,27 @@ let scratch_pos_jn = new Float64Array(64);
  */
 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
@@ -205,6 +228,17 @@ function ensure_capacity(n) {
     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);
+    }
 }
 
 /**
@@ -452,6 +486,8 @@ export function prepare_contacts(manifolds, system, dt_sub) {
     const mus = scratch_mu;
     const pos_jn = scratch_pos_jn;
 
+    g_concave_slot_count = 0;
+
     let c = 0;
     for (let i = 0; i < total_slots; i++) {
         const slot = slot_list[i];
@@ -466,6 +502,19 @@ export function prepare_contacts(manifolds, system, dt_sub) {
         if (colA === null || colB === null) continue;
         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);
 
@@ -545,6 +594,7 @@ export function prepare_contacts(manifolds, system, dt_sub) {
             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;
@@ -654,6 +704,11 @@ export function refresh_contacts(manifolds, system) {
     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];
@@ -704,6 +759,118 @@ export function refresh_contacts(manifolds, system) {
     }
 }
 
+/**
+ * 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
@@ -1006,6 +1173,7 @@ export function solve_contacts(manifolds, system, dt,
                                 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);