pathfinding/AStar.js

import Vector from "../math/Vector.js";

/**
 * Very basic A Star Pathfinding algorithm
 * @see https://en.wikipedia.org/wiki/A*_search_algorithm
 */
class AStar {
    /**
     * @param {Function} getNeighbours 
     * @param {Boolean} diagonal also allow moving diagonal
     */
    constructor(getNeighbours, heuristic = null) {
        this.getNeighbours = getNeighbours;
        this.heuristic = heuristic;

        if (this.heuristic === null) {
            this.heuristic = function(a, b) {
                return Vector.dist(a, b);
            };
        }
    }

    /**
     * Calculate a path from start to end
     * @param {Vector|Object} start start position
     * @param {Vector|Object} end end position
     * @returns {Array} the path as array of points `[{x: start.x, y: start.y}, ..., {x: end.x, y: end.y}]`
     */
    calc(start, end) {
        // The set of discovered nodes that may need to be (re-)expanded.
        // Initially, only the start node is known.
        const open = new Map();

        // For node n, cameFrom[n] is the node immediately preceding it on the cheapest path from start
        // to n currently known.
        const cameFrom = new Map();

        // For node n, gScore[n] is the cost of the cheapest path from start to n currently known.
        const gScore = new Map();

        // For node n, fScore[n] := gScore[n] + h(n). fScore[n] represents our current best guess as to
        // how cheap a path could be from start to finish if it goes through n.
        const fScore = new Map();

        let start_id = this.getNodeId(start);
        open.set(start_id, start);
        gScore.set(start_id, 0);
        fScore.set(start_id, this.heuristic(start, end));

        let end_id = this.getNodeId(end);

        const reconstruct_path = (current) => {
            let p = [current];

            while (cameFrom.has(this.getNodeId(current))) {
                current = cameFrom.get(this.getNodeId(current));
                p.push(current);
            }

            return p.reverse();
        }

        const cleanup = () => {
            open.clear();
            cameFrom.clear();
            gScore.clear();
            fScore.clear();
        };

        while (open.size) {
            let min = Infinity;
            let min_id = null;

            open.forEach((node, id) => {
                let v = fScore.has(id) ? fScore.get(id) : Infinity;
                if (v < min) {
                    min_id = id;
                    min = v;
                }
            })

            let current = open.get(min_id);
            if (min_id === end_id) {
                let p = reconstruct_path(current);
                cleanup();
                return p;
            }

            open.delete(min_id);
            let neighbours = this.getNeighbours(current);

            neighbours.forEach(n => {
                let n_id = this.getNodeId(n);
                // d(current,neighbor) is the weight of the edge from current to neighbor
                // tentative_gScore is the distance from start to the neighbor through current
                let tentative_gScore = gScore.get(min_id) + this.heuristic(current, n);

                if (!gScore.has(n_id) || tentative_gScore < gScore.get(n_id)) {
                    // This path to neighbor is better than any previous one. Record it!
                    cameFrom.set(n_id, current);
                    gScore.set(n_id, tentative_gScore)
                    fScore.set(n_id, tentative_gScore + this.heuristic(n, end));

                    if (!open.has(n_id)) open.set(n_id, n);
                }
            })
        }

        cleanup();

        return null;
    }

    getNodeId(n) {
        return n.x + ":" + n.y;
    }

    /**
     * Create an AStar instance from a grid
     * 
     * @example
     * let astar = AStar.fromGrid(grid, (node) => {
     *    return node === 0;
     * });
     *
     * let path = astar.calc({ x: 0, y: 0 }, { x: 4, y: 4 });
     * @param {Grid} grid 
     * @param {Function} walkable_cb callback that should return true/false if the node is walkable
     * @param {Boolean} diagonal 
     * @returns {AStar}
     */
    static fromGrid(grid, walkable_cb, diagonal = false) {
        return new AStar(function(node) {
            let ret = [];

            if (node.y > 0) ret.push({ x: node.x, y: node.y - 1 });
            if (node.x < grid.w - 1) ret.push({ x: node.x + 1, y: node.y });
            if (node.y < grid.h - 1) ret.push({ x: node.x, y: node.y + 1 });
            if (node.x > 0) ret.push({ x: node.x - 1, y: node.y });

            if (diagonal) {
                if (node.x > 0 && node.y > 0) ret.push({ x: node.x - 1, y: node.y - 1 });
                if (node.x < grid.w - 1 && node.y < grid.h - 1) ret.push({ x: node.x + 1, y: node.y + 1 });
                if (node.x > 0 && node.y < grid.h - 1) ret.push({ x: node.x - 1, y: node.y + 1 });
                if (node.x < grid.w - 1 && node.y > 0) ret.push({ x: node.x + 1, y: node.y - 1 });
            }

            return ret.filter(coords => walkable_cb(grid.get(coords.x, coords.y)));
        })
    }
}

export default AStar