@woosh/meep-engine 2.163.7 → 2.163.9

package/src/engine/navigation/mesh/build/clip_soup_against_overhangs.d.ts.map

@@ -1 +1 @@
-{"version":3,"file":"clip_soup_against_overhangs.d.ts","sourceRoot":"","sources":["../../../../../../src/engine/navigation/mesh/build/clip_soup_against_overhangs.js"],"names":[],"mappings":"AA4IA;;;;;;;;GAQG;AACH,kDARW,MAAM,EAAE,kBACR,MAAM,wCAEN,MAAM,gBACN,MAAM,gBAEJ,MAAM,EAAE,CA4GpB"}
+{"version":3,"file":"clip_soup_against_overhangs.d.ts","sourceRoot":"","sources":["../../../../../../src/engine/navigation/mesh/build/clip_soup_against_overhangs.js"],"names":[],"mappings":"AA+MA;;;;;;;;GAQG;AACH,kDARW,MAAM,EAAE,kBACR,MAAM,wCAEN,MAAM,gBACN,MAAM,gBAEJ,MAAM,EAAE,CAkOpB"}

package/src/engine/navigation/mesh/build/clip_soup_against_overhangs.js

@@ -14,100 +14,140 @@
  * The subtraction uses the disjoint half-plane decomposition of a convex-polygon difference:
  *     T \ F = U_i ( T ∩ inside(e_0..e_{i-1}) ∩ outside(e_i) )
  * each term convex, the terms disjoint, the union exactly T minus the convex (dilated) footprint F.
- * Clipping is in the plane perpendicular to `up`; world XYZ is carried and interpolated so kept pieces
- * stay on the original (possibly tilted) walkable plane. Degenerate pieces/triangles are dropped.
+ *
+ * Everything is data-oriented: clipping happens in the 2D plane perpendicular to `up`, with polygons
+ * held as flat `Float64Array`s of (u,v) pairs and a small set of reused scratch buffers. World XYZ is
+ * recovered for each surviving vertex by barycentric reconstruction on its (planar) source triangle, so
+ * kept pieces stay exactly on the original - possibly tilted - walkable plane. Degenerate triangles are
+ * dropped, and triangles that no footprint overlaps are copied straight through without allocation.
+ *
+ * The footprints are indexed in a BVH (keyed by their (u,v) extent and carve height band), so each soup
+ * triangle only tests the obstacles that can actually overlap it - O(m log k) rather than O(m k) for m
+ * soup triangles and k obstacles.
  */
 
+import { BVH } from "../../../../core/bvh2/bvh3/BVH.js";
+import { bvh_query_user_data_overlaps_aabb } from "../../../../core/bvh2/bvh3/query/bvh_query_user_data_overlaps_aabb.js";
+import { v3_dot } from "../../../../core/geom/vec3/v3_dot.js";
+import { v3_orthonormal_matrix_from_normal } from "../../../../core/geom/vec3/v3_orthonormal_matrix_from_normal.js";
 import { convex_hull_monotone_2d } from "../../../../core/geom/2d/convex-hull/convex_hull_monotone_2d.js";
+import { compute_polygon_area_2d } from "../../../../core/geom/2d/compute_polygon_area_2d.js";
 
 const POINT_EPS = 1e-9;
 const AREA_EPS = 1e-9;
+const HEIGHT_EPS = 1e-3;
 const CORNER_SEGMENT_ANGLE = Math.PI / 6; // ~30 deg per rounded-corner segment
 
