From f955dc6d059942dc41fa174fabdb3c57782639f2 Mon Sep 17 00:00:00 2001 From: Pabloader Date: Mon, 4 May 2026 12:28:26 +0000 Subject: [PATCH] A* --- src/common/navigation/astar.ts | 414 +++++++++++++++++++++++++++++++++ 1 file changed, 414 insertions(+) create mode 100644 src/common/navigation/astar.ts diff --git a/src/common/navigation/astar.ts b/src/common/navigation/astar.ts new file mode 100644 index 0000000..92ce825 --- /dev/null +++ b/src/common/navigation/astar.ts @@ -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({ + * 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 { + node: N; + /** Traversal cost — must be > 0 for A* to be optimal. */ + cost: number; +} + +export interface AStarOptions { + /** Return the reachable neighbours of a node, with edge costs. */ + neighbours: (node: N) => Iterable>; + + /** + * 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 { + /** 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 { + node: N; + nodeKey: string; + g: number; // cost from start + f: number; // g + h +} + +class MinHeap { + private data: HeapItem[] = []; + + get size(): number { return this.data.length; } + + push(item: HeapItem): void { + this.data.push(item); + this._bubbleUp(this.data.length - 1); + } + + pop(): HeapItem | 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 { + private readonly neighbours: (node: N) => Iterable>; + private readonly heuristic: (from: N, to: N) => number; + private readonly key: (node: N) => string; + private readonly maxExpansions: number; + + constructor(options: AStarOptions) { + 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 | 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(); + const cameFrom = new Map(); + + g.set(startKey, 0); + + const open = new MinHeap(); + open.push({ + node: start, + nodeKey: startKey, + g: 0, + f: this.heuristic(start, goal), + }); + + // Nodes fully expanded (closed set). + const closed = new Set(); + + 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, + 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): AStarResult | 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(); + const cameFrom = new Map(); + + g.set(startKey, 0); + + const open = new MinHeap(); + open.push({ node: start, nodeKey: startKey, g: 0, f: heuristic(start) }); + + const closed = new Set(); + 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 { + 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[] = []; + 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, + }); + } +}