@woosh/meep-engine 2.148.0 → 2.149.0

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

@@ -1,201 +1,201 @@
-import {
-    COLUMN_CHILD_1,
-    COLUMN_CHILD_2,
-    COLUMN_USER_DATA,
-    ELEMENT_WORD_COUNT,
-    NULL_NODE,
-} from "../../../core/bvh2/bvh3/BVH.js";
-import { SCRATCH_UINT32_TRAVERSAL_STACK } from "../../../core/collection/SCRATCH_UINT32_TRAVERSAL_STACK.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 { body_id_index } from "../body/BodyStorage.js";
-import { RAY_REFINE_UNSUPPORTED, refine_ray_hit } from "../narrowphase/refine_ray_hit.js";
-
-const stack = SCRATCH_UINT32_TRAVERSAL_STACK;
-
-/**
- * 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.
- *
- * 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 {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 {(entity:number, collider:Collider)=>boolean} filter
- */
-function bvh_raycast_nearest(
-    bvh, root, system,
-    ox, oy, oz, dx, dy, dz,
-    inv_dx, inv_dy, inv_dz,
-    max_distance, filter
-) {
-    if (root === NULL_NODE) return;
-
-    const float32 = bvh.__data_float32;
-    const uint32 = bvh.__data_uint32;
-
-    let pointer = stack.pointer;
-    const stack_top = pointer;
-    stack[pointer++] = root;
-
-    while (pointer > stack_top) {
-        pointer--;
-        const node = stack[pointer];
-        const address = node * ELEMENT_WORD_COUNT;
-
-        const t_near = aabb3_near_distance_to_intersection_ray_segment(
-            float32[address], float32[address + 1], float32[address + 2],
-            float32[address + 3], float32[address + 4], float32[address + 5],
-            ox, oy, oz,
-            inv_dx, inv_dy, inv_dz,
-            0, max_distance
-        );
-
-        // No intersection or this subtree can't beat the best known hit — prune.
-        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;
-            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 = body_id_index(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;
-                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;
-}
-
-/**
- * Raycast against both broadphase trees (static + dynamic) of a
- * {@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.
- *
- * 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] 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, 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;
-    best_normal[0] = 0; best_normal[1] = 0; best_normal[2] = 0;
-
-    const inv_dx = 1 / dx, inv_dy = 1 / dy, inv_dz = 1 / dz;
-
-    bvh_raycast_nearest(
-        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, system,
-        ox, oy, oz, dx, dy, dz, inv_dx, inv_dy, inv_dz, max_distance, filter
-    );
-
-    // 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) return false; // body unlinked concurrently → treat as miss
-
-    const t = acc.best_t;
-    const rp = result.position;
-    const rn = result.normal;
-    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;
-
-    return true;
-}
+import {
+    COLUMN_CHILD_1,
+    COLUMN_CHILD_2,
+    COLUMN_USER_DATA,
+    ELEMENT_WORD_COUNT,
+    NULL_NODE,
+} from "../../../core/bvh2/bvh3/BVH.js";
+import { SCRATCH_UINT32_TRAVERSAL_STACK } from "../../../core/collection/SCRATCH_UINT32_TRAVERSAL_STACK.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 { body_id_index } from "../body/BodyStorage.js";
+import { RAY_REFINE_UNSUPPORTED, refine_ray_hit } from "../narrowphase/refine_ray_hit.js";
+
+const stack = SCRATCH_UINT32_TRAVERSAL_STACK;
+
+/**
+ * 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.
+ *
+ * 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 {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 {(entity:number, collider:Collider)=>boolean} filter
+ */
+function bvh_raycast_nearest(
+    bvh, root, system,
+    ox, oy, oz, dx, dy, dz,
+    inv_dx, inv_dy, inv_dz,
+    max_distance, filter
+) {
+    if (root === NULL_NODE) return;
+
+    const float32 = bvh.__data_float32;
+    const uint32 = bvh.__data_uint32;
+
+    let pointer = stack.pointer;
+    const stack_top = pointer;
+    stack[pointer++] = root;
+
+    while (pointer > stack_top) {
+        pointer--;
+        const node = stack[pointer];
+        const address = node * ELEMENT_WORD_COUNT;
+
+        const t_near = aabb3_near_distance_to_intersection_ray_segment(
+            float32[address], float32[address + 1], float32[address + 2],
+            float32[address + 3], float32[address + 4], float32[address + 5],
+            ox, oy, oz,
+            inv_dx, inv_dy, inv_dz,
+            0, max_distance
+        );
+
+        // No intersection or this subtree can't beat the best known hit — prune.
+        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;
+            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 = body_id_index(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;
+                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;
+}
+
+/**
+ * Raycast against both broadphase trees (static + dynamic) of a
+ * {@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.
+ *
+ * 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] 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, 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;
+    best_normal[0] = 0; best_normal[1] = 0; best_normal[2] = 0;
+
+    const inv_dx = 1 / dx, inv_dy = 1 / dy, inv_dz = 1 / dz;
+
+    bvh_raycast_nearest(
+        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, system,
+        ox, oy, oz, dx, dy, dz, inv_dx, inv_dy, inv_dz, max_distance, filter
+    );
+
+    // 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) return false; // body unlinked concurrently → treat as miss
+
+    const t = acc.best_t;
+    const rp = result.position;
+    const rn = result.normal;
+    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;
+
+    return true;
+}