1type Point = { x: number; y: number };23type Edge = {4 to: Point;5 cost: number;6};78type QueueEntry = {9 point: Point;10 g: number;11 h: number;12 f: number;13};1415type PathResult = {16 path: Point[];17 cost: number;18 expanded: number;19};2021const keyOf = ({ x, y }: Point) => `${x},${y}`;2223function parseKey(key: string): Point {24 const [x, y] = key.split(",").map(Number);25 return { x, y };26}2728function reconstructPath(29 cameFrom: Map<string, string>,30 goalKey: string,31): Point[] {32 const reversed: Point[] = [];33 let current: string | undefined = goalKey;3435 while (current) {36 reversed.push(parseKey(current));37 current = cameFrom.get(current);38 }3940 return reversed.reverse();41}4243function aStar(44 start: Point,45 goal: Point,46 neighbors: (point: Point) => Edge[],47 heuristic: (point: Point, goal: Point) => number,48): PathResult | null {49 const startKey = keyOf(start);50 const goalKey = keyOf(goal);51 const open = new MinHeap<QueueEntry>(52 (a, b) => a.f - b.f || a.h - b.h,53 );54 const gScore = new Map<string, number>([[startKey, 0]]);55 const cameFrom = new Map<string, string>();56 let expanded = 0;5758 const startH = heuristic(start, goal);59 open.push({ point: start, g: 0, h: startH, f: startH });6061 while (open.size > 0) {62 const current = open.pop()!;63 const currentKey = keyOf(current.point);6465 // Bỏ queue entry cũ sau khi có đường tốt hơn.66 if (current.g !== gScore.get(currentKey)) continue;6768 if (currentKey === goalKey) {69 return {70 path: reconstructPath(cameFrom, goalKey),71 cost: current.g,72 expanded,73 };74 }7576 expanded++;7778 for (const edge of neighbors(current.point)) {79 if (!Number.isFinite(edge.cost) || edge.cost < 0) {80 throw new Error("A* requires finite non-negative edge costs");81 }8283 const nextKey = keyOf(edge.to);84 const tentativeG = current.g + edge.cost;85 const knownG = gScore.get(nextKey) ?? Infinity;8687 if (tentativeG >= knownG) continue;8889 cameFrom.set(nextKey, currentKey);90 gScore.set(nextKey, tentativeG);9192 const h = heuristic(edge.to, goal);93 open.push({94 point: edge.to,95 g: tentativeG,96 h,97 f: tentativeG + h,98 });99 }100 }101102 return null;103}
