@woosh/meep-engine/src/engine/navigation/mesh/bvh_query_nearest_face.js

Pure JavaScript game engine. Fully featured and production ready.

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;
}