@woosh/meep-engine 2.147.0 → 2.149.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/body/SolverBodyState.d.ts

@@ -0,0 +1,142 @@
+/**
+ * # SolverBodyState — data-oriented mirror of the per-body solver hot state
+ *
+ * The TGS substep window (contact solver, joint solver, gravity / position
+ * integration) reads and writes a small, fixed set of per-body quantities
+ * thousands of times per step: linear / angular velocity, inverse mass,
+ * inverse-inertia diagonal, and orientation. In the object world those live on
+ * the {@link RigidBody} and {@link Transform} *components*, reached via
+ * `system.__bodies[idx]` / `system.__transforms[idx]` — a chain of pointer
+ * dereferences (array → component object → `Vector3` field) on the hottest
+ * inner loop in the engine.
+ *
+ * This class packs exactly those quantities into one ArrayBuffer-backed,
+ * stride-{@link SBS_STRIDE} `Float64Array` indexed by **body slot index** (the
+ * same index the rest of the physics system already uses). A body's whole
+ * solver state sits in one contiguous span, so the solver reads it with flat
+ * typed-array indexing and no object dereference.
+ *
+ * ## Lifecycle within one `fixedUpdate`
+ *
+ *  - {@link begin} once, after islands are built: ensures capacity and rolls
+ *    the per-step gather stamp.
+ *  - {@link gather} every body referenced this step (the awake set + the
+ *    static / kinematic anchors referenced by contacts and joints). Idempotent
+ *    per step via the stamp, so callers may gather the same body redundantly.
+ *    MUST run after the once-per-step force integration (so the gathered
+ *    velocity is post-force) and before any substep stage reads it.
+ *  - the substep loop mutates `data` in place.
+ *  - {@link scatter} once at the end: write the persistent linear / angular
+ *    velocity back to each gathered body's `RigidBody`. Pose is NOT scattered
+ *    here — `integrate_position` keeps the authoritative pose on the
+ *    `Transform` (write-through, so its `onChanged` subscribers and the
+ *    per-substep concave re-detection still see the moved pose); the
+ *    orientation quaternion is mirrored into `data` purely for the impulse
+ *    loop's world-inverse-inertia evaluation.
+ *
+ * ## Material content
+ *
+ * For Static / Kinematic bodies `invMass` and the inverse-inertia diagonal are
+ * stored as **zero** regardless of the component values — the solver treats
+ * "no inverse mass / no inverse inertia" as immovable, which is exactly how the
+ * object path's `kind !== Dynamic` guards behave. Velocity is mirrored as-is
+ * (a kinematic mover carries a real velocity the contact rows must see; a
+ * static reads zero). Scattering velocity back to a non-dynamic body is a
+ * no-op in value (the solver never moves it), so scatter need not special-case.
+ *
+ * The arithmetic the solver performs on this data is identical to the object
+ * path — only the *source of the operands* changes — so the engine's
+ * same-runtime bit-identical determinism contract is preserved.
+ *
+ * @author Alex Goldring
+ * @copyright Company Named Limited (c) 2026
+ */
+/** Inverse mass (0 for Static / Kinematic). */
+export const SBS_INV_MASS: 0;
+/** Inverse-inertia diagonal, body-local principal frame (0,0,0 for non-dynamic). */
+export const SBS_INV_I_X: 1;
+export const SBS_INV_I_Y: 2;
+export const SBS_INV_I_Z: 3;
+/** Orientation quaternion (mirrors `transform.rotation`, refreshed per substep). */
+export const SBS_QX: 4;
+export const SBS_QY: 5;
+export const SBS_QZ: 6;
+export const SBS_QW: 7;
+/** Persistent linear velocity. */
+export const SBS_LV_X: 8;
+export const SBS_LV_Y: 9;
+export const SBS_LV_Z: 10;
+/** Persistent angular velocity. */
+export const SBS_AV_X: 11;
+export const SBS_AV_Y: 12;
+export const SBS_AV_Z: 13;
+/** Doubles per body. */
+export const SBS_STRIDE: 14;
+export class SolverBodyState {
+    constructor(initial_capacity?: number);
+    /**
+     * Interleaved per-body state, `slot_index * SBS_STRIDE + field`.
+     * @type {Float64Array}
+     */
+    data: Float64Array;
+    /**
+     * Per-body "gathered this step" stamp; equals {@link __gen} when the
+     * body's slot has been gathered in the current step.
+     * @private
+     * @type {Int32Array}
+     */
+    private __stamp;
+    /**
+     * Dense list of slot indices gathered this step, for {@link scatter}.
+     * @private
+     * @type {Uint32Array}
+     */
+    private __gathered;
+    /** @private */
+    private __gathered_count;
+    /**
+     * Monotonic per-step generation. {@link begin} increments it so the
+     * previous step's stamps are stale without an O(n) clear.
+     * @private
+     */
+    private __gen;
+    /** @private */
+    private __capacity;
+    /**
+     * Number of bodies gathered in the current step.
+     * @returns {number}
+     */
+    get gathered_count(): number;
+    /**
+     * Ensure the typed arrays can index slot `[0, hwm)` and roll the per-step
+     * gather state. Call once per `fixedUpdate`, after islands are built.
+     *
+     * @param {number} hwm body-storage high-water mark
+     */
+    begin(hwm: number): void;
+    /**
+     * Mirror one body's solver state into `data` if not already gathered this
+     * step. Idempotent per step. Reads the body's current (post-force)
+     * velocity and current pose.
+     *
+     * @param {number} idx body slot index
+     * @param {RigidBody} rb
+     * @param {Transform} transform
+     */
+    gather(idx: number, rb: RigidBody, transform: Transform): void;
+    /**
+     * Write the solved persistent linear / angular velocity back onto every
+     * gathered body's `RigidBody`. Direct typed-array writes into the
+     * `Vector3` backing — the same observer-bypassing path the solver already
+     * uses for velocity (`lv[0] += …`), so no `onChanged` semantics change.
+     *
+     * @param {RigidBody[]} bodies sparse, indexed by slot
+     */
+    scatter(bodies: RigidBody[]): void;
+    /**
+     * @private
+     * @param {number} hwm
+     */
+    private __grow;
+}
+//# sourceMappingURL=SolverBodyState.d.ts.map

