@woosh/meep-engine 2.163.6 → 2.163.8

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

@@ -1,319 +0,0 @@
-import { assert } from "../../../../core/assert.js";
-import { NULL_NODE } from "../../../../core/bvh2/bvh3/BVH.js";
-import {
-    bvh_query_leaves_ray_segment
-} from "../../../../core/bvh2/bvh3/query/bvh_query_leaves_ray_segment.js";
-import { NULL_POINTER } from "../../../../core/geom/3d/topology/struct/binary/BinaryTopology.js";
-import { compute_triangle_area_3d } from "../../../../core/geom/3d/triangle/compute_triangle_area_3d.js";
-import {
-    computeTriangleRayIntersectionBarycentric
-} from "../../../../core/geom/3d/triangle/computeTriangleRayIntersectionBarycentric.js";
-import { clamp } from "../../../../core/math/clamp.js";
-
-/**
- * Upper bound on the per-edge subdivision factor, so a single huge triangle cannot explode into an
- * unbounded number of sub-triangles. At this cap a triangle is sampled with up to K_MAX^2 points.
- * @type {number}
- */
-const K_MAX = 64;
-
-/**
- * Small offset (in world units) used to lift a ray origin off the surface it sits on, so the upward
- * clearance ray does not immediately re-intersect the floor it starts from.
- * @type {number}
- */
-const SURFACE_EPSILON = 1e-4;
-
-// reused scratch (build-time, but there's no reason to churn allocations per triangle)
-const ray_leaf_buffer = [];
-const intersection_result = new Float32Array(6);
-const tri_a = new Float32Array(3);
-const tri_b = new Float32Array(3);
-const tri_c = new Float32Array(3);
-
-/**
- * True if there is no source geometry within `agent_height` directly above the point, measured along
- * the world up direction. Casts a ray up from the point (nudged off the surface) and checks every
- * source face whose AABB the ray crosses with a precise ray-triangle test.
- *
- * @param {BVH} bvh source-geometry BVH (leaves carry source face IDs)
- * @param {BinaryTopology} source source mesh, used to resolve face IDs to triangle vertices
- * @param {number} px
- * @param {number} py
- * @param {number} pz
- * @param {number} up_x normalized world up
- * @param {number} up_y
- * @param {number} up_z
- * @param {number} agent_height
- * @returns {boolean}
- */
-function point_has_clearance(bvh, source, px, py, pz, up_x, up_y, up_z, agent_height) {
-    const origin_x = px + up_x * SURFACE_EPSILON;
-    const origin_y = py + up_y * SURFACE_EPSILON;
-    const origin_z = pz + up_z * SURFACE_EPSILON;
-
-    const leaf_count = bvh_query_leaves_ray_segment(
-        bvh, bvh.root,
-        ray_leaf_buffer, 0,
-        origin_x, origin_y, origin_z,
-        up_x, up_y, up_z,
-        0, agent_height
-    );
-
-    for (let i = 0; i < leaf_count; i++) {
-        const node = ray_leaf_buffer[i];
-        const face_id = bvh.node_get_user_data(node);
-
-        const loop_a = source.face_read_loop(face_id);
-
-        if (loop_a === NULL_POINTER) {
-            continue;
-        }
-
-        const loop_b = source.loop_read_next(loop_a);
-        const loop_c = source.loop_read_next(loop_b);
-
-        source.vertex_read_coordinate(tri_a, 0, source.loop_read_vertex(loop_a));
-        source.vertex_read_coordinate(tri_b, 0, source.loop_read_vertex(loop_b));
-        source.vertex_read_coordinate(tri_c, 0, source.loop_read_vertex(loop_c));
-
-        const hit = computeTriangleRayIntersectionBarycentric(
-            intersection_result,
-            origin_x, origin_y, origin_z,
-            up_x, up_y, up_z,
-            tri_a[0], tri_a[1], tri_a[2],
-            tri_b[0], tri_b[1], tri_b[2],
-            tri_c[0], tri_c[1], tri_c[2]
-        );
-
-        if (!hit) {
-            continue;
-        }
-
-        const t = intersection_result[0];
-
-        if (t > 0 && t <= agent_height) {
-            // an overhead obstruction sits within the agent's height
-            return false;
-        }
-    }
-
-    return true;
-}
-
-/**
- * Append a triangle (9 floats) to `output`.
- */
-function emit_triangle(
-    output,
-    ax, ay, az,
-    bx, by, bz,
-    cx, cy, cz
-) {
-    output.push(ax, ay, az, bx, by, bz, cx, cy, cz);
-}
-
-/**
- * Knock holes in the walkable triangle soup wherever an agent of the given height would not fit under
- * overhead geometry.
- *
- * Approach: subdivide each candidate triangle to a resolution tied to the agent's footprint, cast an
- * upward clearance ray from each sub-triangle centroid against the source geometry, and keep only the
- * sub-triangles that have clearance. Fully-clear triangles are emitted unchanged (no subdivision), so
- * the common case adds no triangles; fully-blocked triangles are dropped; partially-blocked triangles
- * are replaced by their clear sub-triangles. Accuracy is bounded by the sampling resolution.
- *
- * The `triangles` array is rewritten in place with the surviving triangles.
- *
- * @param {BVH} bvh source-geometry BVH (leaves carry source face IDs)
- * @param {BinaryTopology} source source mesh, used to resolve face IDs to triangle vertices
- * @param {number} agent_height
- * @param {number} agent_radius
- * @param {number} triangle_count number of triangles currently in `triangles`
- * @param {number[]} triangles flat XYZ soup, 9 floats per triangle
- * @param {Vector3} up world up direction
- * @returns {number} new triangle count
- */
-export function enforce_agent_height_clearance({
-                                                   bvh,
-                                                   source,
-                                                   agent_height,
-                                                   agent_radius,
-                                                   triangle_count,
-                                                   triangles,
-                                                   up,
-                                               }) {
-
-    assert.defined(bvh, 'bvh');
-    assert.defined(source, 'source');
-    assert.greaterThan(agent_height, 0, 'agent_height');
-
-    if (bvh.root === NULL_NODE) {
-        // no source geometry to obstruct anything
-        return triangle_count;
-    }
-
-    // normalize up so ray-intersection distances are world distances
-    let up_x = up.x;
-    let up_y = up.y;
-    let up_z = up.z;
-
-    const up_length = Math.sqrt(up_x * up_x + up_y * up_y + up_z * up_z);
-
-    assert.greaterThan(up_length, 0, 'up vector length');
-
-    up_x /= up_length;
-    up_y /= up_length;
-    up_z /= up_length;
-
-    /**
-     * Resolution at which clearance is sampled. Tied to the agent footprint so the sampling grid is
-     * fine enough to resolve gaps the agent could (or could not) fit through.
-     * @type {number}
-     */
-    const sampling_resolution = Math.min(
-        agent_height / 4,
-        Math.max(agent_radius / 2, 0.001)
-    );
-
-    assert.isFinite(sampling_resolution, 'sampling_resolution');
-    assert.greaterThan(sampling_resolution, 0, 'sampling_resolution');
-
-    /**
-     * Surviving triangles. Built separately because the count can grow (subdivision), then copied back.
-     * @type {number[]}
-     */
-    const output = [];
-
-    for (let i = 0; i < triangle_count; i++) {
-        const address = i * 9;
-
-        const ax = triangles[address];
-        const ay = triangles[address + 1];
-        const az = triangles[address + 2];
-
-        const bx = triangles[address + 3];
-        const by = triangles[address + 4];
-        const bz = triangles[address + 5];
-
-        const cx = triangles[address + 6];
-        const cy = triangles[address + 7];
-        const cz = triangles[address + 8];
-
-        const area = compute_triangle_area_3d(ax, ay, az, bx, by, bz, cx, cy, cz);
-
-        if (area === 0) {
-            // degenerate, nothing meaningful to sample - keep as-is
-            emit_triangle(output, ax, ay, az, bx, by, bz, cx, cy, cz);
-            continue;
-        }
-
-        // edges, used both to size the subdivision and to place sub-triangle vertices
-        const e0x = bx - ax, e0y = by - ay, e0z = bz - az; // a -> b
-        const e1x = cx - ax, e1y = cy - ay, e1z = cz - az; // a -> c
-
-        const len_ab = Math.sqrt(e0x * e0x + e0y * e0y + e0z * e0z);
-        const len_ac = Math.sqrt(e1x * e1x + e1y * e1y + e1z * e1z);
-        const dbcx = cx - bx, dbcy = cy - by, dbcz = cz - bz;
-        const len_bc = Math.sqrt(dbcx * dbcx + dbcy * dbcy + dbcz * dbcz);
-
-        const longest_edge = Math.max(len_ab, len_ac, len_bc);
-
-        const k = clamp(Math.ceil(longest_edge / sampling_resolution), 1, K_MAX);
-
-        const inv_k = 1 / k;
-
-        // Sub-triangle vertices are P(i,j) = a + (i/k)*(b-a) + (j/k)*(c-a), for i+j <= k.
-        // Walk the k^2 sub-triangles (k*(k+... )/...) testing each centroid for clearance.
-        let any_blocked = false;
-        let any_clear = false;
-
-        // reuse a per-triangle scratch list of clear sub-triangles only when we actually subdivide
-        const clear_subtriangles = [];
-
-        for (let row = 0; row < k; row++) {
-            for (let col = 0; col < k - row; col++) {
-                // "upward" sub-triangle: P(col,row), P(col+1,row), P(col,row+1)
-                emit_subtriangle_if_clear(
-                    bvh, source, agent_height, up_x, up_y, up_z,
-                    ax, ay, az, e0x, e0y, e0z, e1x, e1y, e1z, inv_k,
-                    col, row, col + 1, row, col, row + 1,
-                    clear_subtriangles
-                ) ? (any_clear = true) : (any_blocked = true);
-
-                // "downward" sub-triangle: P(col+1,row), P(col+1,row+1), P(col,row+1)
-                if (col < k - row - 1) {
-                    emit_subtriangle_if_clear(
-                        bvh, source, agent_height, up_x, up_y, up_z,
-                        ax, ay, az, e0x, e0y, e0z, e1x, e1y, e1z, inv_k,
-                        col + 1, row, col + 1, row + 1, col, row + 1,
-                        clear_subtriangles
-                    ) ? (any_clear = true) : (any_blocked = true);
-                }
-            }
-        }
-
-        if (!any_blocked) {
-            // fully clear - keep the original triangle, no subdivision, no bloat
-            emit_triangle(output, ax, ay, az, bx, by, bz, cx, cy, cz);
-        } else if (any_clear) {
-            // partially blocked - replace with the surviving sub-triangles
-            for (let s = 0; s < clear_subtriangles.length; s++) {
-                output.push(clear_subtriangles[s]);
-            }
-        }
-        // else fully blocked - drop the triangle entirely
-    }
-
-    // rewrite the soup in place with the survivors
-    for (let i = 0; i < output.length; i++) {
-        triangles[i] = output[i];
-    }
-    triangles.length = output.length;
-
-    return (output.length / 9) | 0;
-}
-
-/**
- * Build sub-triangle vertices P(i,j) = a + (i/k)*e0 + (j/k)*e1, test its centroid for clearance, and
- * push its 9 coordinates into `out` when clear. Returns whether it was clear.
- */
-function emit_subtriangle_if_clear(
-    bvh, source, agent_height, up_x, up_y, up_z,
-    ax, ay, az, e0x, e0y, e0z, e1x, e1y, e1z, inv_k,
-    i0, j0, i1, j1, i2, j2,
-    out
-) {
-    const s0 = i0 * inv_k, t0 = j0 * inv_k;
-    const s1 = i1 * inv_k, t1 = j1 * inv_k;
-    const s2 = i2 * inv_k, t2 = j2 * inv_k;
-
-    const p0x = ax + s0 * e0x + t0 * e1x;
-    const p0y = ay + s0 * e0y + t0 * e1y;
-    const p0z = az + s0 * e0z + t0 * e1z;
-
-    const p1x = ax + s1 * e0x + t1 * e1x;
-    const p1y = ay + s1 * e0y + t1 * e1y;
-    const p1z = az + s1 * e0z + t1 * e1z;
-
-    const p2x = ax + s2 * e0x + t2 * e1x;
-    const p2y = ay + s2 * e0y + t2 * e1y;
-    const p2z = az + s2 * e0z + t2 * e1z;
-
-    const centroid_x = (p0x + p1x + p2x) / 3;
-    const centroid_y = (p0y + p1y + p2y) / 3;
-    const centroid_z = (p0z + p1z + p2z) / 3;
-
-    const clear = point_has_clearance(
-        bvh, source,
-        centroid_x, centroid_y, centroid_z,
-        up_x, up_y, up_z,
-        agent_height
-    );
-
-    if (clear) {
-        out.push(p0x, p0y, p0z, p1x, p1y, p1z, p2x, p2y, p2z);
-    }
-
-    return clear;
-}