@woosh/meep-engine 2.163.7 → 2.163.9

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

@@ -0,0 +1,133 @@
+import {
+    COLUMN_CHILD_1,
+    COLUMN_CHILD_2,
+    COLUMN_USER_DATA,
+    ELEMENT_WORD_COUNT,
+    NULL_NODE
+} from "../../../core/bvh2/bvh3/BVH.js";
+import { NULL_POINTER } from "../../../core/geom/3d/topology/struct/binary/BinaryTopology.js";
+
+// Plane-side slack and barycentric slack. Mesh vertices are float32, so a segment endpoint that lies ON
+// a triangle sits within ~coord*2^-23 of its plane; PLANE_EPS keeps those (and grazing/coplanar pieces)
+// from registering as a crossing. Only a genuinely transversal piece -- strictly on opposite sides of a
+// triangle's plane and crossing its interior away from its own endpoints -- counts as a penetration.
+const PLANE_EPS = 1e-3;
+const BARY_EPS = 1e-6;
+
+const _coords = new Float32Array(3);
+const _stack = new Uint32Array(1024); // DFS node stack; navmesh BVHs are shallow (depth ~ log2(faces))
+
+/** Standard slab test: does the segment p + t*d, t in [0,1], intersect the AABB [min,max]? */
+function segment_intersects_aabb(minx, miny, minz, maxx, maxy, maxz, px, py, pz, dx, dy, dz) {
+    let tmin = 0, tmax = 1;
+
+    if (dx > -1e-12 && dx < 1e-12) { if (px < minx || px > maxx) return false; }
+    else { let t1 = (minx - px) / dx, t2 = (maxx - px) / dx; if (t1 > t2) { const t = t1; t1 = t2; t2 = t; } if (t1 > tmin) tmin = t1; if (t2 < tmax) tmax = t2; if (tmin > tmax) return false; }
+
+    if (dy > -1e-12 && dy < 1e-12) { if (py < miny || py > maxy) return false; }
+    else { let t1 = (miny - py) / dy, t2 = (maxy - py) / dy; if (t1 > t2) { const t = t1; t1 = t2; t2 = t; } if (t1 > tmin) tmin = t1; if (t2 < tmax) tmax = t2; if (tmin > tmax) return false; }
+
+    if (dz > -1e-12 && dz < 1e-12) { if (pz < minz || pz > maxz) return false; }
+    else { let t1 = (minz - pz) / dz, t2 = (maxz - pz) / dz; if (t1 > t2) { const t = t1; t1 = t2; t2 = t; } if (t1 > tmin) tmin = t1; if (t2 < tmax) tmax = t2; if (tmin > tmax) return false; }
+
+    return true;
+}
+
+/** True if segment (px,py,pz)->(qx,qy,qz) crosses the interior of triangle (a,b,c) transversally. */
+function segment_crosses_triangle(px, py, pz, qx, qy, qz, ax, ay, az, bx, by, bz, cx, cy, cz) {
+    const ux = bx - ax, uy = by - ay, uz = bz - az;
+    const vx = cx - ax, vy = cy - ay, vz = cz - az;
+    const nx = uy * vz - uz * vy, ny = uz * vx - ux * vz, nz = ux * vy - uy * vx;
+    const nlen = Math.hypot(nx, ny, nz);
+    if (nlen < 1e-12) return false; // degenerate triangle
+
+    const dP = ((px - ax) * nx + (py - ay) * ny + (pz - az) * nz) / nlen;
+    const dQ = ((qx - ax) * nx + (qy - ay) * ny + (qz - az) * nz) / nlen;
+
+    // must cross the plane strictly (not coplanar, not endpoint-on-plane)
+    if (!((dP > PLANE_EPS && dQ < -PLANE_EPS) || (dP < -PLANE_EPS && dQ > PLANE_EPS))) return false;
+
+    const t = dP / (dP - dQ);
+    if (t <= PLANE_EPS || t >= 1 - PLANE_EPS) return false; // crossing at/near an endpoint (on the surface)
+
+    // crossing point, tested for containment via barycentric coordinates in the triangle plane
+    const wx = px + (qx - px) * t - ax, wy = py + (qy - py) * t - ay, wz = pz + (qz - pz) * t - az;
+    const d00 = ux * ux + uy * uy + uz * uz, d01 = ux * vx + uy * vy + uz * vz, d11 = vx * vx + vy * vy + vz * vz;
+    const d20 = wx * ux + wy * uy + wz * uz, d21 = wx * vx + wy * vy + wz * vz;
+    const denom = d00 * d11 - d01 * d01;
+    if (denom > -1e-18 && denom < 1e-18) return false;
+    const s = (d11 * d20 - d01 * d21) / denom;
+    const w = (d00 * d21 - d01 * d20) / denom;
+
+    return s >= -BARY_EPS && w >= -BARY_EPS && s + w <= 1 + BARY_EPS;
+}
+
+/**
+ * Does the segment (ax,ay,az)->(bx,by,bz) pass THROUGH the mesh surface -- cross the interior of any
+ * triangle transversally (entering one side, exiting the other) strictly between its endpoints?
+ *
+ * This is the "penetration" test for a navmesh path piece. A piece that hugs the surface (coplanar with
+ * it) or flies over / under it -- a corner-only waypoint chord on a non-planar surface -- does NOT
+ * penetrate; a piece that tunnels through a wall or floor does. Endpoints lying on the surface (every
+ * path waypoint) are not penetrations.
+ *
+ * @param {BVH} bvh built by bvh_build_from_bt_mesh over `mesh`
+ * @param {BinaryTopology} mesh triangle mesh
+ * @param {number} ax
+ * @param {number} ay
+ * @param {number} az
+ * @param {number} bx
+ * @param {number} by
+ * @param {number} bz
+ * @returns {boolean}
+ */
+export function bvh_segment_penetrates_mesh(bvh, mesh, ax, ay, az, bx, by, bz) {
+    const root = bvh.root;
+    if (root === NULL_NODE) return false;
+
+    const float32 = bvh.__data_float32;
+    const uint32 = bvh.__data_uint32;
+    const dx = bx - ax, dy = by - ay, dz = bz - az;
+
+    let sp = 0;
+    _stack[sp++] = root;
+
+    while (sp > 0) {
+        const node = _stack[--sp];
+        const address = node * ELEMENT_WORD_COUNT;
+
+        if (!segment_intersects_aabb(
+            float32[address], float32[address + 1], float32[address + 2],
+            float32[address + 3], float32[address + 4], float32[address + 5],
+            ax, ay, az, dx, dy, dz
+        )) {
+            continue;
+        }
+
+        const child_1 = uint32[address + COLUMN_CHILD_1];
+        if (child_1 !== NULL_NODE) {
+            _stack[sp++] = child_1;
+            _stack[sp++] = uint32[address + COLUMN_CHILD_2];
+            continue;
+        }
+
+        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(_coords, 0, mesh.loop_read_vertex(loop_a));
+        const tax = _coords[0], tay = _coords[1], taz = _coords[2];
+        mesh.vertex_read_coordinate(_coords, 0, mesh.loop_read_vertex(loop_b));
+        const tbx = _coords[0], tby = _coords[1], tbz = _coords[2];
+        mesh.vertex_read_coordinate(_coords, 0, mesh.loop_read_vertex(loop_c));
+        const tcx = _coords[0], tcy = _coords[1], tcz = _coords[2];
+
+        if (segment_crosses_triangle(ax, ay, az, bx, by, bz, tax, tay, taz, tbx, tby, tbz, tcx, tcy, tcz)) {
+            return true;
+        }
+    }
+
+    return false;
+}