@woosh/meep-engine 2.148.0 → 2.150.0

package/src/engine/navigation/mesh/bvh_query_nearest_face.js

@@ -1,131 +1,164 @@
-import { assert } from "../../../core/assert.js";
-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 {
-    aabb3_unsigned_distance_sqr_to_point
-} from "../../../core/geom/3d/aabb/aabb3_unsigned_distance_sqr_to_point.js";
-import { NULL_POINTER } from "../../../core/geom/3d/topology/struct/binary/BinaryTopology.js";
-import {
-    computeTriangleClosestPointToPointBarycentric
-} from "../../../core/geom/3d/triangle/computeTriangleClosestPointToPointBarycentric.js";
-
-const stack = SCRATCH_UINT32_TRAVERSAL_STACK;
-
-const scratch_barycentric = new Float32Array(2);
-const scratch_coord_a = new Float32Array(3);
-const scratch_coord_b = new Float32Array(3);
-const scratch_coord_c = new Float32Array(3);
-
-/**
- * Find the face of `mesh` whose surface is closest to the given point.
- * Expects `bvh` to have been built by {@link bvh_build_from_bt_mesh}, so each leaf's user data is a face ID.
- * Faces are expected to be triangles (as produced by the navmesh build pipeline).
- *
- * @param {BVH} bvh
- * @param {BinaryTopology} mesh
- * @param {number} x
- * @param {number} y
- * @param {number} z
- * @param {number} [max_distance=Infinity] optional cutoff, only faces within this distance are considered
- * @returns {number} face ID of the nearest face, or {@link NULL_POINTER} if no face was found within the cutoff
- */
-export function bvh_query_nearest_face(bvh, mesh, x, y, z, max_distance = Infinity) {
-    assert.defined(bvh, "bvh");
-    assert.defined(mesh, "mesh");
-    assert.equal(mesh.isBinaryTopology, true, "mesh.isBinaryTopology !== true");
-
-    const root = bvh.root;
-
-    if (root === NULL_NODE) {
-        return NULL_POINTER;
-    }
-
-    const stack_top = stack.pointer++;
-
-    stack[stack_top] = root;
-
-    let best_face = NULL_POINTER;
-    let best_distance_sqr = max_distance * max_distance;
-
-    const float32 = bvh.__data_float32;
-    const uint32 = bvh.__data_uint32;
-
-    do {
-        stack.pointer--;
-
-        const node = stack[stack.pointer];
-        const address = node * ELEMENT_WORD_COUNT;
-
-        // lower bound on distance via AABB
-        const aabb_distance_sqr = aabb3_unsigned_distance_sqr_to_point(
-            float32[address], float32[address + 1], float32[address + 2],
-            float32[address + 3], float32[address + 4], float32[address + 5],
-            x, y, z
-        );
-
-        if (aabb_distance_sqr >= best_distance_sqr) {
-            // this subtree cannot contain a closer face
-            continue;
-        }
-
-        const child_1 = uint32[address + COLUMN_CHILD_1];
-
-        if (child_1 !== NULL_NODE) {
-            // internal node, descend into both children
-            stack[stack.pointer++] = child_1;
-            stack[stack.pointer++] = uint32[address + COLUMN_CHILD_2];
-            continue;
-        }
-
-        // leaf node: refine with actual triangle distance
-        const face_id = uint32[address + COLUMN_USER_DATA];
-
-        const loop_a = mesh.face_read_loop(face_id);
-
-        if (loop_a === NULL_POINTER) {
-            continue;
-        }
-
-        const loop_b = mesh.loop_read_next(loop_a);
-        const loop_c = mesh.loop_read_next(loop_b);
-
-        mesh.vertex_read_coordinate(scratch_coord_a, 0, mesh.loop_read_vertex(loop_a));
-        mesh.vertex_read_coordinate(scratch_coord_b, 0, mesh.loop_read_vertex(loop_b));
-        mesh.vertex_read_coordinate(scratch_coord_c, 0, mesh.loop_read_vertex(loop_c));
-
-        computeTriangleClosestPointToPointBarycentric(
-            scratch_barycentric, 0,
-            x, y, z,
-            scratch_coord_a[0], scratch_coord_a[1], scratch_coord_a[2],
-            scratch_coord_b[0], scratch_coord_b[1], scratch_coord_b[2],
-            scratch_coord_c[0], scratch_coord_c[1], scratch_coord_c[2]
-        );
-
-        const weight_a = scratch_barycentric[0];
-        const weight_b = scratch_barycentric[1];
-        const weight_c = 1 - weight_a - weight_b;
-
-        const contact_x = scratch_coord_a[0] * weight_a + scratch_coord_b[0] * weight_b + scratch_coord_c[0] * weight_c;
-        const contact_y = scratch_coord_a[1] * weight_a + scratch_coord_b[1] * weight_b + scratch_coord_c[1] * weight_c;
-        const contact_z = scratch_coord_a[2] * weight_a + scratch_coord_b[2] * weight_b + scratch_coord_c[2] * weight_c;
-
-        const dx = contact_x - x;
-        const dy = contact_y - y;
-        const dz = contact_z - z;
-
-        const triangle_distance_sqr = dx * dx + dy * dy + dz * dz;
-
-        if (triangle_distance_sqr < best_distance_sqr) {
-            best_distance_sqr = triangle_distance_sqr;
-            best_face = face_id;
-        }
-    } while (stack.pointer > stack_top);
-
-    return best_face;
-}
+import { assert } from "../../../core/assert.js";
+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 {
+    aabb3_unsigned_distance_sqr_to_point
+} from "../../../core/geom/3d/aabb/aabb3_unsigned_distance_sqr_to_point.js";
+import { NULL_POINTER } from "../../../core/geom/3d/topology/struct/binary/BinaryTopology.js";
+import {
+    computeTriangleClosestPointToPointBarycentric
+} from "../../../core/geom/3d/triangle/computeTriangleClosestPointToPointBarycentric.js";
+
+const stack = SCRATCH_UINT32_TRAVERSAL_STACK;
+
+// single reusable coordinate scratch; callers unpack into locals between the calls that fill it
+const scratch_coords = new Float32Array(3);
+
+/**
+ * Find the face of `mesh` whose surface is closest to the given point.
+ * Expects `bvh` to have been built by {@link bvh_build_from_bt_mesh}, so each leaf's user data is a face ID.
+ * Faces are expected to be triangles (as produced by the navmesh build pipeline).
+ *
+ * @param {BVH} bvh
+ * @param {BinaryTopology} mesh
+ * @param {number} x
+ * @param {number} y
+ * @param {number} z
+ * @param {Float32Array|number[]} out_point the closest point ON the winning face's surface (the snapped
+ *        contact point) is written here when a face is found. Required, must not be null.
+ * @param {number} [out_point_offset=0] offset into `out_point` to write the snapped XYZ triple
+ * @param {number} [max_distance=Infinity] optional cutoff, only faces within this distance are considered
+ * @returns {number} face ID of the nearest face, or {@link NULL_POINTER} if no face was found within the cutoff
+ */
+export function bvh_query_nearest_face(
+    bvh,
+    mesh,
+    x, y, z,
+    out_point,
+    out_point_offset = 0,
+    max_distance = Infinity
+) {
+
+    assert.defined(bvh, "bvh");
+    assert.defined(mesh, "mesh");
+    assert.notNull(out_point, "out_point");
+    assert.defined(out_point, "out_point");
+    assert.equal(mesh.isBinaryTopology, true, "mesh.isBinaryTopology !== true");
+
+    const root = bvh.root;
+
+    if (root === NULL_NODE) {
+        return NULL_POINTER;
+    }
+
+    const stack_top = stack.pointer++;
+
+    stack[stack_top] = root;
+
+    let best_face = NULL_POINTER;
+    let best_distance_sqr = max_distance * max_distance;
+
+    // closest point on the winning face's surface, tracked so callers can snap a query point onto the mesh
+    let best_contact_x = 0;
+    let best_contact_y = 0;
+    let best_contact_z = 0;
+
+    const float32 = bvh.__data_float32;
+    const uint32 = bvh.__data_uint32;
+
+    do {
+        stack.pointer--;
+
+        const node = stack[stack.pointer];
+        const address = node * ELEMENT_WORD_COUNT;
+
+        // lower bound on distance via AABB
+        const aabb_distance_sqr = aabb3_unsigned_distance_sqr_to_point(
+            float32[address], float32[address + 1], float32[address + 2],
+            float32[address + 3], float32[address + 4], float32[address + 5],
+            x, y, z
+        );
+
+        if (aabb_distance_sqr >= best_distance_sqr) {
+            // this subtree cannot contain a closer face
+            continue;
+        }
+
+        const child_1 = uint32[address + COLUMN_CHILD_1];
+
+        if (child_1 !== NULL_NODE) {
+            // internal node, descend into both children
+            stack[stack.pointer++] = child_1;
+            stack[stack.pointer++] = uint32[address + COLUMN_CHILD_2];
+            continue;
+        }
+
+        // leaf node: refine with actual triangle distance
+        const face_id = uint32[address + COLUMN_USER_DATA];
+
+        const loop_a = mesh.face_read_loop(face_id);
+
+        if (loop_a === NULL_POINTER) {
+            continue;
+        }
+
+        const loop_b = mesh.loop_read_next(loop_a);
+        const loop_c = mesh.loop_read_next(loop_b);
+
+        // read each vertex into the shared scratch, unpacking into locals before the next read overwrites it
+        mesh.vertex_read_coordinate(scratch_coords, 0, mesh.loop_read_vertex(loop_a));
+        const ax = scratch_coords[0], ay = scratch_coords[1], az = scratch_coords[2];
+
+        mesh.vertex_read_coordinate(scratch_coords, 0, mesh.loop_read_vertex(loop_b));
+        const bx = scratch_coords[0], by = scratch_coords[1], bz = scratch_coords[2];
+
+        mesh.vertex_read_coordinate(scratch_coords, 0, mesh.loop_read_vertex(loop_c));
+        const cx = scratch_coords[0], cy = scratch_coords[1], cz = scratch_coords[2];
+
+        // reuse the same scratch for the barycentric output (only the first two slots are written)
+        computeTriangleClosestPointToPointBarycentric(
+            scratch_coords, 0,
+            x, y, z,
+            ax, ay, az,
+            bx, by, bz,
+            cx, cy, cz
+        );
+
+        const weight_a = scratch_coords[0];
+        const weight_b = scratch_coords[1];
+        const weight_c = 1 - weight_a - weight_b;
+
+        const contact_x = ax * weight_a + bx * weight_b + cx * weight_c;
+        const contact_y = ay * weight_a + by * weight_b + cy * weight_c;
+        const contact_z = az * weight_a + bz * weight_b + cz * weight_c;
+
+        const dx = contact_x - x;
+        const dy = contact_y - y;
+        const dz = contact_z - z;
+
+        const triangle_distance_sqr = dx * dx + dy * dy + dz * dz;
+
+        if (triangle_distance_sqr < best_distance_sqr) {
+            best_distance_sqr = triangle_distance_sqr;
+            best_face = face_id;
+
+            best_contact_x = contact_x;
+            best_contact_y = contact_y;
+            best_contact_z = contact_z;
+        }
+    } while (stack.pointer > stack_top);
+
+    if (best_face !== NULL_POINTER) {
+        out_point[out_point_offset] = best_contact_x;
+        out_point[out_point_offset + 1] = best_contact_y;
+        out_point[out_point_offset + 2] = best_contact_z;
+    }
+
+    return best_face;
+}

