77 lines
2.0 KiB
Python
77 lines
2.0 KiB
Python
import heapq
|
||
|
||
|
||
# 可行走的位置（即值为0，或目标位置）
|
||
def is_walkable(pos):
|
||
r, c = pos
|
||
if 0 <= r < len(parking_lot) and 0 <= c < len(parking_lot[0]):
|
||
return parking_lot[r][c] == 0 or pos == goal
|
||
return False
|
||
|
||
|
||
# 曼哈顿距离作为启发函数
|
||
def heuristic(a, b):
|
||
return abs(a[0] - b[0]) + abs(a[1] - b[1])
|
||
|
||
|
||
# A*搜索
|
||
def a_star(start, goal):
|
||
open_set = []
|
||
heapq.heappush(open_set, (0 + heuristic(start, goal), 0, start))
|
||
came_from = {} # 路径追踪
|
||
g_score = {start: 0}
|
||
|
||
while open_set:
|
||
_, current_cost, current = heapq.heappop(open_set)
|
||
|
||
if current == goal:
|
||
# 还原路径
|
||
path = [current]
|
||
while current in came_from:
|
||
current = came_from[current]
|
||
path.append(current)
|
||
return path[::-1] # 反转
|
||
|
||
# 邻接节点（上下左右）
|
||
for dr, dc in [(-1, 0), (1, 0), (0, -1), (0, 1)]:
|
||
neighbor = (current[0] + dr, current[1] + dc)
|
||
|
||
if not is_walkable(neighbor):
|
||
continue
|
||
|
||
tentative_g = current_cost + 1
|
||
if neighbor not in g_score or tentative_g < g_score[neighbor]:
|
||
g_score[neighbor] = tentative_g
|
||
f = tentative_g + heuristic(neighbor, goal)
|
||
heapq.heappush(open_set, (f, tentative_g, neighbor))
|
||
came_from[neighbor] = current
|
||
|
||
return None # 找不到路径
|
||
|
||
|
||
if __name__ == "__main__":
|
||
# 停车场状态矩阵
|
||
parking_lot = [
|
||
[1, 1, 1, 0, 3, 2, 1],
|
||
[1, 1, 1, 0, 3, 1, 1],
|
||
[0, 0, 0, 0, 0, 0, 0],
|
||
[0, 0, 0, 0, 0, 0, 0],
|
||
[0, 1, 1, 1, 1, 1, 0],
|
||
[0, 2, 2, 2, 2, 2, 0],
|
||
[0, 0, 0, 0, 0, 0, 0],
|
||
]
|
||
|
||
# 起点和终点（行, 列）
|
||
start = (0, 3)
|
||
goal = (3, 5)
|
||
# 执行 A* 路径搜索
|
||
path = a_star(start, goal)
|
||
|
||
# 输出结果
|
||
if path:
|
||
print("找到路径:")
|
||
for step in path:
|
||
print(step)
|
||
else:
|
||
print("找不到路径。")
|