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

Pure JavaScript game engine. Fully featured and production ready.

import { BVH } from "../../../core/bvh2/bvh3/BVH.js";
import { BinaryTopology, NULL_POINTER } from "../../../core/geom/3d/topology/struct/binary/BinaryTopology.js";
import { bt_faces_shared_loop } from "../../../core/geom/3d/topology/struct/binary/query/bt_faces_shared_loop.js";
import Vector3 from "../../../core/geom/Vector3.js";
import { bt_mesh_face_find_path } from "./bt_mesh_face_find_path.js";
import { navmesh_build_topology } from "./build/navmesh_build_topology.js";
import { bvh_build_from_bt_mesh } from "./bvh_build_from_bt_mesh.js";
import { bvh_query_nearest_face } from "./bvh_query_nearest_face.js";
import { funnel_string_pull } from "./funnel_string_pull.js";

/**
 * Hard cap on the number of faces we traverse in a single path query.
 * Limits scratch buffer sizes; paths longer than this are truncated by the A* search.
 * @type {number}
 */
const MAX_FACE_PATH_LENGTH = 1024;

// face path IDs or (later) final path vertex indices
const scratch_array_u32 = new Uint32Array(MAX_FACE_PATH_LENGTH);

// one more portal than face-path length (start portal + one between each consecutive pair of faces + goal portal)
const MAX_PORTAL_COUNT = MAX_FACE_PATH_LENGTH + 1;

// [left0, right0, left1, right1, ...]
const scratch_portal_vertices = new Uint32Array(MAX_PORTAL_COUNT * 2);

// flat XYZ triples, one per portal
const scratch_portal_normals = new Float32Array(MAX_PORTAL_COUNT * 3);

// flat XYZ triples; indices 0 and 1 are start/goal, then 2 vertices per intermediate portal
const scratch_vertices = new Float32Array((2 + (MAX_PORTAL_COUNT - 2) * 2) * 3);

export class NavigationMesh {

    topology = new BinaryTopology();

    /**
     * Used for raycasts and neighborhood search.
     * @type {BVH}
     */
    bvh = new BVH();

    /**
     * Build from given scene geometry
     * @param {BinaryTopology} source
     * @param {number} [agent_radius]
     * @param {number} [agent_height]
     * @param {number} [agent_max_climb_angle] In radians, how steep of an angle can the agent go up by
     * @param {Vector3} [up] Defines world's "UP" direction, this is what the agent will respect for climbing constraint
     */
    build({
              source,
              agent_radius = 0,
              agent_height = 0,
              agent_max_climb_angle = Math.PI / 4,
              up = Vector3.up,
          }) {

        navmesh_build_topology({
            destination: this.topology,
            source,
            agent_radius,
            agent_height,
            agent_max_climb_angle,
            up,
        });

        bvh_build_from_bt_mesh(this.bvh, this.topology);
    }


    /**
     * Compute a walkable path between the two points.
     * The result is a sequence of 3d points written into `output`. First point matches start, last point matches goal (snapped onto the mesh).
     *
     * @param {Float32Array} output packed XYZ triples
     * @param {number} start_x
     * @param {number} start_y
     * @param {number} start_z
     * @param {number} goal_x
     * @param {number} goal_y
     * @param {number} goal_z
     * @returns {number} number of 3d points written to `output` (0 if no path was found)
     */
    find_path(
        output,
        start_x, start_y, start_z,
        goal_x, goal_y, goal_z
    ) {

        const mesh = this.topology;
        const bvh = this.bvh;

        const start_face_id = bvh_query_nearest_face(bvh, mesh, start_x, start_y, start_z);

        if (start_face_id === NULL_POINTER) {
            // probably topology is empty
            return 0;
        }

        const goal_face_id = bvh_query_nearest_face(bvh, mesh, goal_x, goal_y, goal_z);

        if (goal_face_id === NULL_POINTER) {
            // should never happen if we got the start face
            return 0;
        }

        if (goal_face_id === start_face_id) {
            // path within the same triangle

            output[0] = start_x;
            output[1] = start_y;
            output[2] = start_z;

            output[3] = goal_x;
            output[4] = goal_y;
            output[5] = goal_z;

            return 2;
        }

        const face_path_length = bt_mesh_face_find_path(scratch_array_u32, start_face_id, goal_face_id, mesh);

        if (face_path_length === 0) {
            // no face path exists (disconnected topology)
            return 0;
        }

        // build portals
        // ==================

        // initialize vertex data pool, vertex index 0 = start, vertex index 1 = goal
        scratch_vertices[0] = start_x;
        scratch_vertices[1] = start_y;
        scratch_vertices[2] = start_z;

        scratch_vertices[3] = goal_x;
        scratch_vertices[4] = goal_y;
        scratch_vertices[5] = goal_z;

        let next_vertex_index = 2;

        // start portal, degenerate portal at the start point, exiting the start face
        scratch_portal_vertices[0] = 0;
        scratch_portal_vertices[1] = 0;

        mesh.face_read_normal(scratch_portal_normals, 0, start_face_id);

        let portal_index = 1;

        // intermediate portals sit on the shared edge between two consecutive faces along the path;
        // the edge is read in the winding order of the face we are exiting, giving a directed (left, right) pair
        for (let i = 1; i < face_path_length; i++) {

            const face_from = scratch_array_u32[i - 1];
            const face_to = scratch_array_u32[i];

            const loop = bt_faces_shared_loop(mesh, face_from, face_to);

            const left_vertex = mesh.loop_read_vertex(loop);
            const right_vertex = mesh.loop_read_vertex(mesh.loop_read_next(loop));

            const left_index = next_vertex_index++;
            const right_index = next_vertex_index++;

            mesh.vertex_read_coordinate(scratch_vertices, left_index * 3, left_vertex);
            mesh.vertex_read_coordinate(scratch_vertices, right_index * 3, right_vertex);

            const portal_address = portal_index * 2;
            scratch_portal_vertices[portal_address] = left_index;
            scratch_portal_vertices[portal_address + 1] = right_index;

            mesh.face_read_normal(scratch_portal_normals, portal_index * 3, face_from);

            portal_index++;
        }

        // goal portal, degenerate at the goal point, uses the normal of the face containing the goal
        const goal_portal_address = portal_index * 2;

        scratch_portal_vertices[goal_portal_address] = 1;
        scratch_portal_vertices[goal_portal_address + 1] = 1;

        mesh.face_read_normal(scratch_portal_normals, portal_index * 3, goal_face_id);

        portal_index++;

        // execute string pulling
        const path_vertex_count = funnel_string_pull(
            scratch_array_u32,
            0,
            scratch_portal_vertices,
            scratch_portal_normals,
            portal_index,
            scratch_vertices,
        );

        // build the final path
        for (let i = 0; i < path_vertex_count; i++) {
            const vertex_index = scratch_array_u32[i];

            output[i * 3] = scratch_vertices[vertex_index * 3];
            output[i * 3 + 1] = scratch_vertices[vertex_index * 3 + 1];
            output[i * 3 + 2] = scratch_vertices[vertex_index * 3 + 2];

        }

        return path_vertex_count;

    }

}