package/src/engine/physics/broadphase/generate_pairs.js

@@ -1,110 +1,110 @@
-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";
-
-const scratch_aabb = new Float64Array(6);
-const candidates = new Uint32Array(1024);
-
-/**
- * Iterate every awake body, query both BVHs from each of its attached
- * collider leaves, dedupe via the {@link ManifoldStore}, optionally gate
- * each candidate through a user-supplied filter, and append each surviving
- * body-pair to `pair_list_out`.
- *
- * Compound-body aware: a body with multiple colliders has multiple BVH
- * leaves (one per collider). Each leaf's AABB query may turn up the same
- * other-body multiple times (across the multiple leaves on either side)
- * but the per-slot touched flag collapses those into one entry in the
- * canonical `(min, max)` pair list — narrowphase then resolves the
- * collider-cross-product internally.
- *
- * Determinism: pair encounter order is fully determined by the awake-list
- * order × per-body collider-list order × BVH leaf traversal order. All
- * three are deterministic functions of the link/unlink sequence and the
- * BVH state, so two worlds with the same scene history produce identical
- * pair lists.
- *
- * @param {BodyStorage} storage
- * @param {BVH} dynamic_bvh
- * @param {BVH} static_bvh
- * @param {ManifoldStore} manifolds
- * @param {Array<Array<{collider: Collider, transform: Transform, entity: number, bvhNode: number}>>} body_collider_lists
- * @param {PairList} pair_list_out cleared and filled
- * @param {((idA: number, idB: number) => boolean) | null} pair_filter optional
- *        gate. Returning `false` rejects the pair entirely — no manifold
- *        slot, no narrowphase, no contact events. Existing cached
- *        manifolds for a rejected pair time out naturally via the
- *        manifold-store grace counter.
- * @param {*} [pair_filter_this_arg]
- */
-export function generate_pairs(
-    storage,
-    dynamic_bvh,
-    static_bvh,
-    manifolds,
-    body_collider_lists,
-    pair_list_out,
-    pair_filter = null,
-    pair_filter_this_arg = null,
-) {
-    pair_list_out.clear();
-
-    const awake_count = storage.awake_count;
-
-    for (let i = 0; i < awake_count; i++) {
-        const body_idx = storage.awake_at(i);
-        const list = body_collider_lists[body_idx];
-        if (list === undefined || list.length === 0) continue;
-
-        const gen = storage.generation_at(body_idx);
-        const my_packed = (body_idx << 8) | gen;
-
-        for (let k = 0; k < list.length; k++) {
-            const node = list[k].bvhNode;
-            dynamic_bvh.node_get_aabb(node, scratch_aabb);
-
-            // Dynamic ↔ Dynamic.
-            let n = bvh_query_user_data_overlaps_aabb(
-                candidates, 0,
-                dynamic_bvh,
-                scratch_aabb
-            );
-            // The BVH query writes leaves unconditionally — at capacity it both
-            // drops leaves (typed-array OOB writes no-op) AND returns a count
-            // past the buffer end, so the loop below would read `undefined`
-            // candidates and build garbage pairs. Guard the buffer size.
-            assert.lessThan(n, candidates.length, 'generate_pairs: dynamic broadphase overflowed the candidate buffer');
-            for (let c = 0; c < n; c++) {
-                const other = candidates[c];
-                if (other === my_packed) continue;
-
-                const idA = my_packed < other ? my_packed : other;
-                const idB = my_packed < other ? other : my_packed;
-
-                const existing = manifolds.find(idA, idB);
-                if (existing !== -1 && manifolds.is_touched(existing)) continue;
-                if (pair_filter !== null && !pair_filter.call(pair_filter_this_arg, idA, idB)) continue;
-                manifolds.acquire(idA, idB);
-                pair_list_out.push(idA, idB);
-            }
-
-            // Dynamic ↔ Static.
-            n = bvh_query_user_data_overlaps_aabb(
-                candidates, 0,
-                static_bvh,
-                scratch_aabb
-            );
-            assert.lessThan(n, candidates.length, 'generate_pairs: static broadphase overflowed the candidate buffer');
-            for (let c = 0; c < n; c++) {
-                const other = candidates[c];
-                const idA = my_packed < other ? my_packed : other;
-                const idB = my_packed < other ? other : my_packed;
-
-                const existing = manifolds.find(idA, idB);
-                if (existing !== -1 && manifolds.is_touched(existing)) continue;
-                if (pair_filter !== null && !pair_filter.call(pair_filter_this_arg, idA, idB)) continue;
-                manifolds.acquire(idA, idB);
-                pair_list_out.push(idA, idB);
-            }
-        }
-    }
-}
+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";
+
+const scratch_aabb = new Float64Array(6);
+const candidates = new Uint32Array(1024);
+
+/**
+ * Iterate every awake body, query both BVHs from each of its attached
+ * collider leaves, dedupe via the {@link ManifoldStore}, optionally gate
+ * each candidate through a user-supplied filter, and append each surviving
+ * body-pair to `pair_list_out`.
+ *
+ * Compound-body aware: a body with multiple colliders has multiple BVH
+ * leaves (one per collider). Each leaf's AABB query may turn up the same
+ * other-body multiple times (across the multiple leaves on either side)
+ * but the per-slot touched flag collapses those into one entry in the
+ * canonical `(min, max)` pair list — narrowphase then resolves the
+ * collider-cross-product internally.
+ *
+ * Determinism: pair encounter order is fully determined by the awake-list
+ * order × per-body collider-list order × BVH leaf traversal order. All
+ * three are deterministic functions of the link/unlink sequence and the
+ * BVH state, so two worlds with the same scene history produce identical
+ * pair lists.
+ *
+ * @param {BodyStorage} storage
+ * @param {BVH} dynamic_bvh
+ * @param {BVH} static_bvh
+ * @param {ManifoldStore} manifolds
+ * @param {Array<Array<{collider: Collider, transform: Transform, entity: number, bvhNode: number}>>} body_collider_lists
+ * @param {PairList} pair_list_out cleared and filled
+ * @param {((idA: number, idB: number) => boolean) | null} pair_filter optional
+ *        gate. Returning `false` rejects the pair entirely — no manifold
+ *        slot, no narrowphase, no contact events. Existing cached
+ *        manifolds for a rejected pair time out naturally via the
+ *        manifold-store grace counter.
+ * @param {*} [pair_filter_this_arg]
+ */
+export function generate_pairs(
+    storage,
+    dynamic_bvh,
+    static_bvh,
+    manifolds,
+    body_collider_lists,
+    pair_list_out,
+    pair_filter = null,
+    pair_filter_this_arg = null,
+) {
+    pair_list_out.clear();
+
+    const awake_count = storage.awake_count;
+
+    for (let i = 0; i < awake_count; i++) {
+        const body_idx = storage.awake_at(i);
+        const list = body_collider_lists[body_idx];
+        if (list === undefined || list.length === 0) continue;
+
+        const gen = storage.generation_at(body_idx);
+        const my_packed = (body_idx << 8) | gen;
+
+        for (let k = 0; k < list.length; k++) {
+            const node = list[k].bvhNode;
+            dynamic_bvh.node_get_aabb(node, scratch_aabb);
+
+            // Dynamic ↔ Dynamic.
+            let n = bvh_query_user_data_overlaps_aabb(
+                candidates, 0,
+                dynamic_bvh,
+                scratch_aabb
+            );
+            // The BVH query writes leaves unconditionally — at capacity it both
+            // drops leaves (typed-array OOB writes no-op) AND returns a count
+            // past the buffer end, so the loop below would read `undefined`
+            // candidates and build garbage pairs. Guard the buffer size.
+            assert.lessThan(n, candidates.length, 'generate_pairs: dynamic broadphase overflowed the candidate buffer');
+            for (let c = 0; c < n; c++) {
+                const other = candidates[c];
+                if (other === my_packed) continue;
+
+                const idA = my_packed < other ? my_packed : other;
+                const idB = my_packed < other ? other : my_packed;
+
+                const existing = manifolds.find(idA, idB);
+                if (existing !== -1 && manifolds.is_touched(existing)) continue;
+                if (pair_filter !== null && !pair_filter.call(pair_filter_this_arg, idA, idB)) continue;
+                manifolds.acquire(idA, idB);
+                pair_list_out.push(idA, idB);
+            }
+
+            // Dynamic ↔ Static.
+            n = bvh_query_user_data_overlaps_aabb(
+                candidates, 0,
+                static_bvh,
+                scratch_aabb
+            );
+            assert.lessThan(n, candidates.length, 'generate_pairs: static broadphase overflowed the candidate buffer');
+            for (let c = 0; c < n; c++) {
+                const other = candidates[c];
+                const idA = my_packed < other ? my_packed : other;
+                const idB = my_packed < other ? other : my_packed;
+
+                const existing = manifolds.find(idA, idB);
+                if (existing !== -1 && manifolds.is_touched(existing)) continue;
+                if (pair_filter !== null && !pair_filter.call(pair_filter_this_arg, idA, idB)) continue;
+                manifolds.acquire(idA, idB);
+                pair_list_out.push(idA, idB);
+            }
+        }
+    }
+}