This commit is contained in:
parent
8a4d9dc13f
commit
f955dc6d05
|
|
@ -0,0 +1,414 @@
|
|||
/**
|
||||
* A* pathfinding on an arbitrary graph.
|
||||
*
|
||||
* The graph is defined entirely through two callbacks you provide:
|
||||
* - neighbours(node) → { node, cost }[] edge traversal
|
||||
* - heuristic(a, b) → number admissible estimate of remaining cost
|
||||
*
|
||||
* Nodes are identified by a key function (default: String(node)).
|
||||
* Works for any domain: grid cells, navmesh polygons, waypoint graphs,
|
||||
* hex tiles, 3-D voxels, abstract state machines — anything.
|
||||
*
|
||||
* Guarantees:
|
||||
* - Optimal path when heuristic is admissible (never over-estimates).
|
||||
* - Complete: always finds a path if one exists.
|
||||
* - O((V + E) log V) time via a binary min-heap open set.
|
||||
*
|
||||
* Usage:
|
||||
* const nav = new AStar<Vec2>({
|
||||
* neighbours: (n) => grid.passableNeighbours(n).map(nb => ({ node: nb, cost: 1 })),
|
||||
* heuristic: (a, b) => Math.abs(a.x - b.x) + Math.abs(a.y - b.y),
|
||||
* key: (n) => `${n.x},${n.y}`,
|
||||
* });
|
||||
*
|
||||
* const result = nav.find(start, goal);
|
||||
* if (result) console.log(result.path, result.cost);
|
||||
*/
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Types
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
export interface Edge<N> {
|
||||
node: N;
|
||||
/** Traversal cost — must be > 0 for A* to be optimal. */
|
||||
cost: number;
|
||||
}
|
||||
|
||||
export interface AStarOptions<N> {
|
||||
/** Return the reachable neighbours of a node, with edge costs. */
|
||||
neighbours: (node: N) => Iterable<Edge<N>>;
|
||||
|
||||
/**
|
||||
* Admissible heuristic — estimated cost from `a` to `b`.
|
||||
* Must never exceed the true shortest cost (can under-estimate freely).
|
||||
* Use `() => 0` for Dijkstra's algorithm.
|
||||
*/
|
||||
heuristic: (from: N, to: N) => number;
|
||||
|
||||
/**
|
||||
* Unique string key for a node.
|
||||
* Default: String(node) — override whenever two distinct nodes could
|
||||
* serialise the same way (objects, arrays, tuples…).
|
||||
*/
|
||||
key?: (node: N) => string;
|
||||
|
||||
/**
|
||||
* Maximum number of nodes to expand before giving up.
|
||||
* Useful as a safety valve in large or infinite graphs.
|
||||
* Default: Infinity.
|
||||
*/
|
||||
maxExpansions?: number;
|
||||
}
|
||||
|
||||
export interface AStarResult<N> {
|
||||
/** Ordered list of nodes from start (inclusive) to goal (inclusive). */
|
||||
path: N[];
|
||||
/** Total accumulated edge cost along the path. */
|
||||
cost: number;
|
||||
/** Number of nodes popped from the open set during this search. */
|
||||
expansions: number;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Binary min-heap (open set)
|
||||
// Keyed on f = g + h. Ties broken by higher g (closer to goal) — tends to
|
||||
// reduce expansions on uniform grids and navmeshes.
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
interface HeapItem<N> {
|
||||
node: N;
|
||||
nodeKey: string;
|
||||
g: number; // cost from start
|
||||
f: number; // g + h
|
||||
}
|
||||
|
||||
class MinHeap<N> {
|
||||
private data: HeapItem<N>[] = [];
|
||||
|
||||
get size(): number { return this.data.length; }
|
||||
|
||||
push(item: HeapItem<N>): void {
|
||||
this.data.push(item);
|
||||
this._bubbleUp(this.data.length - 1);
|
||||
}
|
||||
|
||||
pop(): HeapItem<N> | undefined {
|
||||
if (this.data.length === 0) return undefined;
|
||||
const top = this.data[0];
|
||||
const last = this.data.pop()!;
|
||||
if (this.data.length > 0) {
|
||||
this.data[0] = last;
|
||||
this._sinkDown(0);
|
||||
}
|
||||
return top;
|
||||
}
|
||||
|
||||
private _bubbleUp(i: number): void {
|
||||
while (i > 0) {
|
||||
const parent = (i - 1) >> 1;
|
||||
if (this._less(i, parent)) {
|
||||
this._swap(i, parent);
|
||||
i = parent;
|
||||
} else break;
|
||||
}
|
||||
}
|
||||
|
||||
private _sinkDown(i: number): void {
|
||||
const n = this.data.length;
|
||||
for (; ;) {
|
||||
let best = i;
|
||||
const l = 2 * i + 1, r = 2 * i + 2;
|
||||
if (l < n && this._less(l, best)) best = l;
|
||||
if (r < n && this._less(r, best)) best = r;
|
||||
if (best === i) break;
|
||||
this._swap(i, best);
|
||||
i = best;
|
||||
}
|
||||
}
|
||||
|
||||
// Lower f wins; equal f → higher g wins (prefer nodes closer to goal).
|
||||
private _less(a: number, b: number): boolean {
|
||||
const da = this.data[a], db = this.data[b];
|
||||
return da.f < db.f || (da.f === db.f && da.g > db.g);
|
||||
}
|
||||
|
||||
private _swap(a: number, b: number): void {
|
||||
[this.data[a], this.data[b]] = [this.data[b], this.data[a]];
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// A* class
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
export class AStar<N> {
|
||||
private readonly neighbours: (node: N) => Iterable<Edge<N>>;
|
||||
private readonly heuristic: (from: N, to: N) => number;
|
||||
private readonly key: (node: N) => string;
|
||||
private readonly maxExpansions: number;
|
||||
|
||||
constructor(options: AStarOptions<N>) {
|
||||
this.neighbours = options.neighbours;
|
||||
this.heuristic = options.heuristic;
|
||||
this.key = options.key ?? ((n) => String(n));
|
||||
this.maxExpansions = options.maxExpansions ?? Infinity;
|
||||
}
|
||||
|
||||
/**
|
||||
* Find the shortest path from `start` to `goal`.
|
||||
*
|
||||
* @returns AStarResult if a path exists, null if unreachable or
|
||||
* maxExpansions was exceeded.
|
||||
*/
|
||||
find(start: N, goal: N): AStarResult<N> | null {
|
||||
const startKey = this.key(start);
|
||||
const goalKey = this.key(goal);
|
||||
|
||||
// Early exit — already there.
|
||||
if (startKey === goalKey) {
|
||||
return { path: [start], cost: 0, expansions: 0 };
|
||||
}
|
||||
|
||||
// g[k] = best known cost from start to node k
|
||||
// cameFrom = for path reconstruction
|
||||
const g = new Map<string, number>();
|
||||
const cameFrom = new Map<string, { node: N; key: string }>();
|
||||
|
||||
g.set(startKey, 0);
|
||||
|
||||
const open = new MinHeap<N>();
|
||||
open.push({
|
||||
node: start,
|
||||
nodeKey: startKey,
|
||||
g: 0,
|
||||
f: this.heuristic(start, goal),
|
||||
});
|
||||
|
||||
// Nodes fully expanded (closed set).
|
||||
const closed = new Set<string>();
|
||||
|
||||
let expansions = 0;
|
||||
|
||||
while (open.size > 0) {
|
||||
if (expansions >= this.maxExpansions) return null;
|
||||
|
||||
const current = open.pop()!;
|
||||
const { node, nodeKey, g: gCurrent } = current;
|
||||
|
||||
// Stale entry — a shorter path to this node was already processed.
|
||||
if (closed.has(nodeKey)) continue;
|
||||
closed.add(nodeKey);
|
||||
expansions++;
|
||||
|
||||
if (nodeKey === goalKey) {
|
||||
return {
|
||||
path: this._reconstructPath(cameFrom, node, nodeKey, start, startKey),
|
||||
cost: gCurrent,
|
||||
expansions,
|
||||
};
|
||||
}
|
||||
|
||||
for (const edge of this.neighbours(node)) {
|
||||
if (edge.cost <= 0) {
|
||||
throw new RangeError(
|
||||
`A*: edge cost must be > 0, got ${edge.cost} from node "${nodeKey}"`,
|
||||
);
|
||||
}
|
||||
|
||||
const neighbourKey = this.key(edge.node);
|
||||
if (closed.has(neighbourKey)) continue;
|
||||
|
||||
const gNext = gCurrent + edge.cost;
|
||||
const gBest = g.get(neighbourKey);
|
||||
|
||||
if (gBest === undefined || gNext < gBest) {
|
||||
g.set(neighbourKey, gNext);
|
||||
cameFrom.set(neighbourKey, { node, key: nodeKey });
|
||||
open.push({
|
||||
node: edge.node,
|
||||
nodeKey: neighbourKey,
|
||||
g: gNext,
|
||||
f: gNext + this.heuristic(edge.node, goal),
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return null; // No path found.
|
||||
}
|
||||
|
||||
/**
|
||||
* Reconstruct the path by walking cameFrom back from goal to start.
|
||||
*/
|
||||
private _reconstructPath(
|
||||
cameFrom: Map<string, { node: N; key: string }>,
|
||||
goalNode: N,
|
||||
goalKey: string,
|
||||
startNode: N,
|
||||
startKey: string,
|
||||
): N[] {
|
||||
const path: N[] = [];
|
||||
let currentKey = goalKey;
|
||||
let currentNode = goalNode;
|
||||
|
||||
while (currentKey !== startKey) {
|
||||
path.push(currentNode);
|
||||
const prev = cameFrom.get(currentKey)!;
|
||||
currentKey = prev.key;
|
||||
currentNode = prev.node;
|
||||
}
|
||||
|
||||
path.push(startNode);
|
||||
path.reverse();
|
||||
return path;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convenience: check reachability without allocating a full path.
|
||||
*/
|
||||
isReachable(start: N, goal: N): boolean {
|
||||
return this.find(start, goal) !== null;
|
||||
}
|
||||
|
||||
/**
|
||||
* Multi-goal search — find the nearest reachable goal from a set.
|
||||
*
|
||||
* Runs a single A* from `start` using a combined heuristic:
|
||||
* min h(node, goal) over all goals. Stops at the first goal expanded.
|
||||
*
|
||||
* @param goals Non-empty iterable of candidate goals.
|
||||
*/
|
||||
findNearest(start: N, goals: Iterable<N>): AStarResult<N> | null {
|
||||
const goalList = [...goals];
|
||||
if (goalList.length === 0) throw new RangeError('findNearest: goals is empty');
|
||||
|
||||
const goalKeys = new Set(goalList.map(g => this.key(g)));
|
||||
const goalNodes = new Map(goalList.map(g => [this.key(g), g]));
|
||||
|
||||
const startKey = this.key(start);
|
||||
|
||||
if (goalKeys.has(startKey)) {
|
||||
return { path: [start], cost: 0, expansions: 0 };
|
||||
}
|
||||
|
||||
const heuristic = (node: N) =>
|
||||
Math.min(...goalList.map(g => this.heuristic(node, g)));
|
||||
|
||||
const g = new Map<string, number>();
|
||||
const cameFrom = new Map<string, { node: N; key: string }>();
|
||||
|
||||
g.set(startKey, 0);
|
||||
|
||||
const open = new MinHeap<N>();
|
||||
open.push({ node: start, nodeKey: startKey, g: 0, f: heuristic(start) });
|
||||
|
||||
const closed = new Set<string>();
|
||||
let expansions = 0;
|
||||
|
||||
while (open.size > 0) {
|
||||
if (expansions >= this.maxExpansions) return null;
|
||||
|
||||
const current = open.pop()!;
|
||||
const { node, nodeKey, g: gCurrent } = current;
|
||||
|
||||
if (closed.has(nodeKey)) continue;
|
||||
closed.add(nodeKey);
|
||||
expansions++;
|
||||
|
||||
if (goalKeys.has(nodeKey)) {
|
||||
const goalNode = goalNodes.get(nodeKey)!;
|
||||
return {
|
||||
path: this._reconstructPath(cameFrom, goalNode, nodeKey, start, startKey),
|
||||
cost: gCurrent,
|
||||
expansions,
|
||||
};
|
||||
}
|
||||
|
||||
for (const edge of this.neighbours(node)) {
|
||||
const neighbourKey = this.key(edge.node);
|
||||
if (closed.has(neighbourKey)) continue;
|
||||
|
||||
const gNext = gCurrent + edge.cost;
|
||||
if (!g.has(neighbourKey) || gNext < g.get(neighbourKey)!) {
|
||||
g.set(neighbourKey, gNext);
|
||||
cameFrom.set(neighbourKey, { node, key: nodeKey });
|
||||
open.push({
|
||||
node: edge.node,
|
||||
nodeKey: neighbourKey,
|
||||
g: gNext,
|
||||
f: gNext + heuristic(edge.node),
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// AStar2D — convenience subclass for 2-D grids
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
export type Node2D = [number, number];
|
||||
|
||||
export interface AStar2DOptions {
|
||||
/**
|
||||
* Return true if the cell (toX, toY) can be entered from (fromX, fromY).
|
||||
* The from-coordinates let you model direction-dependent rules such as
|
||||
* one-way ramps, wall-hugging costs, or diagonal corner-cutting checks.
|
||||
*/
|
||||
passable: (toX: number, toY: number, fromX: number, fromY: number) => boolean;
|
||||
|
||||
/**
|
||||
* Cost to move from one cell to a neighbour.
|
||||
* Receives the same arguments as `passable` after the passability check.
|
||||
* Default: 1 for cardinal moves, √2 for diagonal moves.
|
||||
*/
|
||||
cost?: (toX: number, toY: number, fromX: number, fromY: number) => number;
|
||||
|
||||
/**
|
||||
* Which neighbour directions to consider.
|
||||
* - 4-directional (N S E W, default)
|
||||
* - 8-directional
|
||||
*/
|
||||
directions?: 4 | 8;
|
||||
|
||||
/** Forwarded to AStar. */
|
||||
maxExpansions?: number;
|
||||
}
|
||||
|
||||
const DIRS_4: Node2D[] = [[-1, 0], [1, 0], [0, -1], [0, 1]];
|
||||
const DIRS_8: Node2D[] = [...DIRS_4, [-1, -1], [-1, 1], [1, -1], [1, 1]];
|
||||
|
||||
export class AStar2D extends AStar<Node2D> {
|
||||
constructor(options: AStar2DOptions) {
|
||||
const dirs = options.directions === 8 ? DIRS_8 : DIRS_4;
|
||||
const costFn = options.cost
|
||||
?? ((tx, ty, fx, fy) => (tx !== fx && ty !== fy ? Math.SQRT2 : 1));
|
||||
|
||||
super({
|
||||
neighbours: ([x, y]) => {
|
||||
const edges: Edge<Node2D>[] = [];
|
||||
for (const [dx, dy] of dirs) {
|
||||
const nx = x + dx, ny = y + dy;
|
||||
if (options.passable(nx, ny, x, y)) {
|
||||
edges.push({ node: [nx, ny], cost: costFn(nx, ny, x, y) });
|
||||
}
|
||||
}
|
||||
return edges;
|
||||
},
|
||||
// Octile heuristic — consistent for 8-directional grids;
|
||||
// falls back to Manhattan for cardinal-only (diagonal term is 0).
|
||||
heuristic: ([ax, ay], [bx, by]) => {
|
||||
const dx = Math.abs(ax - bx), dy = Math.abs(ay - by);
|
||||
return dirs === DIRS_8
|
||||
? (dx + dy) + (Math.SQRT2 - 2) * Math.min(dx, dy) // octile
|
||||
: dx + dy; // manhattan
|
||||
},
|
||||
key: ([x, y]) => `${x},${y}`,
|
||||
maxExpansions: options.maxExpansions,
|
||||
});
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue