diff --git a/searches/astar.py b/searches/astar.py new file mode 100644 index 000000000000..5c1de087f6aa --- /dev/null +++ b/searches/astar.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python3 + +""" +Pure Python implementation of the A* (A-star) pathfinding algorithm. + +For doctests run: + python3 -m doctest -v astar.py + +For manual testing run: + python3 astar.py +""" + +import heapq +from typing import Callable, Iterable, Tuple, List, Dict, Optional, Set + +# Type aliases for readability +Node = Tuple[int, int] +NeighborsFn = Callable[[Node], Iterable[Tuple[Node, float]]] +HeuristicFn = Callable[[Node, Node], float] + + +def astar( + start: Node, goal: Node, neighbors: NeighborsFn, h: HeuristicFn +) -> Optional[List[Node]]: + """ + A* algorithm for pathfinding on a graph defined by a neighbor function. + + A* maintains: + -> g[n]: cost from start to node n (best known so far) + -> f[n] = g[n] + h(n, goal): estimated total cost of a path through n to goal + -> open_list: min-heap of candidate nodes prioritized by smallest f-score + -> closed_list: set of nodes already expanded (best path to them fixed) + + :param start: starting node + :param goal: target node + :param neighbors: function returning (neighbor, step_cost) pairs for a node + :param h: admissible heuristic h(n, goal) estimating remaining cost + :return: list of nodes from start to goal (inclusive), or None if no path + + Examples: + >>> def _h(a, b): # Manhattan distance + ... (x1, y1), (x2, y2) = a, b + ... return abs(x1 - x2) + abs(y1 - y2) + >>> def _nbrs(p): # 4-connected grid, unit costs, unbounded + ... x, y = p + ... return [((x + 1, y), 1), ((x - 1, y), 1), ((x, y + 1), 1), ((x, y - 1), 1)] + >>> astar((0, 0), (2, 2), _nbrs, _h)[-1] + (2, 2) + """ + # Min-heap of (f_score, node). We only store (priority, node) to keep it simple; + # if your nodes aren't directly comparable, add a tiebreaker counter to the tuple. + open_list: List[Tuple[float, Node]] = [] + + # Nodes we've fully explored (their best path is finalized). + closed_list: Set[Node] = set() + + # g-scores: best known cost to reach each node from start + g: Dict[Node, float] = {start: 0.0} + + # Parent map to reconstruct the path once we reach the goal + parent: Dict[Node, Optional[Node]] = {start: None} + + # Initialize the frontier with the start node (f = h(start, goal)) + heapq.heappush(open_list, (h(start, goal), start)) + + while open_list: + # Pop the node with the smallest f-score (best promising path so far) + _, current = heapq.heappop(open_list) + + # If we've already expanded this node via a better path, skip it + if current in closed_list: + continue + closed_list.add(current) + + # Goal check: reconstruct the path by following parents backward + if current == goal: + path: List[Node] = [] + while current is not None: + path.append(current) + current = parent[current] + return path[::-1] # reverse to (start ... goal) + + # Explore current's neighbors + for neighbor, cost in neighbors(current): + # If neighbor was already finalized, ignore + if neighbor in closed_list: + continue + + # Tentative g-score via current + tentative_g = g[current] + cost + + # If this is the first time we see neighbor, or we found a cheaper path to it + if neighbor not in g or tentative_g < g[neighbor]: + g[neighbor] = tentative_g + parent[neighbor] = current + f_score = tentative_g + h(neighbor, goal) + heapq.heappush(open_list, (f_score, neighbor)) + + # If the frontier empties without reaching the goal, no path exists + return None + + +def heuristic(n: Node, goal: Node) -> float: + """ + Manhattan (L1) distance heuristic for 4-connected grid movement with unit costs. + Admissible and consistent for axis-aligned moves. + + :param n: current node + :param goal: target node + :return: |x1 - x2| + |y1 - y2| + """ + x1, y1 = n + x2, y2 = goal + return abs(x1 - x2) + abs(y1 - y2) + + +def neighbors(node: Node) -> Iterable[Tuple[Node, float]]: + """ + 4-neighborhood on an unbounded grid with unit edge costs. + + Replace/extend this for: + -> bounded grids (check bounds before yielding) + -> obstacles (skip blocked cells) + -> diagonal moves (add the 4 diagonals with cost sqrt(2) and switch heuristic) + + :param node: (x, y) coordinate + :return: iterable of ((nx, ny), step_cost) + """ + x, y = node + return [ + ((x + 1, y), 1), + ((x - 1, y), 1), + ((x, y + 1), 1), + ((x, y - 1), 1), + ] + + +if __name__ == "__main__": + # Example usage / manual test + start: Node = (0, 0) + goal: Node = (5, 5) + path = astar(start, goal, neighbors, heuristic) + print("Path found:", path) + # Expected (one optimal path; yours may differ but length should be 10 moves + start): + # Path found: [(0, 0), (0, 1), (0, 2), (0, 3), (0, 4), (0, 5), + # (1, 5), (2, 5), (3, 5), (4, 5), (5, 5)]