Python Implementation
import heapq
def heuristic(a, b):
# Manhattan distance
return abs(a[0]-b[0]) + abs(a[1]-b[1])
def astar(grid, start, end):
open_set = []
heapq.heappush(open_set,
(0, start))
came_from = {}
g = {start: 0}
f = {start:
heuristic(start, end)}
while open_set:
_, curr = heapq.heappop(
open_set)
if curr == end:
return reconstruct(
came_from, curr)
for nb in neighbors(grid, curr):
tg = g[curr] + 1
if tg < g.get(nb, inf):
came_from[nb] = curr
g[nb] = tg
f[nb] = tg + heuristic(
nb, end)
heapq.heappush(
open_set,(f[nb],nb))
return None
How It Works
A* uses f(n) = g(n) + h(n)
g(n) — cost from start to node n
h(n) — heuristic: estimated cost to goal
f(n) — total estimated path cost
The priority queue always expands the node with lowest f(n), guaranteeing the shortest path when h(n) is admissible.