@woosh/meep-engine/src/engine/navigation/grid/find_path_on_grid_astar.js

Pure JavaScript game engine. Fully featured and production ready.

import { assert } from "../../../core/assert.js";
import { BitSet } from "../../../core/binary/BitSet.js";
import { Uint32Heap } from "../../../core/collection/heap/Uint32Heap.js";
import Vector2 from '../../../core/geom/Vector2.js';
import { sign } from "../../../core/math/sign.js";

const neighbors = new Uint32Array(4);

/**
 *
 * @param {Vector2} result
 * @param {number} index
 * @param {number} width
 */
function index2point(result, index, width) {
    const x = index % width;
    const y = (index / width) | 0;

    result.set(x, y);
}

/**
 *
 * @param {number[]|Uint32Array} result
 * @param {number} index
 * @param {number} width
 * @param {number} height
 * @returns {number}
 */
function compute_neighbors(result, index, width, height) {
    const x = index % width;
    const y = (index / width) | 0;

    let neighbour_count = 0;

    if (y > 0) {
        result[neighbour_count] = index - width;
        neighbour_count++
    }

    if (x > 0) {
        result[neighbour_count] = index - 1;
        neighbour_count++;
    }

    if (x < width - 1) {
        result[neighbour_count] = index + 1;
        neighbour_count++;
    }

    if (y < height - 1) {
        result[neighbour_count] = index + width;
        neighbour_count++
    }

    return neighbour_count;
}

/**
 *
 * @param {number} node
 * @param {number[]|Float32Array} g_score
 * @param {number} width
 * @param {number} height
 * @returns {number[]}
 */
function compute_path(node, g_score, width, height) {
    let v_previous = new Vector2(-1, -1);
    let v_current = new Vector2();

    let dx = 1,
        dy = 1,
        _dx = 0,
        _dy = 0;

    const result = [];

    let current = node;
    let prev = current;

    let last_value = g_score[current];

    while (current !== -1) {
        index2point(v_current, current, width);

        _dx = v_current.x - v_previous.x;
        _dy = v_current.y - v_previous.y;

        // normalize
        if (_dx !== 0) {
            _dx = sign(_dx);
        }

        if (_dy !== 0) {
            _dy = sign(_dy);
        }

        const direction_change = dx !== _dx || dy !== _dy;

        if (direction_change) {
            // direction changed
            dx = _dx;
            dy = _dy;

            //only record points where connection bends to save space
            result.push(prev);
        }

        prev = current;


        //swap vectors
        const t = v_previous;
        v_previous = v_current;
        v_current = t;

        // find next point
        const neighbour_count = compute_neighbors(neighbors, current, width, height);

        current = -1;

        for (let i = 0; i < neighbour_count; i++) {
            const neighbor = neighbors[i];

            const score = g_score[neighbor];

            if (score < last_value) {
                current = neighbor;
                last_value = score;
            } else if (score === last_value) {
                // same score, prefer non-bending neighbours

                index2point(v_current, neighbor, width);

                _dx = v_current.x - v_previous.x;
                _dy = v_current.y - v_previous.y;

                if (_dx === dx && _dy === dy) {
                    // same direction
                    current = neighbor;
                    last_value = score;
                }

            }
        }

    }

    if (result[result.length - 1] !== prev) {

        //check if last node needs to be added
        result.push(prev);

    }

    // so far the path is from goal to start, we need to reverse it
    result.reverse();

    return result;
}


/**
 *
 * @param {number} index0
 * @param {number} index1
 * @param {number} width
 * @returns {number}
 */
function heuristic(index0, index1, width) {
    const x1 = index0 % width;
    const y1 = (index0 / width) | 0;

    const x2 = index1 % width;
    const y2 = (index1 / width) | 0;

    // deltas
    const dx = x2 - x1;
    const dy = y2 - y1;

    // we skip expensive SQRT, but still get a good guiding heuristic which has the same monotonic order
    return dx * dx + dy * dy;
}


const open = new Uint32Heap();

const closed = new BitSet();
closed.preventShrink();

/**
 * Contains refined heuristic value
 * @type {Float32Array}
 */
let g_score = new Float32Array(1024);

/**
 *
 * @param {number[]|Uint8Array|Uint16Array|Float32Array} field
 * @param {number} width
 * @param {number} height
 * @param {number} start
 * @param {number} goal
 * @param {number} block_value value in the field that signifies impassible obstacle
 * @returns {Array.<number>} array of indices representing path from start to end
 */
export function find_path_on_grid_astar(
    field,
    width, height,
    start, goal,
    block_value
) {

    assert.defined(field, 'field');
    assert.notNull(field, 'field');

    assert.isNonNegativeInteger(start, "start");
    assert.isNonNegativeInteger(goal, "goal");

    assert.isNumber(block_value, 'blockValue');

    // prepare sets
    open.clear();
    closed.reset();

    const cell_count = width * height;

    if (g_score.length < cell_count) {
        g_score = new Float32Array(cell_count);
    }

    g_score.fill(Infinity, 0, cell_count);

    g_score[start] = 0;

    open.insert(start, heuristic(start, goal, width));

    while (!open.is_empty()) {

        // Grab the lowest f(x) to process next.  Heap keeps this sorted for us.
        const currentNode = open.pop_min();

        if (currentNode === goal) {
            // End case - result has been found, return the traced path.
            return compute_path(currentNode, g_score, width, height);
        }

        // Normal case -- move currentNode from open to closed, process each of its neighbors.
        closed.set(currentNode, true);

        // Find all neighbors for the current node.
        const neighbour_count = compute_neighbors(neighbors, currentNode, width, height);

        for (let i = 0; i < neighbour_count; ++i) {
            const neighbor = neighbors[i];

            if (closed.get(neighbor)) {
                // Already closed, skip to next neighbor.
                continue;
            }

            // The g score is the shortest distance from start to current node.
            // We need to check if the path we have arrived at this neighbor is the shortest one we have seen yet.
            const neighbor_value = field[neighbor];

            if (neighbor_value === block_value) {
                //cell is blocked, close and continue
                closed.set(neighbor, true);
                continue;
            }

            // Cost of traversing to this neighbour
            const transition_cost = neighbor_value + 1;

            // updated path cost
            const cost_so_far = g_score[currentNode] + transition_cost;

            if (cost_so_far < g_score[neighbor]) {

                // update actual cost
                g_score[neighbor] = cost_so_far;

                const remaining_heuristic = heuristic(neighbor, goal, width);

                const refined_heuristic = cost_so_far + remaining_heuristic;

                // Pushing to heap will put it in proper place based on the 'f' value.
                open.insert_or_update(neighbor, refined_heuristic)
            }

        }
    }

    // No result was found - empty array signifies failure to find path.
    return [];
}