diff --git a/src/common/navigation/astar.ts b/src/common/navigation/astar.ts index 92ce825..2a69573 100644 --- a/src/common/navigation/astar.ts +++ b/src/common/navigation/astar.ts @@ -25,118 +25,16 @@ * if (result) console.log(result.path, result.cost); */ -// --------------------------------------------------------------------------- -// Types -// --------------------------------------------------------------------------- +import type { Point } from "@common/geometry"; +import { + buildAStarOptionsFrom2D, + MinHeap, + type AStar2DOptions, + type AStarOptions, + type AStarResult, + type Edge, +} from "@common/navigation/utils"; -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 @@ -351,64 +249,8 @@ export class AStar { // 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 { +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, - }); + super(buildAStarOptionsFrom2D(options)); } } diff --git a/src/common/navigation/dijkstra.ts b/src/common/navigation/dijkstra.ts new file mode 100644 index 0000000..7fbad80 --- /dev/null +++ b/src/common/navigation/dijkstra.ts @@ -0,0 +1,276 @@ +// --------------------------------------------------------------------------- +// Shared types — keep Edge in a shared types file if A* uses the same shape. +// --------------------------------------------------------------------------- + +import type { Point } from "@common/geometry"; +import { buildDijkstraOptionsFrom2D, MinHeap, type Dijkstra2DOptions, type DijkstraOptions, type Edge } from "@common/navigation/utils"; + +// --------------------------------------------------------------------------- +// DijkstraMap +// --------------------------------------------------------------------------- + +export class DijkstraMap { + private readonly distances: Map = new Map(); + private readonly nodeRefs: Map = new Map(); + + private readonly _key: (node: N) => string; + private readonly _neighbours: (node: N) => Iterable>; + + constructor(sources: Iterable, opts: DijkstraOptions) { + this._key = opts.key ?? ((n) => String(n)); + this._neighbours = opts.neighbours; + this.build(sources, opts.maxExpansions ?? Infinity); + } + + private build(sources: Iterable, maxExpansions: number): void { + // HeapItem.f = HeapItem.g = distance — no heuristic, so f and g are + // always equal. The existing _less comparator (lower f wins, ties broken + // by higher g) reduces to a plain min-distance order, which is correct. + const heap = new MinHeap(); + + for (const source of sources) { + const nodeKey = this._key(source); + if (!this.distances.has(nodeKey)) { + this.distances.set(nodeKey, 0); + this.nodeRefs.set(nodeKey, source); + heap.push({ node: source, nodeKey, g: 0, f: 0 }); + } + } + + let expansions = 0; + + while (heap.size > 0 && expansions < maxExpansions) { + const { node, nodeKey, g: dist } = heap.pop()!; + + // Discard stale entries produced by relaxation. + if (dist > this.distances.get(nodeKey)!) continue; + + expansions++; + + for (const edge of this._neighbours(node)) { + const neighbourKey = this._key(edge.node); + const candidate = dist + edge.cost; + const existing = this.distances.get(neighbourKey); + + if (existing === undefined || candidate < existing) { + this.distances.set(neighbourKey, candidate); + this.nodeRefs.set(neighbourKey, edge.node); + // f = g = candidate: Dijkstra is A* with a zero heuristic. + heap.push({ node: edge.node, nodeKey: neighbourKey, g: candidate, f: candidate }); + } + } + } + } + + // --------------------------------------------------------------------------- + // Distance queries + // --------------------------------------------------------------------------- + + /** + * Shortest-path cost from `node` to the nearest source. + * Returns `undefined` if the node was not reached (unreachable or beyond + * `maxExpansions`). + */ + distance(node: N): number | undefined { + return this.distances.get(this._key(node)); + } + + /** `true` if `node` was reached during map construction. */ + reachable(node: N): boolean { + return this.distances.has(this._key(node)); + } + + /** Total number of nodes reached. */ + get size(): number { + return this.distances.size; + } + + // --------------------------------------------------------------------------- + // Gradient navigation + // --------------------------------------------------------------------------- + + /** + * The neighbour of `node` with the lowest map distance — the next step an + * entity should take to move *toward* the nearest source. + * + * Only reachable neighbours are considered. Returns `undefined` when `node` + * is isolated or all neighbours are unreachable. + * + * @example + * const next = playerMap.approach(enemy.position); + * if (next) enemy.moveTo(next); + */ + approach(node: N): N | undefined { + return this.gradient(node, 'min'); + } + + /** + * The neighbour of `node` with the highest map distance — the next step an + * entity should take to move *away from* all sources. + * + * @example + * const next = playerMap.flee(enemy.position); + * if (next) enemy.moveTo(next); + */ + flee(node: N): N | undefined { + return this.gradient(node, 'max'); + } + + private gradient(node: N, mode: 'min' | 'max'): N | undefined { + let best: N | undefined; + let bestDist = mode === 'min' ? Infinity : -Infinity; + + for (const edge of this._neighbours(node)) { + const d = this.distances.get(this._key(edge.node)); + if (d === undefined) continue; // unreachable — never move there + if (mode === 'min' ? d < bestDist : d > bestDist) { + bestDist = d; + best = edge.node; + } + } + + return best; + } + + // --------------------------------------------------------------------------- + // Iteration + // --------------------------------------------------------------------------- + + /** + * Iterates all `[node, distance]` pairs in the map. + * + * @example + * for (const [node, dist] of playerMap.entries()) { + * renderer.tint(node, distToColor(dist)); + * } + */ + *entries(): IterableIterator<[N, number]> { + for (const [k, dist] of this.distances) { + yield [this.nodeRefs.get(k)!, dist]; + } + } + + [Symbol.iterator]() { + return this.entries(); + } + + // --------------------------------------------------------------------------- + // Static factory: influence map + // --------------------------------------------------------------------------- + + /** + * Combines multiple `DijkstraMap` layers into a single **influence map**. + * + * Each layer contributes `weight × distance(node)` to a total score. + * Use **negative weights** to attract toward a source, **positive** to repel. + * + * `approach()` moves toward the lowest combined score (most desirable cell). + * `flee()` moves toward the highest combined score (emergency escape). + * + * @example + * const goblinAI = DijkstraMap.combine( + * [ + * { map: playerMap, weight: -1.5 }, // chase + * { map: archerMap, weight: 2.0 }, // dodge archers + * { map: lootMap, weight: -0.5 }, // weakly drawn to loot + * ], + * { neighbours: grid.walkable, key: grid.key }, + * ); + * + * const next = goblinAI.approach(goblin.position); + */ + static combine( + layers: ReadonlyArray<{ map: DijkstraMap; weight: number }>, + opts: DijkstraOptions, + ): InfluenceMap { + return new InfluenceMap(layers, opts); + } +} + +// --------------------------------------------------------------------------- +// InfluenceMap — weighted sum of Dijkstra layers +// --------------------------------------------------------------------------- + +type Layers = ReadonlyArray<{ map: DijkstraMap; weight: number }>; + +export class InfluenceMap { + private readonly layers: Layers; + private readonly _neighbours: (node: N) => Iterable>; + + /** @internal — use `DijkstraMap.combine()` instead. */ + constructor( + layers: Layers, + opts: DijkstraOptions, + ) { + this.layers = layers; + this._neighbours = opts.neighbours; + } + + /** + * Combined influence score for `node`: `Σ (weight_i × distance_i(node))`. + * + * Returns `undefined` if the node is unreachable in every layer. + * Layers that don't reach the node are omitted from the sum rather than + * treated as 0, so a node reachable by only some layers still gets a + * meaningful score. + */ + score(node: N): number | undefined { + let total = 0; + let hasAny = false; + + for (const { map, weight } of this.layers) { + const d = map.distance(node); + if (d !== undefined) { + total += weight * d; + hasAny = true; + } + } + + return hasAny ? total : undefined; + } + + /** Step toward the lowest-scoring neighbour (most desirable position). */ + approach(node: N): N | undefined { + return this.gradient(node, 'min'); + } + + /** Step toward the highest-scoring neighbour (least desirable position). */ + flee(node: N): N | undefined { + return this.gradient(node, 'max'); + } + + private gradient(node: N, mode: 'min' | 'max'): N | undefined { + let best: N | undefined; + let bestScore = mode === 'min' ? Infinity : -Infinity; + + for (const edge of this._neighbours(node)) { + const s = this.score(edge.node); + if (s === undefined) continue; + if (mode === 'min' ? s < bestScore : s > bestScore) { + bestScore = s; + best = edge.node; + } + } + + return best; + } +} +// --------------------------------------------------------------------------- +// DijkstraMap2D — convenience subclass for 2-D grids +// --------------------------------------------------------------------------- + +export class DijkstraMap2D extends DijkstraMap { + constructor(sources: Iterable, options: Dijkstra2DOptions) { + super(sources, buildDijkstraOptionsFrom2D(options)); + } +} + +// --------------------------------------------------------------------------- +// InfluenceMap2D — convenience subclass for 2-D grids +// --------------------------------------------------------------------------- + +export class InfluenceMap2D extends InfluenceMap { + constructor(layers: Layers, options: Dijkstra2DOptions) { + super(layers, buildDijkstraOptionsFrom2D(options)); + } +} diff --git a/src/common/navigation/utils.ts b/src/common/navigation/utils.ts new file mode 100644 index 0000000..142c40e --- /dev/null +++ b/src/common/navigation/utils.ts @@ -0,0 +1,185 @@ +// --------------------------------------------------------------------------- +// MinHeap +// --------------------------------------------------------------------------- + +import type { Point } from "@common/geometry"; + +export interface HeapItem { + node: N; + nodeKey: string; + g: number; // cost from start + f: number; // g + h +} + +export 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]]; + } +} + +// --------------------------------------------------------------------------- +// Types +// --------------------------------------------------------------------------- + +export interface Edge { + node: N; + /** Traversal cost — must be > 0 for A* to be optimal. */ + cost: number; +} + +export interface DijkstraOptions { + /** + * Return the reachable neighbours of a node, with edge costs. + * Costs must be non-negative. + */ + neighbours: (node: N) => Iterable>; + + /** + * Unique string key for a node. + * Default: `String(node)` — override whenever two distinct nodes could + * serialise the same way (objects, arrays, coordinate tuples…). + */ + key?: (node: N) => string; + + /** + * Maximum number of nodes to expand before stopping. + * Nodes beyond the frontier are left unreachable (`distance` → `undefined`). + * Default: `Infinity`. + */ + maxExpansions?: number; +} + +export interface AStarOptions extends DijkstraOptions { + /** + * 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; +} + +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; +} + +export interface Dijkstra2DOptions { + /** + * 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; +} + +export interface AStar2DOptions extends Dijkstra2DOptions { } + +const DIRS_4: Point[] = [{ x: -1, y: 0 }, { x: 1, y: 0 }, { x: 0, y: -1 }, { x: 0, y: 1 }]; +const DIRS_8: Point[] = [...DIRS_4, { x: -1, y: -1 }, { x: -1, y: 1 }, { x: 1, y: -1 }, { x: 1, y: 1 }]; + +export const buildDijkstraOptionsFrom2D = (options: Dijkstra2DOptions): DijkstraOptions => { + const dirs = options.directions === 8 ? DIRS_8 : DIRS_4; + const costFn = options.cost + ?? ((tx, ty, fx, fy) => (tx !== fx && ty !== fy ? Math.SQRT2 : 1)); + + return { + neighbours: ({ x, y }) => { + const edges: Edge[] = []; + for (const { x: dx, y: dy } of dirs) { + const nx = x + dx, ny = y + dy; + if (options.passable(nx, ny, x, y)) { + edges.push({ node: { x: nx, y: ny }, cost: costFn(nx, ny, x, y) }); + } + } + return edges; + }, + key: ({ x, y }) => `${x},${y}`, + maxExpansions: options.maxExpansions, + } +} + +export const buildAStarOptionsFrom2D = (options: AStar2DOptions): AStarOptions => { + const dirs = options.directions; + + return { + ...buildDijkstraOptionsFrom2D(options), + // Octile heuristic — consistent for 8-directional grids; + // falls back to Manhattan for cardinal-only (diagonal term is 0). + heuristic: ({ x: ax, y: ay }, { x: bx, y: by }) => { + const dx = Math.abs(ax - bx), dy = Math.abs(ay - by); + return dirs === 8 + ? (dx + dy) + (Math.SQRT2 - 2) * Math.min(dx, dy) // octile + : dx + dy; // manhattan + }, + } +}