package/src/engine/physics/body/SolverBodyState.d.ts.map

@@ -0,0 +1 @@
+{"version":3,"file":"SolverBodyState.d.ts","sourceRoot":"","sources":["../../../../../src/engine/physics/body/SolverBodyState.js"],"names":[],"mappings":"AAGA;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;GAoDG;AAEH,+CAA+C;AAC/C,6BAA8B;AAC9B,oFAAoF;AACpF,4BAA6B;AAC7B,4BAA6B;AAC7B,4BAA6B;AAC7B,oFAAoF;AACpF,uBAAwB;AACxB,uBAAwB;AACxB,uBAAwB;AACxB,uBAAwB;AACxB,kCAAkC;AAClC,yBAA0B;AAC1B,yBAA0B;AAC1B,0BAA2B;AAC3B,mCAAmC;AACnC,0BAA2B;AAC3B,0BAA2B;AAC3B,0BAA2B;AAE3B,wBAAwB;AACxB,4BAA6B;AAE7B;IACI,uCAoCC;IAjCG;;;OAGG;IACH,MAFU,YAAY,CAEwB;IAE9C;;;;;OAKG;IACH,gBAAkC;IAElC;;;;OAIG;IACH,mBAAsC;IAEtC,eAAe;IACf,yBAAyB;IAEzB;;;;OAIG;IACH,cAAc;IAEd,eAAe;IACf,mBAAqB;IAGzB;;;OAGG;IACH,6BAEC;IAED;;;;;OAKG;IACH,WAFW,MAAM,QAiBhB;IAED;;;;;;;;OAQG;IACH,YAJW,MAAM,6CA8ChB;IAED;;;;;;;OAOG;IACH,gBAFW,WAAW,QAoBrB;IAED;;;OAGG;IACH,eAcC;CACJ"}