python
import heapq
import itertools
GRID = [
"S...#....",
".##.#.##.",
"...#.....",
".#...#...",
"...#....G",
]
def locate(grid, symbol):
for y, row in enumerate(grid):
for x, cell in enumerate(row):
if cell == symbol:
return (x, y)
raise ValueError(f"找不到 {symbol}")
def astar(grid, heuristic="manhattan"):
"""
网格 A* 搜索。
- 只允许上下左右移动
- 每次移动成本为1
- 支持 h=0 或曼哈顿距离
- 使用懒删除处理优先队列旧条目
- 改善已关闭节点时允许重新打开
返回：路径、成本、扩展节点数
"""
width = len(grid[0])
if any(len(row) != width for row in grid):
raise ValueError("网格每行长度必须相同")
height = len(grid)
start = locate(grid, "S")
goal = locate(grid, "G")
def h(node):
if heuristic == "zero":
return 0
if heuristic == "manhattan":
x, y = node
gx, gy = goal
return abs(x - gx) + abs(y - gy)
raise ValueError("未知启发式")
def neighbors(node):
x, y = node
for dx, dy in [
(1, 0),
(0, 1),
(-1, 0),
(0, -1),
]:
nx, ny = x + dx, y + dy
if not (0 <= nx < width):
continue
if not (0 <= ny < height):
continue
if grid[ny][nx] == "#":
continue
yield (nx, ny)
counter = itertools.count()
open_heap = []
heapq.heappush(
open_heap,
(h(start), h(start), next(counter), 0, start)
)
g_score = {start: 0}
came_from = {}
closed = set()
expansions = 0
while open_heap:
f, h_value, _, g, current = heapq.heappop(
open_heap
)
if g != g_score.get(current):
continue
if current in closed:
continue
closed.add(current)
expansions += 1
if current == goal:
path = [goal]
while path[-1] != start:
path.append(came_from[path[-1]])
path.reverse()
return {
"path": path,
"cost": g_score[goal],
"expanded": expansions
}
for nxt in neighbors(current):
tentative = g_score[current] + 1
if tentative >= g_score.get(
nxt, float("inf")
):
continue
g_score[nxt] = tentative
came_from[nxt] = current
closed.discard(nxt)
hn = h(nxt)
fn = tentative + hn
heapq.heappush(
open_heap,
(
fn,
hn,
next(counter),
tentative,
nxt
)
)
return {
"path": None,
"cost": None,
"expanded": expansions
}
def draw_path(grid, path):
if path is None:
return "\n".join(grid)
canvas = [list(row) for row in grid]
for x, y in path[1:-1]:
canvas[y][x] = "*"
return "\n".join(
"".join(row) for row in canvas
)
astar_result = astar(GRID, "manhattan")
dijkstra_result = astar(GRID, "zero")
print("A*路径成本:", astar_result["cost"])
print("A*扩展节点:", astar_result["expanded"])
print(
"Dijkstra路径成本:",
dijkstra_result["cost"]
)
print(
"Dijkstra扩展节点:",
dijkstra_result["expanded"]
)
print(draw_path(GRID, astar_result["path"]))
assert astar_result["cost"] == dijkstra_result["cost"]