@woosh/meep-engine 2.140.0 → 2.141.0

package/src/engine/physics/narrowphase/narrowphase_step.js

@@ -1420,3 +1420,78 @@ export function narrowphase_step(pair_list, manifolds, lists) {
         }
     }
 }
+
+/**
+ * Re-detect contact GEOMETRY for one existing manifold slot at the bodies'
+ * current poses, updating the witness points / normal / depth of the slot's
+ * existing contacts in place. Does NOT change the contact count, the
+ * feature ids, or the accumulated impulses — it only refreshes geometry.
+ *
+ * This is the per-substep concave path (TGS): for a contact pair involving a
+ * concave body, the contact *feature* (which triangle is deepest, and its
+ * normal) genuinely changes as the body rocks, so the solver's cheap analytic
+ * refresh — which freezes the feature for the whole outer step — pumps energy
+ * in. Re-running the narrowphase geometry each substep gives a fresh,
+ * correct normal so the body settles instead of rocking. Convex pairs keep
+ * the analytic refresh and never call this (their feature is stable).
+ *
+ * Matching is by feature id (stable per-triangle for the decomposition path),
+ * so a contact's geometry tracks the same triangle across substeps. A contact
+ * whose triangle isn't found this substep keeps its previous geometry (a rare
+ * transient; the once-per-frame {@link narrowphase_step} re-establishes the
+ * contact set next outer step). Count never changes here, so the solver's
+ * per-contact scratch (sized once at prepare) stays aligned.
+ *
+ * @param {ManifoldStore} manifolds
+ * @param {number} slot
+ * @param {Array<{collider: Collider, transform: Transform}>} list_a
+ * @param {Array<{collider: Collider, transform: Transform}>} list_b
+ */
+export function redetect_pair_geometry(manifolds, slot, list_a, list_b) {
+    if (list_a === undefined || list_b === undefined) return;
+    const la_len = list_a.length;
+    const lb_len = list_b.length;
+    if (la_len === 0 || lb_len === 0) return;
+
+    const count = manifolds.contact_count(slot);
+    if (count === 0) return;
+
+    const gjk_axis_buf = manifolds.slot_axis_buffer;
+    const gjk_axis_off = manifolds.slot_axis_offset(slot);
+
+    let cc = 0;
+    for (let a = 0; a < la_len; a++) {
+        const ea = list_a[a];
+        for (let b = 0; b < lb_len; b++) {
+            const eb = list_b[b];
+            cc = dispatch_pair(cc, ea.collider, ea.transform, eb.collider, eb.transform,
+                gjk_axis_buf, gjk_axis_off);
+        }
+    }
+    if (cc === 0) return; // nothing re-detected this substep — keep frozen geometry
+
+    const data = manifolds.data_buffer;
+    const slot_off = manifolds.slot_data_offset(slot);
+    for (let j = 0; j < count; j++) {
+        const off = slot_off + j * CONTACT_STRIDE;
+        const fid = data[off + 13];
+        if (fid === 0) continue; // no feature info to match on
+        for (let k = 0; k < cc; k++) {
+            const co = k * CANDIDATE_STRIDE;
+            if (candidates[co + 10] === fid) {
+                // Overwrite geometry only: witnesses, normal, depth.
+                data[off]     = candidates[co];
+                data[off + 1] = candidates[co + 1];
+                data[off + 2] = candidates[co + 2];
+                data[off + 3] = candidates[co + 3];
+                data[off + 4] = candidates[co + 4];
+                data[off + 5] = candidates[co + 5];
+                data[off + 6] = candidates[co + 6];
+                data[off + 7] = candidates[co + 7];
+                data[off + 8] = candidates[co + 8];
+                data[off + 9] = candidates[co + 9];
+                break;
+            }
+        }
+    }
+}

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);