@woosh/meep-engine/src/engine/graphics/geometry/buffered/query/bvh32_geometry_nearest.js

Pure JavaScript game engine. Fully featured and production ready.

import { assert } from "../../../../../core/assert.js";
import {
    typed_array_value_denormalize
} from "../../../../../core/collection/array/typed/typed_array_value_denormalize.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 {
    computeTriangleClosestPointToPointBarycentric
} from "../../../../../core/geom/3d/triangle/computeTriangleClosestPointToPointBarycentric.js";
import { v3_compute_triangle_normal } from "../../../../../core/geom/3d/triangle/v3_compute_triangle_normal.js";
import { v3_distance_sqr } from "../../../../../core/geom/vec3/v3_distance_sqr.js";

const stack = SCRATCH_UINT32_TRAVERSAL_STACK;

/**
 *
 * @type {number[]}
 */
const v3_scratch_0 = [];

/**
 * Get the nearest point on the geometry, relative to query point
 *
 * @param {SurfacePoint3} result
 * @param {BinaryUint32BVH} bvh
 * @param {number[]|ArrayLike<number>} vertices
 * @param {number} vertex_offset Unless you're using an interleaved buffer of some kind, this will be 0
 * @param {number} vertex_stride Unless you're using an interleaved buffer, this should be 3
 * @param {boolean} vertex_data_normalized do we need to denormalize vertex data?
 * @param {number[]|ArrayLike<number>|undefined} indices if this is set to undefined - implicit indexing will be used
 * @param {number} x Query point X
 * @param {number} y Query point Y
 * @param {number} z Query point Z
 * @param {number} [max_distance] limit search distance, useful for optimization
 * @returns {boolean} true iff a point was found, false otherwise
 */
