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

Pure JavaScript game engine. Fully featured and production ready.

import { assert } from "../../../core/assert.js";
import { Uint32Heap } from "../../../core/collection/heap/Uint32Heap.js";
import { bt_face_get_centroid } from "../../../core/geom/3d/topology/struct/binary/query/bt_face_get_centroid.js";
import {
    bt_face_get_neighbour_faces
} from "../../../core/geom/3d/topology/struct/binary/query/bt_face_get_neighbour_faces.js";

const open = new Uint32Heap();

/**
 *
 * @type {Set<number>}
 */
const closed = new Set();

/**
 *
 * @type {Map<number, number>}
 */
const g_score = new Map();

/**
 * Note that we limit the supported number of neighbors, a reasonable meshes will fit this criteria
 * @type {Uint32Array}
 */
const neighbors = new Uint32Array(256);

const scratch_array_f32 = new Float32Array(6);

/**
 *
 * @param {number} a
 * @param {number} b
 * @param {BinaryTopology} topology
 * @returns {number}
 */
function heuristic(a, b, topology) {
    // compare centroid distances
    bt_face_get_centroid(scratch_array_f32, 0, topology, a);
    bt_face_get_centroid(scratch_array_f32, 3, topology, b);

    // delta
    const dx = scratch_array_f32[0] - scratch_array_f32[3];
    const dy = scratch_array_f32[1] - scratch_array_f32[4];
    const dz = scratch_array_f32[2] - scratch_array_f32[5];

    // distance
    return dx * dx + dy * dy + dz * dz;
}

/**
 *
 * @param {number[]|Uint32Array} output
 * @param {number} goal
 * @param {Map<number, number>} g_score
 * @param {BinaryTopology} topology
 * @returns {number}
 */
function construct_path(output, goal, g_score, topology) {

    let current = goal;
    let path_length = 0;

    // 1. Walk back via lowest g-score path until we get to 0 (start)
    while (true) {
        output[path_length++] = current;

        const current_g = g_score.get(current);

        // If we reached the start node (g_score === 0), we're done traversing
        if (current_g === 0) {
            break;
        }

        const neighbor_count = bt_face_get_neighbour_faces(neighbors, 0, topology, current);

        let best_neighbor = -1;
        let lowest_g = Infinity;

        // Find the neighbor with the lowest g_score
        for (let i = 0; i < neighbor_count; i++) {
            const neighbor = neighbors[i];

            if (g_score.has(neighbor)) {
                const g = g_score.get(neighbor);

                if (g < lowest_g) {
                    lowest_g = g;
                    best_neighbor = neighbor;
                }

            }
        }

        // Safeguard: Break if no valid neighbor is found or if we aren't strictly descending.
        // (Since your traversal_cost is currently 1.0, lowest_g should be current_g - 1.0)
        if (best_neighbor === -1 || lowest_g >= current_g) {
            break;
        }

        current = best_neighbor;
    }

    // 2. Reverse the path in-place to get START -> GOAL order
    const half_length = Math.floor(path_length / 2);

    for (let i = 0; i < half_length; i++) {
        const mirror_i = path_length - 1 - i;
        const temp = output[i];

        output[i] = output[mirror_i];
        output[mirror_i] = temp;
    }

    return path_length;
}


/**
 * Find a path through topology faces.
 * If a path is found - the result will contain start and goal faces.
 *
 * NOTE: if either start or goal faces are not part of the topology - an empty path will be produced.
 *
 * @param {number[]|Uint32Array} output path will be written here as a sequence of face IDs
 * @param {number} start_face_id
 * @param {number} goal_face_id
 * @param {BinaryTopology} topology
 * @returns {number} number of faces that make up the path. This will be 0 if no path is found.
 */
export function bt_mesh_face_find_path(
    output,
    start_face_id,
    goal_face_id,
    topology,
) {
    assert.isArrayLike(output, 'output');
    assert.isNonNegativeInteger(start_face_id, 'start_face_id');
    assert.isNonNegativeInteger(goal_face_id, 'goal_face_id');
    assert.notNull(topology, 'topology');
    assert.defined(topology, 'topology');
    assert.equal(topology.isBinaryTopology, true, 'topology.isBinaryTopology !== true');

    // TODO we can switch traversal to edges instead, that way we can get much better distance heuristics
    //      we don't know where exactly we're going to cross through the triangle, but we know we're going to cross the edge at least, which narrows the domain

    open.clear();
    closed.clear();
    g_score.clear();

    g_score.set(start_face_id, 0);

    open.insert(start_face_id, heuristic(start_face_id, goal_face_id, topology));

    while (!open.is_empty()) {

        const current_node = open.pop_min();

        if (current_node === goal_face_id) {
            // Reached the goal
            return construct_path(output, goal_face_id, g_score, topology);
        }

        closed.add(current_node);

        const neighbor_count = bt_face_get_neighbour_faces(neighbors, 0, topology, current_node);

        for (let i = 0; i < neighbor_count; i++) {

            const neighbor = neighbors[i];

            if (closed.has(neighbor)) {
                // already closed
                continue;
            }

            // TODO compute correct traversal cost based on triangle size
            const traversal_cost = 1.0;

            const current_g_score = g_score.get(current_node);

            const cost_so_far = current_g_score + traversal_cost;

            if (!g_score.has(neighbor) || cost_so_far < g_score.get(neighbor)) {
                // better path

                g_score.set(neighbor, cost_so_far);

                const remaining_heuristic = heuristic(neighbor, goal_face_id, topology);

                const refined_heuristic = cost_so_far + remaining_heuristic;

                open.insert_or_update(neighbor, refined_heuristic);

            }

        }

    }

    // No result found
    return 0;
}