69 lines
1.8 KiB
Python
69 lines
1.8 KiB
Python
|
import heapq
|
|||
|
|
|
|||
|
|
# 停车场状态矩阵
|
|||
|
|
parking_lot = [
|
|||
|
|
[3, 2, 2, 0, 2, 2, 2],
|
|||
|
|
[3, 1, 1, 0, 1, 1, 1],
|
|||
|
|
[0, 0, 0, 0, 0, 0, 0]
|
|||
|
|
]
|
|||
|
|
|
|||
|
|
# 起点和终点（行, 列）
|
|||
|
|
start = (0, 3)
|
|||
|
|
goal = (1, 1)
|
|||
|
|
|
|||
|
|
# 可行走的位置（即值为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 # 找不到路径
|
|||
|
|
|
|||
|
|
# 执行 A* 路径搜索
|
|||
|
|
path = a_star(start, goal)
|
|||
|
|
|
|||
|
|
# 输出结果
|
|||
|
|
if path:
|
|||
|
|
print("找到路径:")
|
|||
|
|
for step in path:
|
|||
|
|
print(step)
|
|||
|
|
else:
|
|||
|
|
print("找不到路径。")
|