export function bvh32_geometry_nearest(
    result,
    bvh,
    vertices, vertex_offset, vertex_stride,
    vertex_data_normalized,
    indices,
    x, y, z,
    max_distance = Infinity,
) {

    let nearest_distance_sqr = max_distance * max_distance;

    let got_a_hit = false;

    const binary_node_count = bvh.binary_node_count;

    if (binary_node_count <= 0) {
        // this should not happen
        return false;
    }

    /**
     *
     * @type {number}
     */
    const stack_top = stack.pointer++;

    /**
     * After performing empirical tests, stack-based depth-first traversal turns out faster than using a queue
     * @type {number}
     */
    stack[stack_top] = 0;

    const last_valid_index = binary_node_count + bvh.leaf_node_count;

    const float32 = bvh.float32;
    const uint32 = bvh.uint32;

    do {
        stack.pointer--;

        // query_bvh_frustum_from_objects.iteration_count++;
        const node_index = stack[stack.pointer];

        const node_address = bvh.getNodeAddress(node_index);

        const is_intermediate_node = node_index < binary_node_count;

        if (is_intermediate_node) {
            // is intermediate node

            const left_index = (node_index << 1) + 1;
            const right_index = left_index + 1;

            if (right_index < last_valid_index) {

                const child_1_address = bvh.getNodeAddress(left_index);

                const distance_sqr_to_child1 = aabb3_unsigned_distance_sqr_to_point(
                    float32[child_1_address], float32[child_1_address + 1], float32[child_1_address + 2],
                    float32[child_1_address + 3], float32[child_1_address + 4], float32[child_1_address + 5],
                    x, y, z
                );

                const child_2_address = bvh.getNodeAddress(right_index);

                const distance_sqr_to_child2 = aabb3_unsigned_distance_sqr_to_point(
                    float32[child_2_address], float32[child_2_address + 1], float32[child_2_address + 2],
                    float32[child_2_address + 3], float32[child_2_address + 4], float32[child_2_address + 5],
                    x, y, z
                );

                if (distance_sqr_to_child1 < distance_sqr_to_child2) {

                    if (distance_sqr_to_child2 < nearest_distance_sqr) {
                        stack[stack.pointer++] = right_index;
                    }

                    if (distance_sqr_to_child1 < nearest_distance_sqr) {
                        stack[stack.pointer++] = left_index;
                    }

                } else {

                    if (distance_sqr_to_child1 < nearest_distance_sqr) {
                        stack[stack.pointer++] = left_index;
                    }

                    if (distance_sqr_to_child2 < nearest_distance_sqr) {
                        stack[stack.pointer++] = right_index;
                    }

                }
            } else if (left_index < last_valid_index) {
                stack[stack.pointer++] = left_index;
            }


        } else {

            // leaf node
            // read triangle data
            const triangle_index = uint32[node_address + 6];
            const index3 = triangle_index * 3;

            let a, b, c;
            if (indices !== undefined) {
                assert.lessThan(index3 + 2, indices.length, 'triangle index overflow, possibly geometry changed but tree was not rebuilt?');

                a = indices[index3];
                b = indices[index3 + 1];
                c = indices[index3 + 2];
            } else {
                // implicit indices
                a = index3;
                b = index3 + 1;
                c = index3 + 2;
            }

            const a_address = a * vertex_stride + vertex_offset;
            const b_address = b * vertex_stride + vertex_offset;
            const c_address = c * vertex_stride + vertex_offset;

            assert.lessThan(a_address + 2, vertices.length, 'a-vertex overflow');
            assert.lessThan(b_address + 2, vertices.length, 'b-vertex overflow');
            assert.lessThan(c_address + 2, vertices.length, 'c-vertex overflow');

            let ax = vertices[a_address];
            let ay = vertices[a_address + 1];
            let az = vertices[a_address + 2];

            let bx = vertices[b_address];
            let by = vertices[b_address + 1];
            let bz = vertices[b_address + 2];

            let cx = vertices[c_address];
            let cy = vertices[c_address + 1];
            let cz = vertices[c_address + 2];

            // denormalize if necessary
            if (vertex_data_normalized) {
                ax = typed_array_value_denormalize(ax, vertices);
                ay = typed_array_value_denormalize(ay, vertices);
                az = typed_array_value_denormalize(az, vertices);

                bx = typed_array_value_denormalize(bx, vertices);
                by = typed_array_value_denormalize(by, vertices);
                bz = typed_array_value_denormalize(bz, vertices);

                cx = typed_array_value_denormalize(cx, vertices);
                cy = typed_array_value_denormalize(cy, vertices);
                cz = typed_array_value_denormalize(cz, vertices);
            }

            // denormalize if necessary
            if (vertex_data_normalized) {
                ax = typed_array_value_denormalize(ax, vertices);
                ay = typed_array_value_denormalize(ay, vertices);
                az = typed_array_value_denormalize(az, vertices);

                bx = typed_array_value_denormalize(bx, vertices);
                by = typed_array_value_denormalize(by, vertices);
                bz = typed_array_value_denormalize(bz, vertices);

                cx = typed_array_value_denormalize(cx, vertices);
                cy = typed_array_value_denormalize(cy, vertices);
                cz = typed_array_value_denormalize(cz, vertices);
            }

            computeTriangleClosestPointToPointBarycentric(
                v3_scratch_0, 0,
                x, x, x,
                ax, ay, az,
                bx, by, bz,
                cx, cy, cz
            );

            const u = v3_scratch_0[0];
            const v = v3_scratch_0[1];
            const w = 1 - u - v;

            // construct edge

            const contact_x = ax * u + bx * v + cx * w;
            const contact_y = ay * u + by * v + cy * w;
            const contact_z = az * u + bz * v + cz * w;

            const distance_to_triangle_sqr = v3_distance_sqr(
                contact_x, contact_y, contact_z,
                x, y, z
            );

            if (distance_to_triangle_sqr >= nearest_distance_sqr) {
                continue;
            }

            nearest_distance_sqr = distance_to_triangle_sqr;

            result.position.set(contact_x, contact_y, contact_z);

            v3_compute_triangle_normal(result.normal, 0,
                ax, ay, az,
                bx, by, bz,
                cx, cy, cz
            );

            result.index = triangle_index;

            got_a_hit = true;
        }
    } while (stack.pointer > stack_top);

    return got_a_hit;
}