@woosh/meep-engine 2.156.0 → 2.157.0

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

@@ -1,393 +1,417 @@
-import { bvh_query_user_data_overlaps_aabb } from "../../../core/bvh2/bvh3/query/bvh_query_user_data_overlaps_aabb.js";
-import { returnTrue } from "../../../core/function/returnTrue.js";
-import { aabb3_transform_oriented } from "../../../core/geom/3d/aabb/aabb3_transform_oriented.js";
-import { body_id_index } from "../body/BodyStorage.js";
-import { gjk } from "../gjk/gjk.js";
-import { mpr } from "../gjk/mpr.js";
-import { PosedShape } from "../narrowphase/PosedShape.js";
-
-/**
- * Bisection halts when the search interval is below this fraction of
- * the sweep length. With `MAX_BISECTION = 32` and a unit-length sweep
- * the achieved precision is ≈ 1 / 2³² (well below any practical
- * world-scale tolerance), but we cap on relative tolerance so a
- * 10 m sweep doesn't insist on sub-micron precision.
- * @type {number}
- */
-const TOI_TOLERANCE = 1e-4;
-
-/**
- * Maximum bisection iterations. `log2(1 / TOI_TOLERANCE)` is enough
- * for any practical sweep; this is the hard upper bound to prevent
- * pathological inputs (NaN ray, etc.) from hanging the query.
- * @type {number}
- */
-const MAX_BISECTION = 32;
-
-/**
- * Coarse linear-search resolution for finding *any* overlap moment
- * along the per-candidate AABB-overlap interval (NOT the full sweep —
- * see `aabb_overlap_interval` for the analytical narrowing that runs
- * upstream). 32 substeps over the AABB-overlap window is dense enough
- * for any sweep where the shape's smallest extent is on the order of
- * its AABB's extent (true for spheres / boxes / capsules); the slab
- * intersection keeps that interval small even for kilometre-scale
- * sweeps, so this isn't the limit it would otherwise be.
- * @type {number}
- */
-const COARSE_STEPS = 32;
-
-const swept_aabb = new Float64Array(6);
-const local_aabb = new Float64Array(6);
-const initial_aabb = new Float64Array(6);
-const target_local_aabb = new Float64Array(6);
-const target_world_aabb = new Float64Array(6);
-
-// GJK simplex buffer (used to detect overlap during the bisection).
-const simplex_buf = new Float64Array(12);
-
-// Scratch for MPR's MTV output — same vec3 convention as EPA: direction
-// is "A's interior into B" with the magnitude being the depth. We use
-// MPR for the contact-normal recovery at the kiss point because it
-// converges reliably on smooth-vs-smooth supports where EPA hits its
-// iteration cap before tightening (sphere-vs-sphere shallow overlap).
-const mpr_result = new Float64Array(3);
-
-/**
- * Long-lived PosedShape adapters for the swept shape and the
- * candidate's collider. Both are re-bound per query — the swept one
- * also gets its position field re-written every bisection step
- * without re-running `setup()`, since shape and rotation are
- * constant through the sweep.
- */
-const swept_posed = new PosedShape();
-const candidate_posed = new PosedShape();
-
-/**
- * Buffer for the union of static-BVH and dynamic-BVH candidates that
- * overlap the swept AABB. Grows by doubling on the rare overflow.
- * @type {Uint32Array}
- */
-let scratch_candidates = new Uint32Array(64);
-
-/**
- * Sweep a convex shape along a ray and find the first body it would
- * hit. Used by character controllers (sweep a capsule along intended
- * movement to find blockers), high-speed projectile movement (avoid
- * tunnelling without paying for full per-body CCD), and any kinematic
- * "where would this body end up if I tried to move it?" query.
- *
- * Pipeline:
- *   1. Build the swept AABB: the shape's local-frame AABB rotated
- *      into world by `rotation`, translated to `ray.origin`, then
- *      stretched along `ray.direction * ray.tMax`.
- *   2. Query both broadphase BVHs (static + dynamic) for leaves whose
- *      AABB overlaps the swept volume.
- *   3. For each candidate, bisect the [0, best_t] interval for the
- *      smallest `t` at which the swept shape overlaps the candidate's
- *      collider. GJK on each midpoint.
- *
- * Picking `best_t` (rather than always running the full [0, tMax]
- * bisection) is the early-termination optimisation: once we have a
- * hit at some t, no later candidate can produce a smaller TOI by
- * being checked at its full [0, tMax] interval.
- *
- * Output on hit:
- *   - `result.t` — sweep distance to impact (in the same units as
- *     `ray.direction`'s magnitude; for a unit-direction ray, this is
- *     metres).
- *   - `result.position` — `ray.origin + t * ray.direction`, i.e. the
- *     centre of the swept shape at the moment of first contact. NOT
- *     a point on the target's surface; that would need narrowphase
- *     refinement which the broadphase-only architecture doesn't have
- *     wired up yet.
- *   - `result.normal` — target surface's outward normal at the kiss
- *     point (unit length), computed by running GJK + EPA against the
- *     just-overlapping configuration at the TOI. Falls back to
- *     `-ray.direction` on the rare case EPA degenerates (NaN / zero
- *     depth) — see comment near the EPA call.
- *   - `result.entity` / `result.body_id` — the hit body.
- *
- * Output on miss: untouched; same convention as `raycast`.
- *
- * @param {PhysicsSystem} system
- * @param {Ray3} ray  origin + unit direction + `tMax`
- * @param {AbstractShape3D} shape  shape being swept, in its local frame
- * @param {{x:number,y:number,z:number,w:number}} rotation  fixed orientation
- * @param {PhysicsSurfacePoint} result  populated on hit; untouched on miss
- * @param {(entity:number, collider:Collider)=>boolean} [filter] mandatory in
- *   contract; defaults to {@link returnTrue}
- * @returns {boolean}
- */
-export function shape_cast(system, ray, shape, rotation, 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 tMax = ray.tMax;
-    if (!(tMax > 0) || !Number.isFinite(tMax)) return false;
-
-    const rx = rotation.x, ry = rotation.y, rz = rotation.z, rw = rotation.w;
-
-    // ── 1. Swept AABB ──────────────────────────────────────────────
-    shape.compute_bounding_box(local_aabb);
-    // Rotation around ray.origin gives the shape's initial AABB in
-    // world space.
-    aabb3_transform_oriented(
-        initial_aabb, 0,
-        local_aabb[0], local_aabb[1], local_aabb[2],
-        local_aabb[3], local_aabb[4], local_aabb[5],
-        ox, oy, oz,
-        rx, ry, rz, rw
-    );
-    // Stretch by the sweep displacement. Positive direction components
-    // push the +face out; negative components push the −face out.
-    swept_aabb[0] = initial_aabb[0];
-    swept_aabb[1] = initial_aabb[1];
-    swept_aabb[2] = initial_aabb[2];
-    swept_aabb[3] = initial_aabb[3];
-    swept_aabb[4] = initial_aabb[4];
-    swept_aabb[5] = initial_aabb[5];
-    if (dx > 0) swept_aabb[3] += dx * tMax; else swept_aabb[0] += dx * tMax;
-    if (dy > 0) swept_aabb[4] += dy * tMax; else swept_aabb[1] += dy * tMax;
-    if (dz > 0) swept_aabb[5] += dz * tMax; else swept_aabb[2] += dz * tMax;
-
-    // ── 2. Broadphase: gather candidates from both trees ───────────
-    // The BVH query returns the TRUE overlap count and silently drops writes past
-    // the buffer end. An undersized buffer would make the narrowphase loop read
-    // `undefined` candidates and silently MISS the true nearest hit (tunnelling on
-    // a long sweep through a dense scene). Grow by doubling and re-query until
-    // both fit (deterministic queries → converges in one resize).
-    let n_static, n_dynamic, n_total;
-    for (;;) {
-        n_static = bvh_query_user_data_overlaps_aabb(scratch_candidates, 0, system.staticBvh, swept_aabb);
-        n_dynamic = bvh_query_user_data_overlaps_aabb(scratch_candidates, n_static, system.dynamicBvh, swept_aabb);
-        n_total = n_static + n_dynamic;
-        if (n_total <= scratch_candidates.length) break;
-        scratch_candidates = new Uint32Array(Math.max(n_total, scratch_candidates.length * 2));
-    }
-    if (n_total === 0) return false;
-
-    // ── 3. Narrowphase: per-candidate bisection ────────────────────
-    //
-    // Re-bind the swept shape's PosedShape once for shape + rotation;
-    // we update `px/py/pz` per bisection midpoint without rerunning
-    // setup (px/py/pz are public fields by contract).
-    swept_posed.shape = shape;
-    swept_posed.qx = rx; swept_posed.qy = ry; swept_posed.qz = rz; swept_posed.qw = rw;
-
-    let best_t = tMax;
-    let best_body_id = -1;
-    let best_entity = -1;
-
-    for (let i = 0; i < n_total; i++) {
-        const body_id = scratch_candidates[i];
-        const body_idx = body_id_index(body_id);
-        const entity = system.entityOf(body_id);
-        if (entity < 0) continue;
-
-        const collider = system.__primary_collider(body_idx);
-        if (collider === null) continue;
-        if (!filter(entity, collider)) continue;
-
-        const target_tr = system.__transforms[body_idx];
-        candidate_posed.setup(collider.shape, target_tr.position, target_tr.rotation);
-
-        // Already overlapping at t = 0? Return immediately — can't beat
-        // this, and the bisection's "no overlap at lo, overlap at hi"
-        // invariant doesn't hold.
-        swept_posed.px = ox; swept_posed.py = oy; swept_posed.pz = oz;
-        if (gjk(simplex_buf, swept_posed, candidate_posed)) {
-            best_t = 0;
-            best_body_id = body_id;
-            best_entity = entity;
-            break;
-        }
-
-        // ── Slab intersection: narrow the search interval analytically ─
-        //
-        // Compute the body's tight world AABB and intersect it with the
-        // swept shape's AABB sliding along the sweep direction. The
-        // result is the t-interval `[t_aabb_enter, t_aabb_exit]` during
-        // which the two AABBs overlap. Outside this interval the actual
-        // shapes can't overlap either (AABB is a superset of the
-        // shape), so we get to skip all of [0, t_aabb_enter) and
-        // (t_aabb_exit, best_t] without testing.
-        //
-        // This is the key to making `shapeCast` scale to long sweeps:
-        // a 1 km cast of a 1 m sphere against another 1 m sphere has an
-        // AABB-overlap window of ≈ 2 m (the sum of diameters along the
-        // sweep axis), reducing the coarse-step grid from a 30 m
-        // resolution (1000/32) to 6 cm (2/32).
-        collider.shape.compute_bounding_box(target_local_aabb);
-        aabb3_transform_oriented(
-            target_world_aabb, 0,
-            target_local_aabb[0], target_local_aabb[1], target_local_aabb[2],
-            target_local_aabb[3], target_local_aabb[4], target_local_aabb[5],
-            target_tr.position.x, target_tr.position.y, target_tr.position.z,
-            target_tr.rotation.x, target_tr.rotation.y, target_tr.rotation.z, target_tr.rotation.w
-        );
-
-        let t_aabb_enter = -Infinity;
-        let t_aabb_exit = Infinity;
-        let skip_candidate = false;
-        for (let ax = 0; ax < 3; ax++) {
-            const init_min = initial_aabb[ax];
-            const init_max = initial_aabb[ax + 3];
-            const body_min = target_world_aabb[ax];
-            const body_max = target_world_aabb[ax + 3];
-            const d_ax = ax === 0 ? dx : (ax === 1 ? dy : dz);
-            if (d_ax === 0) {
-                // Static along this axis: AABBs must overlap in their
-                // initial position or this candidate is unreachable.
-                if (init_max < body_min || init_min > body_max) {
-                    skip_candidate = true;
-                    break;
-                }
-                continue;
-            }
-            // swept_min(t) = init_min + d·t, swept_max(t) = init_max + d·t.
-            // Slab-enter: swept_max(t) = body_min  →  t = (body_min − init_max) / d
-            // Slab-exit:  swept_min(t) = body_max  →  t = (body_max − init_min) / d
-            const t1 = (body_min - init_max) / d_ax;
-            const t2 = (body_max - init_min) / d_ax;
-            const lo = t1 < t2 ? t1 : t2;
-            const hi = t1 < t2 ? t2 : t1;
-            if (lo > t_aabb_enter) t_aabb_enter = lo;
-            if (hi < t_aabb_exit)  t_aabb_exit  = hi;
-        }
-        if (skip_candidate) continue;
-        if (t_aabb_enter > t_aabb_exit) continue;
-
-        // Clamp to the relevant t-window: [0, best_t]. Anything beyond
-        // best_t can't tighten the answer.
-        const t_start = t_aabb_enter > 0     ? t_aabb_enter : 0;
-        const t_end   = t_aabb_exit  < best_t ? t_aabb_exit  : best_t;
-        if (t_start >= t_end) continue;
-
-        // Coarse-search within the narrowed interval to find any t for
-        // which the shapes overlap (their AABBs do by construction; the
-        // shapes themselves may not, e.g. two spheres passing alongside).
-        const step_dt = (t_end - t_start) / COARSE_STEPS;
-        let t_prev = t_start;
-        let t_hi = -1;
-        for (let s = 1; s <= COARSE_STEPS; s++) {
-            const t = t_start + s * step_dt;
-            swept_posed.px = ox + dx * t;
-            swept_posed.py = oy + dy * t;
-            swept_posed.pz = oz + dz * t;
-            if (gjk(simplex_buf, swept_posed, candidate_posed)) {
-                t_hi = t;
-                break;
-            }
-            t_prev = t;
-        }
-        if (t_hi < 0) continue;
-
-        // Bisect [t_prev, t_hi]: invariant is `gjk(t_lo) === false` and
-        // `gjk(t_hi) === true`.
-        let t_lo = t_prev;
-        const stop_width = TOI_TOLERANCE * tMax;
-        for (let bi = 0; bi < MAX_BISECTION && (t_hi - t_lo) > stop_width; bi++) {
-            const t_mid = (t_lo + t_hi) * 0.5;
-            swept_posed.px = ox + dx * t_mid;
-            swept_posed.py = oy + dy * t_mid;
-            swept_posed.pz = oz + dz * t_mid;
-            if (gjk(simplex_buf, swept_posed, candidate_posed)) {
-                t_hi = t_mid;
-            } else {
-                t_lo = t_mid;
-            }
-        }
-
-        // Use t_lo (the just-SEPARATING side of the bisection) rather
-        // than t_hi (just-overlapping). With the fixed GJK that no
-        // longer flags 5 mm-clear gaps as overlap, the bisection is
-        // tight; reporting t_lo gives kinematic callers a conservative
-        // TOI — the swept shape at the reported moment is provably NOT
-        // overlapping the target. The normal-probe below steps a few
-        // mm past best_t to get a valid simplex for MPR.
-        if (t_lo < best_t) {
-            best_t = t_lo;
-            best_body_id = body_id;
-            best_entity = entity;
-        }
-    }
-
-    if (best_body_id === -1) return false;
-
-    const rp = result.position;
-    const rn = result.normal;
-
-    const swept_x = ox + dx * best_t;
-    const swept_y = oy + dy * best_t;
-    const swept_z = oz + dz * best_t;
-
-    rp[0] = swept_x;
-    rp[1] = swept_y;
-    rp[2] = swept_z;
-
-    // ── Contact-normal recovery via MPR ────────────────────────────
-    //
-    // Bisection converges with `swept_posed` somewhere within
-    // `stop_width = TOI_TOLERANCE * tMax` of the true contact. At
-    // exactly `best_t` the overlap depth is therefore sub-millimetre
-    // on practical sweeps. EPA on smooth-vs-smooth supports at this
-    // overlap can't tighten before its iteration cap and returns a
-    // direction off by ~30° from truth. MPR is the better tool here:
-    // it converges in 5-15 iterations on smooth surfaces, same MTV
-    // output convention as EPA (direction = A's interior into B,
-    // magnitude = depth).
-    //
-    // We still advance the swept centre by NORMAL_PROBE_OFFSET past
-    // best_t (a few mm into the target) before sampling — the extra
-    // overlap gives both MPR and the body-centre sign check a more
-    // robust geometric basis. The reported `result.t` stays at
-    // `best_t`; only the normal probe moves.
-    //
-    // Sign-validation: MPR's polytope can also settle on either side
-    // of the origin for highly symmetric configurations, so we dot
-    // against the body-centre axis and flip if needed (same pattern
-    // as `narrowphase_step`'s EPA path). The stored normal is the
-    // target's outward surface normal — `B → A` direction — i.e. the
-    // negated (validated) MTV.
-    const NORMAL_PROBE_OFFSET = 0.01;
-    const probe_t = best_t + NORMAL_PROBE_OFFSET;
-    const probe_x = ox + dx * probe_t;
-    const probe_y = oy + dy * probe_t;
-    const probe_z = oz + dz * probe_t;
-    const best_body_idx = body_id_index(best_body_id);
-    const best_collider = system.__primary_collider(best_body_idx);
-    let normal_x = -dx, normal_y = -dy, normal_z = -dz;
-    if (best_collider !== null) {
-        const best_tr = system.__transforms[best_body_idx];
-        candidate_posed.setup(best_collider.shape, best_tr.position, best_tr.rotation);
-        swept_posed.px = probe_x;
-        swept_posed.py = probe_y;
-        swept_posed.pz = probe_z;
-        if (mpr(mpr_result, 0, swept_posed, candidate_posed)) {
-            let ex = mpr_result[0], ey = mpr_result[1], ez = mpr_result[2];
-            const depth = Math.sqrt(ex * ex + ey * ey + ez * ez);
-            if (depth > 0 && Number.isFinite(depth)) {
-                const ab_x = best_tr.position.x - probe_x;
-                const ab_y = best_tr.position.y - probe_y;
-                const ab_z = best_tr.position.z - probe_z;
-                if (ex * ab_x + ey * ab_y + ez * ab_z < 0) {
-                    ex = -ex; ey = -ey; ez = -ez;
-                }
-                const inv = 1 / depth;
-                normal_x = -ex * inv;
-                normal_y = -ey * inv;
-                normal_z = -ez * inv;
-            }
-        }
-    }
-
-    rn[0] = normal_x; rn[1] = normal_y; rn[2] = normal_z;
-    result.t = best_t;
-    result.entity = best_entity;
-    result.body_id = best_body_id;
-    return true;
-}
+import { assert } from "../../../core/assert.js";
+import { bvh_query_user_data_overlaps_aabb } from "../../../core/bvh2/bvh3/query/bvh_query_user_data_overlaps_aabb.js";
+import { returnTrue } from "../../../core/function/returnTrue.js";
+import { aabb3_transform_oriented } from "../../../core/geom/3d/aabb/aabb3_transform_oriented.js";
+import { body_id_index } from "../body/BodyStorage.js";
+import { gjk } from "../../../core/geom/3d/gjk/gjk.js";
+import { mpr } from "../../../core/geom/3d/gjk/mpr.js";
+import { PosedShape3D } from "../../../core/geom/3d/shape/PosedShape3D.js";
+
+/**
+ * Bisection halts when the search interval is below this fraction of
+ * the sweep length. With `MAX_BISECTION = 32` and a unit-length sweep
+ * the achieved precision is ≈ 1 / 2³² (well below any practical
+ * world-scale tolerance), but we cap on relative tolerance so a
+ * 10 m sweep doesn't insist on sub-micron precision.
+ * @type {number}
+ */
+const TOI_TOLERANCE = 1e-4;
+
+/**
+ * Maximum bisection iterations. `log2(1 / TOI_TOLERANCE)` is enough
+ * for any practical sweep; this is the hard upper bound to prevent
+ * pathological inputs (NaN ray, etc.) from hanging the query.
+ * @type {number}
+ */
+const MAX_BISECTION = 32;
+
+/**
+ * Coarse linear-search resolution for finding *any* overlap moment
+ * along the per-candidate AABB-overlap interval (NOT the full sweep —
+ * see `aabb_overlap_interval` for the analytical narrowing that runs
+ * upstream). 32 substeps over the AABB-overlap window is dense enough
+ * for any sweep where the shape's smallest extent is on the order of
+ * its AABB's extent (true for spheres / boxes / capsules); the slab
+ * intersection keeps that interval small even for kilometre-scale
+ * sweeps, so this isn't the limit it would otherwise be.
+ * @type {number}
+ */
+const COARSE_STEPS = 32;
+
+const swept_aabb = new Float64Array(6);
+const local_aabb = new Float64Array(6);
+const initial_aabb = new Float64Array(6);
+const target_local_aabb = new Float64Array(6);
+const target_world_aabb = new Float64Array(6);
+
+// GJK simplex buffer (used to detect overlap during the bisection).
+const simplex_buf = new Float64Array(12);
+
+// Scratch for MPR's MTV output — same vec3 convention as EPA: direction
+// is "A's interior into B" with the magnitude being the depth. We use
+// MPR for the contact-normal recovery at the kiss point because it
+// converges reliably on smooth-vs-smooth supports where EPA hits its
+// iteration cap before tightening (sphere-vs-sphere shallow overlap).
+const mpr_result = new Float64Array(3);
+
+/**
+ * Long-lived PosedShape3D adapters for the swept shape and the
+ * candidate's collider. Both are re-bound per query — the swept one
+ * also gets its position field re-written every bisection step
+ * without re-running `setup()`, since shape and rotation are
+ * constant through the sweep.
+ */
+const swept_posed = new PosedShape3D();
+const candidate_posed = new PosedShape3D();
+
+/**
+ * Buffer for the union of static-BVH and dynamic-BVH candidates that
+ * overlap the swept AABB. Grows by doubling on the rare overflow.
+ * @type {Uint32Array}
+ */
+let scratch_candidates = new Uint32Array(64);
+
+/**
+ * Sweep a convex shape along a ray and find the first body it would
+ * hit. Used by character controllers (sweep a capsule along intended
+ * movement to find blockers), high-speed projectile movement (avoid
+ * tunnelling without paying for full per-body CCD), and any kinematic
+ * "where would this body end up if I tried to move it?" query.
+ *
+ * Pipeline:
+ *   1. Build the swept AABB: the shape's local-frame AABB rotated
+ *      into world by `rotation`, translated to `ray.origin`, then
+ *      stretched along `ray.direction * ray.tMax`.
+ *   2. Query both broadphase BVHs (static + dynamic) for leaves whose
+ *      AABB overlaps the swept volume.
+ *   3. For each candidate, bisect the [0, best_t] interval for the
+ *      smallest `t` at which the swept shape overlaps the candidate's
+ *      collider. GJK on each midpoint.
+ *
+ * Picking `best_t` (rather than always running the full [0, tMax]
+ * bisection) is the early-termination optimisation: once we have a
+ * hit at some t, no later candidate can produce a smaller TOI by
+ * being checked at its full [0, tMax] interval.
+ *
+ * Output on hit:
+ *   - `result.t` — sweep distance to impact (in the same units as
+ *     `ray.direction`'s magnitude; for a unit-direction ray, this is
+ *     metres).
+ *   - `result.position` — `ray.origin + t * ray.direction`, i.e. the
+ *     centre of the swept shape at the moment of first contact. NOT
+ *     a point on the target's surface; that would need narrowphase
+ *     refinement which the broadphase-only architecture doesn't have
+ *     wired up yet.
+ *   - `result.normal` — target surface's outward normal at the kiss
+ *     point (unit length), computed by running GJK + EPA against the
+ *     just-overlapping configuration at the TOI. Falls back to
+ *     `-ray.direction` on the rare case EPA degenerates (NaN / zero
+ *     depth) — see comment near the EPA call.
+ *   - `result.entity` / `result.body_id` — the hit body.
+ *
+ * Output on miss: untouched; same convention as `raycast`.
+ *
+ * @param {PhysicsSystem} system
+ * @param {Ray3|{origin_x:number,origin_y:number,origin_z:number,direction_x:number,direction_y:number,direction_z:number,tMax:number}} ray
+ *   origin + unit direction + `tMax`. Only the named accessors are read, so a
+ *   plain double-precision ray-like works — `Ray3` is Float32Array-backed and
+ *   quantises large-coordinate sweeps (the CCD pass passes a f64 ray-like)
+ * @param {AbstractShape3D} shape  shape being swept, in its local frame
+ * @param {{x:number,y:number,z:number,w:number}} rotation  fixed orientation
+ * @param {PhysicsSurfacePoint} result  populated on hit; untouched on miss
+ * @param {(entity:number, collider:Collider, body_id:number)=>boolean} [filter]
+ *   mandatory in contract; defaults to {@link returnTrue}. Receives the
+ *   candidate body's entity, primary collider, and packed body id (the id
+ *   distinguishes bodies when colliders live on child entities)
+ * @param {boolean} [skip_initial_overlaps=false] how a candidate ALREADY
+ *   overlapping the swept shape at `t = 0` is treated. `false` (kinematic
+ *   "can I move?" semantics): the cast is blocked where it stands — report a
+ *   hit at `t = 0`. `true` (CCD semantics): such a candidate is a resting /
+ *   sliding contact the discrete solver already owns, NOT a tunnelling
+ *   threat — skip it and keep scanning the remaining candidates for real
+ *   blockers along the sweep. Without the skip, one resting contact at the
+ *   start of the sweep would blind the cast to every other obstacle.
+ * @returns {boolean}
+ */
+export function shape_cast(system, ray, shape, rotation, result, filter = returnTrue, skip_initial_overlaps = false) {
+    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 tMax = ray.tMax;
+    // A fresh Ray3 defaults tMax to Infinity, which a sweep cannot use (the
+    // swept AABB and the bisection interval would be unbounded). Loud in dev
+    // — silently returning false for the DEFAULT ray construction reads as
+    // "nothing there" and is a debugging trap; prod degrades to a miss.
+    assert.ok(Number.isFinite(tMax) && tMax > 0,
+        `shape_cast: ray.tMax must be a positive finite sweep length, got ${tMax}`);
+    if (!(tMax > 0) || !Number.isFinite(tMax)) return false;
+
+    const rx = rotation.x, ry = rotation.y, rz = rotation.z, rw = rotation.w;
+
+    // ── 1. Swept AABB ──────────────────────────────────────────────
+    shape.compute_bounding_box(local_aabb);
+    // Rotation around ray.origin gives the shape's initial AABB in
+    // world space.
+    aabb3_transform_oriented(
+        initial_aabb, 0,
+        local_aabb[0], local_aabb[1], local_aabb[2],
+        local_aabb[3], local_aabb[4], local_aabb[5],
+        ox, oy, oz,
+        rx, ry, rz, rw
+    );
+    // Stretch by the sweep displacement. Positive direction components
+    // push the +face out; negative components push the −face out.
+    swept_aabb[0] = initial_aabb[0];
+    swept_aabb[1] = initial_aabb[1];
+    swept_aabb[2] = initial_aabb[2];
+    swept_aabb[3] = initial_aabb[3];
+    swept_aabb[4] = initial_aabb[4];
+    swept_aabb[5] = initial_aabb[5];
+    if (dx > 0) swept_aabb[3] += dx * tMax; else swept_aabb[0] += dx * tMax;
+    if (dy > 0) swept_aabb[4] += dy * tMax; else swept_aabb[1] += dy * tMax;
+    if (dz > 0) swept_aabb[5] += dz * tMax; else swept_aabb[2] += dz * tMax;
+
+    // ── 2. Broadphase: gather candidates from both trees ───────────
+    // The BVH query returns the TRUE overlap count and silently drops writes past
+    // the buffer end. An undersized buffer would make the narrowphase loop read
+    // `undefined` candidates and silently MISS the true nearest hit (tunnelling on
+    // a long sweep through a dense scene). Grow by doubling and re-query until
+    // both fit (deterministic queries → converges in one resize).
+    let n_static, n_dynamic, n_total;
+    for (;;) {
+        n_static = bvh_query_user_data_overlaps_aabb(scratch_candidates, 0, system.staticBvh, swept_aabb);
+        n_dynamic = bvh_query_user_data_overlaps_aabb(scratch_candidates, n_static, system.dynamicBvh, swept_aabb);
+        n_total = n_static + n_dynamic;
+        if (n_total <= scratch_candidates.length) break;
+        scratch_candidates = new Uint32Array(Math.max(n_total, scratch_candidates.length * 2));
+    }
+    if (n_total === 0) return false;
+
+    // ── 3. Narrowphase: per-candidate bisection ────────────────────
+    //
+    // Re-bind the swept shape's PosedShape3D once for shape + rotation;
+    // we update `px/py/pz` per bisection midpoint without rerunning
+    // setup (px/py/pz are public fields by contract).
+    swept_posed.shape = shape;
+    swept_posed.qx = rx; swept_posed.qy = ry; swept_posed.qz = rz; swept_posed.qw = rw;
+
+    let best_t = tMax;
+    let best_body_id = -1;
+    let best_entity = -1;
+
+    for (let i = 0; i < n_total; i++) {
+        const body_id = scratch_candidates[i];
+        const body_idx = body_id_index(body_id);
+        const entity = system.entityOf(body_id);
+        if (entity < 0) continue;
+
+        const collider = system.__primary_collider(body_idx);
+        if (collider === null) continue;
+        if (!filter(entity, collider, body_id)) continue;
+
+        const target_tr = system.__transforms[body_idx];
+        candidate_posed.setup(collider.shape, target_tr.position, target_tr.rotation);
+
+        // Already overlapping at t = 0? The bisection's "no overlap at lo,
+        // overlap at hi" invariant doesn't hold for this candidate. Either
+        // it IS the answer (kinematic semantics: blocked where it stands,
+        // t = 0 — nothing can beat that, stop scanning) or it is a resting
+        // contact to step over (CCD semantics: skip THIS candidate only;
+        // the remaining candidates must still be scanned for real blockers).
+        swept_posed.px = ox; swept_posed.py = oy; swept_posed.pz = oz;
+        if (gjk(simplex_buf, swept_posed, candidate_posed)) {
+            if (skip_initial_overlaps) continue;
+            best_t = 0;
+            best_body_id = body_id;
+            best_entity = entity;
+            break;
+        }
+
+        // ── Slab intersection: narrow the search interval analytically ─
+        //
+        // Compute the body's tight world AABB and intersect it with the
+        // swept shape's AABB sliding along the sweep direction. The
+        // result is the t-interval `[t_aabb_enter, t_aabb_exit]` during
+        // which the two AABBs overlap. Outside this interval the actual
+        // shapes can't overlap either (AABB is a superset of the
+        // shape), so we get to skip all of [0, t_aabb_enter) and
+        // (t_aabb_exit, best_t] without testing.
+        //
+        // This is the key to making `shapeCast` scale to long sweeps:
+        // a 1 km cast of a 1 m sphere against another 1 m sphere has an
+        // AABB-overlap window of ≈ 2 m (the sum of diameters along the
+        // sweep axis), reducing the coarse-step grid from a 30 m
+        // resolution (1000/32) to 6 cm (2/32).
+        collider.shape.compute_bounding_box(target_local_aabb);
+        aabb3_transform_oriented(
+            target_world_aabb, 0,
+            target_local_aabb[0], target_local_aabb[1], target_local_aabb[2],
+            target_local_aabb[3], target_local_aabb[4], target_local_aabb[5],
+            target_tr.position.x, target_tr.position.y, target_tr.position.z,
+            target_tr.rotation.x, target_tr.rotation.y, target_tr.rotation.z, target_tr.rotation.w
+        );
+
+        let t_aabb_enter = -Infinity;
+        let t_aabb_exit = Infinity;
+        let skip_candidate = false;
+        for (let ax = 0; ax < 3; ax++) {
+            const init_min = initial_aabb[ax];
+            const init_max = initial_aabb[ax + 3];
+            const body_min = target_world_aabb[ax];
+            const body_max = target_world_aabb[ax + 3];
+            const d_ax = ax === 0 ? dx : (ax === 1 ? dy : dz);
+            if (d_ax === 0) {
+                // Static along this axis: AABBs must overlap in their
+                // initial position or this candidate is unreachable.
+                if (init_max < body_min || init_min > body_max) {
+                    skip_candidate = true;
+                    break;
+                }
+                continue;
+            }
+            // swept_min(t) = init_min + d·t, swept_max(t) = init_max + d·t.
+            // Slab-enter: swept_max(t) = body_min  →  t = (body_min − init_max) / d
+            // Slab-exit:  swept_min(t) = body_max  →  t = (body_max − init_min) / d
+            const t1 = (body_min - init_max) / d_ax;
+            const t2 = (body_max - init_min) / d_ax;
+            const lo = t1 < t2 ? t1 : t2;
+            const hi = t1 < t2 ? t2 : t1;
+            if (lo > t_aabb_enter) t_aabb_enter = lo;
+            if (hi < t_aabb_exit)  t_aabb_exit  = hi;
+        }
+        if (skip_candidate) continue;
+        if (t_aabb_enter > t_aabb_exit) continue;
+
+        // Clamp to the relevant t-window: [0, best_t]. Anything beyond
+        // best_t can't tighten the answer.
+        const t_start = t_aabb_enter > 0     ? t_aabb_enter : 0;
+        const t_end   = t_aabb_exit  < best_t ? t_aabb_exit  : best_t;
+        if (t_start >= t_end) continue;
+
+        // Coarse-search within the narrowed interval to find any t for
+        // which the shapes overlap (their AABBs do by construction; the
+        // shapes themselves may not, e.g. two spheres passing alongside).
+        const step_dt = (t_end - t_start) / COARSE_STEPS;
+        let t_prev = t_start;
+        let t_hi = -1;
+        for (let s = 1; s <= COARSE_STEPS; s++) {
+            const t = t_start + s * step_dt;
+            swept_posed.px = ox + dx * t;
+            swept_posed.py = oy + dy * t;
+            swept_posed.pz = oz + dz * t;
+            if (gjk(simplex_buf, swept_posed, candidate_posed)) {
+                t_hi = t;
+                break;
+            }
+            t_prev = t;
+        }
+        if (t_hi < 0) continue;
+
+        // Bisect [t_prev, t_hi]: invariant is `gjk(t_lo) === false` and
+        // `gjk(t_hi) === true`.
+        let t_lo = t_prev;
+        const stop_width = TOI_TOLERANCE * tMax;
+        for (let bi = 0; bi < MAX_BISECTION && (t_hi - t_lo) > stop_width; bi++) {
+            const t_mid = (t_lo + t_hi) * 0.5;
+            swept_posed.px = ox + dx * t_mid;
+            swept_posed.py = oy + dy * t_mid;
+            swept_posed.pz = oz + dz * t_mid;
+            if (gjk(simplex_buf, swept_posed, candidate_posed)) {
+                t_hi = t_mid;
+            } else {
+                t_lo = t_mid;
+            }
+        }
+
+        // Use t_lo (the just-SEPARATING side of the bisection) rather
+        // than t_hi (just-overlapping). With the fixed GJK that no
+        // longer flags 5 mm-clear gaps as overlap, the bisection is
+        // tight; reporting t_lo gives kinematic callers a conservative
+        // TOI — the swept shape at the reported moment is provably NOT
+        // overlapping the target. The normal-probe below steps a few
+        // mm past best_t to get a valid simplex for MPR.
+        if (t_lo < best_t) {
+            best_t = t_lo;
+            best_body_id = body_id;
+            best_entity = entity;
+        }
+    }
+
+    if (best_body_id === -1) return false;
+
+    const rp = result.position;
+    const rn = result.normal;
+
+    const swept_x = ox + dx * best_t;
+    const swept_y = oy + dy * best_t;
+    const swept_z = oz + dz * best_t;
+
+    rp[0] = swept_x;
+    rp[1] = swept_y;
+    rp[2] = swept_z;
+
+    // ── Contact-normal recovery via MPR ────────────────────────────
+    //
+    // Bisection converges with `swept_posed` somewhere within
+    // `stop_width = TOI_TOLERANCE * tMax` of the true contact. At
+    // exactly `best_t` the overlap depth is therefore sub-millimetre
+    // on practical sweeps. EPA on smooth-vs-smooth supports at this
+    // overlap can't tighten before its iteration cap and returns a
+    // direction off by ~30° from truth. MPR is the better tool here:
+    // it converges in 5-15 iterations on smooth surfaces, same MTV
+    // output convention as EPA (direction = A's interior into B,
+    // magnitude = depth).
+    //
+    // We still advance the swept centre by NORMAL_PROBE_OFFSET past
+    // best_t (a few mm into the target) before sampling — the extra
+    // overlap gives both MPR and the body-centre sign check a more
+    // robust geometric basis. The reported `result.t` stays at
+    // `best_t`; only the normal probe moves.
+    //
+    // Sign-validation: MPR's polytope can also settle on either side
+    // of the origin for highly symmetric configurations, so we dot
+    // against the body-centre axis and flip if needed (same pattern
+    // as `narrowphase_step`'s EPA path). The stored normal is the
+    // target's outward surface normal — `B → A` direction — i.e. the
+    // negated (validated) MTV.
+    const NORMAL_PROBE_OFFSET = 0.01;
+    const probe_t = best_t + NORMAL_PROBE_OFFSET;
+    const probe_x = ox + dx * probe_t;
+    const probe_y = oy + dy * probe_t;
+    const probe_z = oz + dz * probe_t;
+    const best_body_idx = body_id_index(best_body_id);
+    const best_collider = system.__primary_collider(best_body_idx);
+    let normal_x = -dx, normal_y = -dy, normal_z = -dz;
+    if (best_collider !== null) {
+        const best_tr = system.__transforms[best_body_idx];
+        candidate_posed.setup(best_collider.shape, best_tr.position, best_tr.rotation);
+        swept_posed.px = probe_x;
+        swept_posed.py = probe_y;
+        swept_posed.pz = probe_z;
+        if (mpr(mpr_result, 0, swept_posed, candidate_posed)) {
+            let ex = mpr_result[0], ey = mpr_result[1], ez = mpr_result[2];
+            const depth = Math.sqrt(ex * ex + ey * ey + ez * ez);
+            if (depth > 0 && Number.isFinite(depth)) {
+                const ab_x = best_tr.position.x - probe_x;
+                const ab_y = best_tr.position.y - probe_y;
+                const ab_z = best_tr.position.z - probe_z;
+                if (ex * ab_x + ey * ab_y + ez * ab_z < 0) {
+                    ex = -ex; ey = -ey; ez = -ez;
+                }
+                const inv = 1 / depth;
+                normal_x = -ex * inv;
+                normal_y = -ey * inv;
+                normal_z = -ez * inv;
+            }
+        }
+    }
+
+    rn[0] = normal_x; rn[1] = normal_y; rn[2] = normal_z;
+    result.t = best_t;
+    result.entity = best_entity;
+    result.body_id = best_body_id;
+    return true;
+}