-function basis_for_up(up_x, up_y, up_z) {
-    let rx = 1, ry = 0, rz = 0;
-    if (Math.abs(up_x) > 0.9) { rx = 0; ry = 1; rz = 0; }
-    let ux = up_y * rz - up_z * ry;
-    let uy = up_z * rx - up_x * rz;
-    let uz = up_x * ry - up_y * rx;
-    const ul = Math.hypot(ux, uy, uz) || 1;
-    ux /= ul; uy /= ul; uz /= ul;
-    const vx = up_y * uz - up_z * uy;
-    const vy = up_z * ux - up_x * uz;
-    const vz = up_x * uy - up_y * ux;
-    return { ux, uy, uz, vx, vy, vz };
+// Vertex coincidence key. Quantize each projected coordinate to QUANTIZE (1mm) and pack the two integer
+// coordinates into one safe-integer key: cheaper than a string and with no per-probe allocation. Each
+// axis occupies a biased 26-bit field, so the key stays unique while |u|,|v| < 2^25 / QUANTIZE
+// (~33,000 units). A 16-bit-per-axis pack would only reach ~3.3 units and silently collide - navmesh
+// coordinates routinely exceed that (e.g. geometry built 10,000 units from the origin). Coincident
+// vertices are the same source vertex projected identically, so they match at any quantization; the
+// precision only needs to keep genuinely distinct obstacle corners apart (the smallest is ~0.3 units).
+const QUANTIZE = 1e3;
+const KEY_BIAS = 1 << 25;   // 33,554,432
+const KEY_STRIDE = 1 << 26; // 67,108,864
+
+// Reused clip scratch (stride 2: u,v). Grown on demand so a single allocation serves the whole soup.
+let _cur = new Float64Array(64);
+let _tmp = new Float64Array(64);
+let _side = new Float64Array(64);
+
+/**
+ * Ensure `buf` can hold `vertex_count` (u,v) pairs, returning a large-enough buffer (the same one when
+ * it already fits). Contents are not preserved - callers grow before writing.
+ *
+ * @param {Float64Array} buf
+ * @param {number} vertex_count
+ * @returns {Float64Array}
+ */
+function ensure_capacity(buf, vertex_count) {
+    const needed = vertex_count * 2;
+    if (buf.length >= needed) return buf;
+    let size = buf.length;
+    while (size < needed) size *= 2;
+    return new Float64Array(size);
 }
 
-function clip_halfplane(poly, ax, az, dx, dz, keep_left) {
-    const n = poly.length;
-    if (n === 0) return poly;
+/**
+ * Sutherland-Hodgman clip of a 2D polygon against one half-plane of a directed edge - the general-edge
+ * analogue of {@link polygon2_clip_axis_halfplane}. The edge passes through (ex, ez) with direction
+ * (dx, dz); `keep_left` keeps the side with `cross(dir, P - e) >= 0`, otherwise the right side.
+ *
+ * @param {Float64Array} in_pts input vertices (stride 2)
+ * @param {number} in_n input vertex count
+ * @param {Float64Array} out_pts output buffer (stride 2), sized for at least in_n + 1 vertices
+ * @param {number} ex
+ * @param {number} ez a point on the edge
+ * @param {number} dx
+ * @param {number} dz the edge direction
+ * @param {boolean} keep_left keep the left half-plane when true
+ * @returns {number} surviving vertex count
+ */
+function clip_halfplane(in_pts, in_n, out_pts, ex, ez, dx, dz, keep_left) {
+    let out_n = 0;
+
+    for (let i = 0; i < in_n; i++) {
+        const j = (i + 1) % in_n;
 
-    const out = [];
-    const push = (p) => {
-        const last = out[out.length - 1];
-        if (last && Math.abs(last.u - p.u) < POINT_EPS && Math.abs(last.v - p.v) < POINT_EPS) return;
-        out.push(p);
-    };
+        const a_u = in_pts[i * 2], a_v = in_pts[i * 2 + 1];
+        const b_u = in_pts[j * 2], b_v = in_pts[j * 2 + 1];
 
-    for (let i = 0; i < n; i++) {
-        const A = poly[i];
-        const C = poly[(i + 1) % n];
-        const sa = dx * (A.v - az) - dz * (A.u - ax);
-        const sc = dx * (C.v - az) - dz * (C.u - ax);
-        const inA = keep_left ? (sa >= -POINT_EPS) : (sa <= POINT_EPS);
-        const inC = keep_left ? (sc >= -POINT_EPS) : (sc <= POINT_EPS);
-        if (inA) push(A);
-        if (inA !== inC) {
-            const t = sa / (sa - sc);
-            push({
-                x: A.x + t * (C.x - A.x),
-                y: A.y + t * (C.y - A.y),
-                z: A.z + t * (C.z - A.z),
-                u: A.u + t * (C.u - A.u),
-                v: A.v + t * (C.v - A.v),
-            });
+        const sa = dx * (a_v - ez) - dz * (a_u - ex);
+        const sb = dx * (b_v - ez) - dz * (b_u - ex);
+
+        const in_a = keep_left ? (sa >= -POINT_EPS) : (sa <= POINT_EPS);
+        const in_b = keep_left ? (sb >= -POINT_EPS) : (sb <= POINT_EPS);
+
+        if (in_a) {
+            out_pts[out_n * 2] = a_u;
+            out_pts[out_n * 2 + 1] = a_v;
+            out_n++;
         }
-    }
-    if (out.length > 1) {
-        const f = out[0], l = out[out.length - 1];
-        if (Math.abs(f.u - l.u) < POINT_EPS && Math.abs(f.v - l.v) < POINT_EPS) out.pop();
-    }
-    return out;
-}
 
-function signed_area_uv(poly) {
-    let a = 0;
-    for (let i = 0; i < poly.length; i++) {
-        const A = poly[i], C = poly[(i + 1) % poly.length];
-        a += A.u * C.v - C.u * A.v;
+        if (in_a !== in_b) {
+            const t = sa / (sa - sb);
+            out_pts[out_n * 2] = a_u + t * (b_u - a_u);
+            out_pts[out_n * 2 + 1] = a_v + t * (b_v - a_v);
+            out_n++;
+        }
     }
-    return a * 0.5;
-}
 
-/** Outward (Minkowski-with-disc) offset of a convex CCW polygon by r, with rounded corners. */
-function offset_convex_ccw(poly, r) {
-    const n = poly.length;
-    if (n < 3 || r <= 0) return poly;
+    return out_n;
+}
 
+/**
+ * Outward Minkowski-with-disc offset of a convex CCW polygon by `r`: each edge is pushed out by `r` and
+ * each convex corner is replaced by a rounded arc. Input/output are flat (u,v) pairs.
+ *
+ * @param {Float64Array} pts convex CCW polygon (stride 2)
+ * @param {number} n vertex count
+ * @param {number} r offset radius (> 0)
+ * @returns {Float64Array} the offset polygon (CCW, stride 2)
+ */
+function offset_convex_ccw(pts, n, r) {
     // outward edge normals (right of each CCW edge)
-    const enx = new Array(n), enz = new Array(n);
+    const enx = new Float64Array(n);
+    const enz = new Float64Array(n);
     for (let i = 0; i < n; i++) {
-        const a = poly[i], b = poly[(i + 1) % n];
-        let dx = b.u - a.u, dz = b.v - a.v;
+        const j = (i + 1) % n;
+        let dx = pts[j * 2] - pts[i * 2];
+        let dz = pts[j * 2 + 1] - pts[i * 2 + 1];
         const len = Math.hypot(dx, dz) || 1;
         dx /= len; dz /= len;
-        enx[i] = dz;   // (dir.v, -dir.u) = right/outward of a CCW edge
+        enx[i] = dz;    // (dir.v, -dir.u) = right/outward of a CCW edge
         enz[i] = -dx;
     }
 
     const out = [];
     for (let i = 0; i < n; i++) {
-        const prevEdge = (i - 1 + n) % n; // edge ending at vertex i
-        const nextEdge = i;               // edge starting at vertex i
-        const v = poly[i];
+        const prev_edge = (i - 1 + n) % n; // edge ending at vertex i
+        const next_edge = i;               // edge starting at vertex i
+        const vu = pts[i * 2], vv = pts[i * 2 + 1];
 
-        let a1 = Math.atan2(enz[prevEdge], enx[prevEdge]);
-        let a2 = Math.atan2(enz[nextEdge], enx[nextEdge]);
+        const a1 = Math.atan2(enz[prev_edge], enx[prev_edge]);
+        const a2 = Math.atan2(enz[next_edge], enx[next_edge]);
         let da = a2 - a1;
         while (da < 0) da += 2 * Math.PI;
         while (da > 2 * Math.PI) da -= 2 * Math.PI;
@@ -115,27 +155,54 @@ function offset_convex_ccw(poly, r) {
         const steps = Math.max(1, Math.ceil(da / CORNER_SEGMENT_ANGLE));
         for (let s = 0; s <= steps; s++) {
             const a = a1 + da * (s / steps);
-            out.push({ u: v.u + r * Math.cos(a), v: v.v + r * Math.sin(a) });
+            out.push(vu + r * Math.cos(a), vv + r * Math.sin(a));
         }
     }
-    return out;
+
+    return Float64Array.from(out);
 }
 
-function subtract_convex(P, F) {
-    const pieces = [];
-    let current = P;
-    for (let i = 0; i < F.length; i++) {
-        if (current.length < 3) break;
-        const a = F[i];
-        const b = F[(i + 1) % F.length];
-        const ax = a.u, az = a.v;
-        const dx = b.u - a.u, dz = b.v - a.v;
-        if (Math.abs(dx) < POINT_EPS && Math.abs(dz) < POINT_EPS) continue; // skip degenerate edge
-        const outside = clip_halfplane(current, ax, az, dx, dz, false);
-        if (outside.length >= 3 && Math.abs(signed_area_uv(outside)) > AREA_EPS) pieces.push(outside);
-        current = clip_halfplane(current, ax, az, dx, dz, true);
+/**
+ * Subtract convex footprint `fp` from a single convex polygon, appending the convex, disjoint pieces of
+ * (polygon \ fp) to `out_pieces` as right-sized `Float64Array`s. `fp` must be CCW (interior on the left
+ * of each edge). The polygon's interior remainder (the part inside `fp`) is discarded.
+ *
+ * @param {Float64Array} poly polygon vertices (stride 2)
+ * @param {number} poly_n polygon vertex count
+ * @param {Float64Array} fp footprint vertices (stride 2, CCW)
+ * @param {number} fp_n footprint vertex count
+ * @param {Float64Array[]} out_pieces destination list; surviving outside pieces are pushed
+ */
+function subtract_footprint(poly, poly_n, fp, fp_n, out_pieces) {
+    // current = the running intersection with the inside half-planes processed so far
+    _cur = ensure_capacity(_cur, poly_n + fp_n);
+    _tmp = ensure_capacity(_tmp, poly_n + fp_n);
+    _side = ensure_capacity(_side, poly_n + fp_n);
+
+    let cur = _cur;
+    let tmp = _tmp;
+    for (let i = 0; i < poly_n * 2; i++) cur[i] = poly[i];
+    let cur_n = poly_n;
+
+    for (let i = 0; i < fp_n; i++) {
+        if (cur_n < 3) return; // fully consumed - nothing outside remains
+
+        const j = (i + 1) % fp_n;
+        const ex = fp[i * 2], ez = fp[i * 2 + 1];
+        const dx = fp[j * 2] - ex;
+        const dz = fp[j * 2 + 1] - ez;
+        if (Math.abs(dx) < POINT_EPS && Math.abs(dz) < POINT_EPS) continue; // degenerate edge
+
+        // piece outside this edge (and inside all earlier edges) leaves the footprint here -> it survives
+        const side_n = clip_halfplane(cur, cur_n, _side, ex, ez, dx, dz, false);
+        if (side_n >= 3 && Math.abs(compute_polygon_area_2d(_side, side_n)) > AREA_EPS) {
+            out_pieces.push(_side.slice(0, side_n * 2));
+        }
+
+        // keep marching with the part still inside this edge
+        cur_n = clip_halfplane(cur, cur_n, tmp, ex, ez, dx, dz, true);
+        const swap = cur; cur = tmp; tmp = swap;
     }
-    return pieces;
 }
 
 /**
@@ -153,104 +220,253 @@ export function clip_soup_against_overhangs(soup, triangle_count, source, agent_
     if (ul === 0) return soup.slice(0, triangle_count * 9);
     upx /= ul; upy /= ul; upz /= ul;
 
-    const B = basis_for_up(upx, upy, upz);
-    const along = (x, y, z) => x * upx + y * upy + z * upz;
-    const uOf = (x, y, z) => x * B.ux + y * B.uy + z * B.uz;
-    const vOf = (x, y, z) => x * B.vx + y * B.vy + z * B.vz;
+    // orthonormal basis: rows 0,1 span the plane perpendicular to up (our u,v axes), row 2 is up itself
+    const basis = new Float64Array(9);
+    v3_orthonormal_matrix_from_normal(basis, 0, upx, upy, upz);
+    const u_x = basis[0], u_y = basis[1], u_z = basis[2];
+    const v_x = basis[3], v_y = basis[4], v_z = basis[5];
 
-    // collect downward-facing (overhead) source triangles in (u,v) with their along-up height
-    const overhead = [];
+    // collect downward-facing (overhead) source triangles as flat (u,v) triples plus their mean height
+    const overhead_uv = []; // stride 6: u0,v0,u1,v1,u2,v2
+    const overhead_h = [];
     const fn = new Float32Array(3);
     const fc = new Float32Array(3);
-    const faceCount = source.faces.size;
-    for (let f = 0; f < faceCount; f++) {
+    const face_count = source.faces.size;
+    for (let f = 0; f < face_count; f++) {
         if (!source.faces.is_allocated(f)) continue;
         source.face_read_normal(fn, 0, f);
-        if (fn[0] * upx + fn[1] * upy + fn[2] * upz >= -1e-3) continue;
+        if (fn[0] * upx + fn[1] * upy + fn[2] * upz >= -HEIGHT_EPS) continue; // not downward-facing
 
-        const la = source.face_read_loop(f);
-        const lb = source.loop_read_next(la);
-        const lc = source.loop_read_next(lb);
-        const uv = [];
+        let l = source.face_read_loop(f);
         let hsum = 0;
-        for (const l of [la, lb, lc]) {
+        for (let k = 0; k < 3; k++) {
             source.vertex_read_coordinate(fc, 0, source.loop_read_vertex(l));
-            uv.push({ u: uOf(fc[0], fc[1], fc[2]), v: vOf(fc[0], fc[1], fc[2]) });
-            hsum += along(fc[0], fc[1], fc[2]);
+            const x = fc[0], y = fc[1], z = fc[2];
+            overhead_uv.push(v3_dot(x, y, z, u_x, u_y, u_z), v3_dot(x, y, z, v_x, v_y, v_z));
+            hsum += v3_dot(x, y, z, upx, upy, upz);
+            l = source.loop_read_next(l);
         }
-        overhead.push({ uv, h: hsum / 3 });
+        overhead_h.push(hsum / 3);
     }
 
-    if (overhead.length === 0) return soup.slice(0, triangle_count * 9);
+    const overhead_count = overhead_h.length;
+    if (overhead_count === 0) return soup.slice(0, triangle_count * 9);
 
     // Group overhead triangles that share a (coincident) vertex into one obstacle. Each obstacle's
     // footprint is the convex hull of its vertices, dilated by the agent radius. Treating the obstacle
     // as one polygon (rather than per-triangle) avoids self-touching/pinched union boundaries.
-    const QK = 1e4;
-    const vkey = (p) => Math.round(p.u * QK) + "," + Math.round(p.v * QK);
-    const parent = [];
+    const parent = new Int32Array(overhead_count);
+    for (let i = 0; i < overhead_count; i++) parent[i] = i;
     const find = (a) => { while (parent[a] !== a) { parent[a] = parent[parent[a]]; a = parent[a]; } return a; };
-    for (let i = 0; i < overhead.length; i++) parent[i] = i;
-    const keyToTri = new Map();
-    for (let i = 0; i < overhead.length; i++) {
-        for (const p of overhead[i].uv) {
-            const k = vkey(p);
-            if (keyToTri.has(k)) parent[find(i)] = find(keyToTri.get(k)); else keyToTri.set(k, i);
+
+    const key_to_tri = new Map();
+    for (let i = 0; i < overhead_count; i++) {
+        for (let k = 0; k < 3; k++) {
+            const ku = Math.round(overhead_uv[i * 6 + k * 2] * QUANTIZE);
+            const kv = Math.round(overhead_uv[i * 6 + k * 2 + 1] * QUANTIZE);
+            const key = (ku + KEY_BIAS) * KEY_STRIDE + (kv + KEY_BIAS);
+            const seen = key_to_tri.get(key);
+            if (seen !== undefined) parent[find(i)] = find(seen); else key_to_tri.set(key, i);
         }
     }
+
+    /** @type {Map<number, {pts: number[], hsum: number, n: number}>} */
     const groups = new Map();
-    for (let i = 0; i < overhead.length; i++) {
-        const r = find(i);
-        let g = groups.get(r);
-        if (!g) { g = { pts: [], hsum: 0, n: 0 }; groups.set(r, g); }
-        for (const p of overhead[i].uv) g.pts.push(p.u, p.v);
-        g.hsum += overhead[i].h; g.n++;
+    for (let i = 0; i < overhead_count; i++) {
+        const root = find(i);
+        let g = groups.get(root);
+        if (g === undefined) { g = { pts: [], hsum: 0, n: 0 }; groups.set(root, g); }
+        for (let k = 0; k < 6; k++) g.pts.push(overhead_uv[i * 6 + k]);
+        g.hsum += overhead_h[i];
+        g.n++;
     }
 
-    const footprints = [];
+    // each footprint: a CCW (dilated) convex polygon in (u,v) plus the obstacle's mean height
+    /** @type {Float64Array[]} */
+    const footprint_poly = [];
+    const footprint_h = [];
     for (const g of groups.values()) {
         const hull = convex_hull_monotone_2d(g.pts, g.pts.length / 2);
         if (hull.length < 3) continue;
-        let poly = hull.map((idx) => ({ u: g.pts[idx * 2], v: g.pts[idx * 2 + 1] }));
-        if (signed_area_uv(poly) < 0) poly = poly.slice().reverse();
-        if (agent_radius > 0) poly = offset_convex_ccw(poly, agent_radius);
-        footprints.push({ poly, h: g.hsum / g.n });
+
+        let poly = new Float64Array(hull.length * 2);
+        for (let i = 0; i < hull.length; i++) {
+            poly[i * 2] = g.pts[hull[i] * 2];
+            poly[i * 2 + 1] = g.pts[hull[i] * 2 + 1];
+        }
+        // ensure CCW (interior to the left of each edge) for the subtraction
+        if (compute_polygon_area_2d(poly, hull.length) < 0) {
+            const rev = new Float64Array(poly.length);
+            for (let i = 0; i < hull.length; i++) {
+                rev[i * 2] = poly[(hull.length - 1 - i) * 2];
+                rev[i * 2 + 1] = poly[(hull.length - 1 - i) * 2 + 1];
+            }
+            poly = rev;
+        }
+        if (agent_radius > 0) poly = offset_convex_ccw(poly, poly.length / 2, agent_radius);
+
+        footprint_poly.push(poly);
+        footprint_h.push(g.hsum / g.n);
     }
 
-    if (footprints.length === 0) return soup.slice(0, triangle_count * 9);
+    const footprint_count = footprint_poly.length;
+    if (footprint_count === 0) return soup.slice(0, triangle_count * 9);
+
+    // Broad-phase index: one leaf per footprint, in (u, v, height) space. The first two axes are the
+    // dilated footprint's (u,v) extent; the third is the band of soup heights it can carve - a triangle
+    // at mean height `th` is carved by an obstacle at height `h` iff 0 < h - th <= agent_height. A soup
+    // triangle then queries this BVH with its own (u,v) extent at `th` to get just the candidate
+    // obstacles, instead of scanning all of them.
+    const footprint_bvh = new BVH();
+    footprint_bvh.release_all();
+    for (let fi = 0; fi < footprint_count; fi++) {
+        const poly = footprint_poly[fi];
+        let min_u = Infinity, min_v = Infinity, max_u = -Infinity, max_v = -Infinity;
+        for (let i = 0; i < poly.length; i += 2) {
+            const u = poly[i], v = poly[i + 1];
+            if (u < min_u) min_u = u;
+            if (u > max_u) max_u = u;
+            if (v < min_v) min_v = v;
+            if (v > max_v) max_v = v;
+        }
+        const h = footprint_h[fi];
+        const node = footprint_bvh.allocate_node();
+        footprint_bvh.node_set_user_data(node, fi);
+        footprint_bvh.node_set_aabb_primitive(
+            node,
+            min_u, min_v, h - agent_height - HEIGHT_EPS,
+            max_u, max_v, h - HEIGHT_EPS
+        );
+        footprint_bvh.insert_leaf(node);
+    }
 
     const out = [];
+
+    // per-triangle reusable storage
+    const tri_uv = new Float64Array(6);          // u0,v0,u1,v1,u2,v2 of the source triangle
+    const px = new Float64Array(3), py = new Float64Array(3), pz = new Float64Array(3);
+    const query_aabb = new Float64Array(6);
+    const query_hits = new Int32Array(footprint_count);
+    /** @type {Float64Array[]} */
+    let pieces = [];
+    /** @type {Float64Array[]} */
+    let next_pieces = [];
+
     for (let t = 0; t < triangle_count; t++) {
         const o = t * 9;
-        const tri = [];
+
         let th = 0;
         for (let k = 0; k < 3; k++) {
             const x = soup[o + k * 3], y = soup[o + k * 3 + 1], z = soup[o + k * 3 + 2];
-            tri.push({ x, y, z, u: uOf(x, y, z), v: vOf(x, y, z) });
-            th += along(x, y, z);
+            px[k] = x; py[k] = y; pz[k] = z;
+            tri_uv[k * 2] = v3_dot(x, y, z, u_x, u_y, u_z);
+            tri_uv[k * 2 + 1] = v3_dot(x, y, z, v_x, v_y, v_z);
+            th += v3_dot(x, y, z, upx, upy, upz);
         }
         th /= 3;
 
-        let pieces = [tri];
-        for (const fp of footprints) {
-            const dh = fp.h - th;
-            if (dh <= 1e-3 || dh > agent_height + 1e-3) continue;
-            const next = [];
-            for (const p of pieces) {
-                for (const s of subtract_convex(p, fp.poly)) next.push(s);
+        // barycentric denominator (twice the signed uv area of the triangle)
+        const e1u = tri_uv[2] - tri_uv[0], e1v = tri_uv[3] - tri_uv[1];
+        const e2u = tri_uv[4] - tri_uv[0], e2v = tri_uv[5] - tri_uv[1];
+        const det = e1u * e2v - e2u * e1v;
+
+        if (Math.abs(det) <= AREA_EPS) {
+            // degenerate in (u,v) (a near-vertical triangle): cannot carve meaningfully, pass through
+            for (let i = 0; i < 9; i++) out.push(soup[o + i]);
+            continue;
+        }
+
+        // broad-phase: only the obstacles whose (u,v) extent and height band overlap this triangle
+        let min_u = tri_uv[0], max_u = tri_uv[0], min_v = tri_uv[1], max_v = tri_uv[1];
+        for (let k = 1; k < 3; k++) {
+            const u = tri_uv[k * 2], v = tri_uv[k * 2 + 1];
+            if (u < min_u) min_u = u;
+            if (u > max_u) max_u = u;
+            if (v < min_v) min_v = v;
+            if (v > max_v) max_v = v;
+        }
+        query_aabb[0] = min_u; query_aabb[1] = min_v; query_aabb[2] = th;
+        query_aabb[3] = max_u; query_aabb[4] = max_v; query_aabb[5] = th;
+
+        const hit_count = bvh_query_user_data_overlaps_aabb(query_hits, 0, footprint_bvh, query_aabb);
+        if (hit_count === 0) {
+            // no obstacle overlaps here - copy straight through
+            for (let i = 0; i < 9; i++) out.push(soup[o + i]);
+            continue;
+        }
+
+        // start with the whole triangle as one piece, then subtract each overlapping footprint
+        pieces.length = 0;
+        pieces.push(Float64Array.of(tri_uv[0], tri_uv[1], tri_uv[2], tri_uv[3], tri_uv[4], tri_uv[5]));
+
+        for (let qi = 0; qi < hit_count; qi++) {
+            const fi = query_hits[qi];
+            const dh = footprint_h[fi] - th;
+            if (dh <= HEIGHT_EPS || dh > agent_height + HEIGHT_EPS) continue; // exact band test
+
+            const fp = footprint_poly[fi];
+            const fp_n = fp.length / 2;
+
+            next_pieces.length = 0;
+            for (let pi = 0; pi < pieces.length; pi++) {
+                subtract_footprint(pieces[pi], pieces[pi].length / 2, fp, fp_n, next_pieces);
             }
-            pieces = next;
+
+            const swap = pieces; pieces = next_pieces; next_pieces = swap;
             if (pieces.length === 0) break;
         }
 
-        for (const p of pieces) {
-            for (let i = 1; i + 1 < p.length; i++) {
-                const a0 = p[0], a1 = p[i], a2 = p[i + 1];
-                const area2 = Math.abs((a1.u - a0.u) * (a2.v - a0.v) - (a2.u - a0.u) * (a1.v - a0.v));
+        const inv_det = 1 / det;
+        for (let pi = 0; pi < pieces.length; pi++) {
+            const poly = pieces[pi];
+            const poly_n = poly.length / 2;
+
+            // fan-triangulate the surviving piece and reconstruct world XYZ per vertex
+            for (let i = 1; i + 1 < poly_n; i++) {
+                const u0 = poly[0], v0 = poly[1];
+                const u1 = poly[i * 2], v1 = poly[i * 2 + 1];
+                const u2 = poly[(i + 1) * 2], v2 = poly[(i + 1) * 2 + 1];
+
+                const area2 = Math.abs((u1 - u0) * (v2 - v0) - (u2 - u0) * (v1 - v0));
                 if (area2 < 2 * AREA_EPS) continue;
-                out.push(a0.x, a0.y, a0.z, a1.x, a1.y, a1.z, a2.x, a2.y, a2.z);
+
+                emit_xyz(out, u0, v0, tri_uv, px, py, pz, e1u, e1v, e2u, e2v, inv_det);
+                emit_xyz(out, u1, v1, tri_uv, px, py, pz, e1u, e1v, e2u, e2v, inv_det);
+                emit_xyz(out, u2, v2, tri_uv, px, py, pz, e1u, e1v, e2u, e2v, inv_det);
             }
         }
     }
+
     return out;
 }
+
+/**
+ * Reconstruct the world XYZ of a (u,v) point lying in a source triangle (barycentric on the triangle's
+ * planar uv coordinates) and append it to `out`.
+ *
+ * @param {number[]} out destination soup
+ * @param {number} u
+ * @param {number} v the point in (u,v)
+ * @param {Float64Array} tri_uv the triangle's uv corners (stride 2, vertex 0 is the origin)
+ * @param {Float64Array} px
+ * @param {Float64Array} py
+ * @param {Float64Array} pz the triangle's world corners
+ * @param {number} e1u
+ * @param {number} e1v edge (v1 - v0) in uv
+ * @param {number} e2u
+ * @param {number} e2v edge (v2 - v0) in uv
+ * @param {number} inv_det reciprocal of the uv area determinant
+ */
+function emit_xyz(out, u, v, tri_uv, px, py, pz, e1u, e1v, e2u, e2v, inv_det) {
+    const du = u - tri_uv[0];
+    const dv = v - tri_uv[1];
+    const w1 = (du * e2v - e2u * dv) * inv_det;
+    const w2 = (e1u * dv - du * e1v) * inv_det;
+    const w0 = 1 - w1 - w2;
+
+    out.push(
+        w0 * px[0] + w1 * px[1] + w2 * px[2],
+        w0 * py[0] + w1 * py[1] + w2 * py[2],
+        w0 * pz[0] + w1 * pz[1] + w2 * pz[2]
+    );
+}

package/src/engine/navigation/mesh/bvh_segment_penetrates_mesh.d.ts

@@ -0,0 +1,21 @@
+/**
+ * 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: BVH, mesh: BinaryTopology, ax: number, ay: number, az: number, bx: number, by: number, bz: number): boolean;
+//# sourceMappingURL=bvh_segment_penetrates_mesh.d.ts.map

package/src/engine/navigation/mesh/bvh_segment_penetrates_mesh.d.ts.map

@@ -0,0 +1 @@
+{"version":3,"file":"bvh_segment_penetrates_mesh.d.ts","sourceRoot":"","sources":["../../../../../src/engine/navigation/mesh/bvh_segment_penetrates_mesh.js"],"names":[],"mappings":"AAgEA;;;;;;;;;;;;;;;;;;GAkBG;AACH,gFARW,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,GACJ,OAAO,CAmDnB"}