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

Pure JavaScript game engine. Fully featured and production ready.

import { assert } from "../../../core/assert.js";


/**
 *
 * @param {number} i0
 * @param {number} i1
 * @param {number[]|ArrayLike<number>} vertex_data
 * @returns {boolean}
 */
function vertices_equal(i0, i1, vertex_data) {

    assert.isNonNegativeInteger(i0, "i0");
    assert.isNonNegativeInteger(i1, "i1");

    const address_0 = i0 * 3;

    const x0 = vertex_data[address_0];
    const y0 = vertex_data[address_0 + 1];
    const z0 = vertex_data[address_0 + 2];

    const address_1 = i1 * 3;

    const x1 = vertex_data[address_1];
    const y1 = vertex_data[address_1 + 1];
    const z1 = vertex_data[address_1 + 2];

    const dx = x1 - x0;
    const dy = y1 - y0;
    const dz = z1 - z0;

    const distance_sqr = dx * dx + dy * dy + dz * dz;

    return distance_sqr < 1e-7;

}

/**
 *
 * @param {number} i0
 * @param {number} i1
 * @param {number} i2
 * @param {number[]|Float32Array} vertex_data
 * @param {number} normal_x
 * @param {number} normal_y
 * @param {number} normal_z
 */
function triangle_signed_area(
    i0, i1, i2,
    vertex_data,
    normal_x, normal_y, normal_z
) {
    assert.isNonNegativeInteger(i0, "i0");
    assert.isNonNegativeInteger(i1, "i1");
    assert.isNonNegativeInteger(i2, "i2");

    assert.isNumber(normal_x, "normal_x");
    assert.isNumber(normal_y, "normal_y");
    assert.isNumber(normal_z, "normal_z");

    const address_0 = i0 * 3;

    const x0 = vertex_data[address_0];
    const y0 = vertex_data[address_0 + 1];
    const z0 = vertex_data[address_0 + 2];

    const address_1 = i1 * 3;

    const x1 = vertex_data[address_1];
    const y1 = vertex_data[address_1 + 1];
    const z1 = vertex_data[address_1 + 2];

    const address_2 = i2 * 3;

    const x2 = vertex_data[address_2];
    const y2 = vertex_data[address_2 + 1];
    const z2 = vertex_data[address_2 + 2];


    // 1. Calculate Vector U (b - a)
    const ux = x1 - x0;
    const uy = y1 - y0;
    const uz = z1 - z0;

    // 2. Calculate Vector V (c - a)
    const vx = x2 - x0;
    const vy = y2 - y0;
    const vz = z2 - z0;

    // 3. Calculate 3D Cross Product (U x V)
    const crossX = uy * vz - uz * vy;
    const crossY = uz * vx - ux * vz;
    const crossZ = ux * vy - uy * vx;

    // 4. Dot Product with the Surface Normal
    return (crossX * normal_x) + (crossY * normal_y) + (crossZ * normal_z);
}

/**
 * Implementation of the string-pulling algorithm for funnel path refinement.
 * Does not allocate memory.
 *
 *
 * @param {Uint32Array|number[]|ArrayLike<number>} output Indices of vertices are written here
 * @param {number} output_offset where to start writing into the output, typically you want to pass 0 here.
 * @param {Uint32Array|Uint16Array|Uint8Array|number[]} portal_vertices Flat array of vertex index pairs [left0,right0, left1, right1 ... leftN, rightN]
 * @param {Float32Array|number[]|ArrayLike<number>} portal_normals Normal vectors for each portal. Should be set to the triangle normal, which we exit through this portal. can be filled with [up, up, up... ] if your portals lay in the horizontal plane mostly.
 * @param {number} portal_count how many portals do we have to work with
 * @param {Float32Array|number[]} vertex_data actual vertex positions
 * @returns number of vertices written to the output
 */
export function funnel_string_pull(
    output,
    output_offset,
    portal_vertices,
    portal_normals,
    portal_count,
    vertex_data
) {
    assert.isArrayLike(output, "output");
    assert.isNonNegativeInteger(output_offset, "output_offset");
    assert.isArrayLike(portal_vertices, "portal_vertices");
    assert.isArrayLike(portal_normals, "portal_normals");
    assert.isNonNegativeInteger(portal_count, "portal_count");
    assert.isArrayLike(vertex_data, "vertex_data");

    if (portal_count === 0) {
        // special case, no data
        return 0;
    }

    // see https://github.com/donmccurdy/three-pathfinding/blob/1f0345a516b7d1a1dac6649cdb6e815b47df6dce/src/Channel.js#L16

    let output_cursor = output_offset;

    // Init scan state
    let apexIndex = 0,
        leftIndex = 0,
        rightIndex = 0;

    let portalApex = portal_vertices[0];
    let portalLeft = portal_vertices[0];
    let portalRight = portal_vertices[1];

    // Add start point.
    output[output_cursor++] = portalApex;

    for (let i = 1; i < portal_count; i++) {
        const portal_address = i * 2;

        const left = portal_vertices[portal_address];
        const right = portal_vertices[portal_address + 1];

        const portal_normal_x = portal_normals[i * 3];
        const portal_normal_y = portal_normals[i * 3 + 1];
        const portal_normal_z = portal_normals[i * 3 + 2];

        // Update right vertex.
        if (triangle_signed_area(portalApex, portalRight, right, vertex_data, portal_normal_x, portal_normal_y, portal_normal_z) <= 0.0) {
            if (vertices_equal(portalApex, portalRight, vertex_data) || triangle_signed_area(portalApex, portalLeft, right, vertex_data, portal_normal_x, portal_normal_y, portal_normal_z) > 0.0) {
                // Tighten the funnel.
                portalRight = right;
                rightIndex = i;
            } else {
                // Right over left, insert left to path and restart scan from portal left point.
                output[output_cursor++] = portalLeft;

                // Make current left the new apex.
                portalApex = portalLeft;
                apexIndex = leftIndex;

                // Reset portal
                portalLeft = portalApex;
                portalRight = portalApex;
                leftIndex = apexIndex;
                rightIndex = apexIndex;

                // Restart scan
                i = apexIndex;
                continue;
            }
        }

        // Update left vertex.
        if (triangle_signed_area(portalApex, portalLeft, left, vertex_data, portal_normal_x, portal_normal_y, portal_normal_z) >= 0.0) {
            if (vertices_equal(portalApex, portalLeft, vertex_data) || triangle_signed_area(portalApex, portalRight, left, vertex_data, portal_normal_x, portal_normal_y, portal_normal_z) < 0.0) {

                // Tighten the funnel.
                portalLeft = left;
                leftIndex = i;

            } else {

                // Left over right, insert right to path and restart scan from portal right point.
                output[output_cursor++] = portalRight;

                // Make current right the new apex.
                portalApex = portalRight;
                apexIndex = rightIndex;

                // Reset portal
                portalLeft = portalApex;
                portalRight = portalApex;
                leftIndex = apexIndex;
                rightIndex = apexIndex;

                // Restart scan
                i = apexIndex;
            }
        }
    }

    const last_portal_address = (portal_count - 1) * 2;

    if (
        (output_cursor === output_offset)
        || (!vertices_equal(output[output_cursor - 1], portal_vertices[last_portal_address], vertex_data))
    ) {
        // Append last point to path.
        output[output_cursor++] = portal_vertices[last_portal_address]
    }


    return output_cursor - output_offset;
}