redblob有代码，我在此基础上修改了一些内容并做了可视化
A*算法教程
视频：https://www.bilibili.com/video/BV1bv411y79P?from=search&seid=1736083035601105399
图文：https://www.redblobgames.com/pathfinding/a-star/introduction.html
代码
[zd-plane title=""]
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
| '''A star算法部分
[url=https://www.bilibili.com/video/BV1bv411y79P?from=search&seid=1736083035601105399]https://www.bilibili.com/video/BV1bv411y79P?from=search&seid=1736083035601105399[/url]
[url=https://www.redblobgames.com/pathfinding/a-star/introduction.html]https://www.redblobgames.com/pathfinding/a-star/introduction.html[/url]
'''
# 每个结点的附近四个结点
def neighbors(the_one):
return [[the_one[0], the_one[1] - 1], [the_one[0] + 1, the_one[1]], [the_one[0], the_one[1] + 1],
[the_one[0] - 1, the_one[1]]]
# 当前代价
def weight(first, second):
return 1
# 预估代价，采用欧式距离
def heuristic(first, second):
x_dis = abs(first[0] - second[0])
y_dis = abs(first[1] - second[1])
return math.sqrt(pow(x_dis, 2) + pow(y_dis, 2))
# 主代码
def A_star(the_map):
# 迷宫长度宽度
len_row = len(the_map)
len_col = len(the_map[0])
# 起点
start = [man_x[0], man_y[0]]
# 出口
target = [door_x[0], door_y[0]]
# 等待遍历的点，左边是当前的总代价=当前（距离起点）代价+预估代价
frontier = [(0, start)]
# 记录当前结点的来向结点
came_from = {}
# 记录已经走过的结点，用字典存，key是结点的(x,y)降维后的一个数字,如下面,value是当前代价
# start[0] * 3.141 + start[1] * 2.727 只是个标识，数字随便乘的，当作cost_so_far的key
cost_so_far = {}
came_from[start[0] * 3.141 + start[1] * 2.727] = None
cost_so_far[start[0] * 3.141 + start[1] * 2.727] = 0
# 等待遍历的点不为空
while len(frontier) != 0:
# 弹出总代价最小的结点
ww = frontier.pop(0)
current = ww[1]
# 当前结点就是出口，退出
if current[0] == target[0] and current[1] == target[1]:
break
# 遍历当前结点的上下左右的邻居结点
for next_one in neighbors(current):
# 下面都是对这个邻居结点操作
# 邻居结点没超过地图范围 && 邻居结点还在障碍物中
if 0 <= next_one[0] <= len_col and 0 <= next_one[1] <= len_row and next_one not in barriers:
# 计算 到达邻居结点的当前代价
new_cost = cost_so_far[current[0] * 3.141 + current[1] * 2.727] + weight(current, next_one)
# 如果邻居结点不在已经走过的集合中 或者 走邻居结点的代价小于走当前结点的代价
if next_one[0] * 3.141 + next_one[1] * 2.727 not in cost_so_far.keys() or new_cost < cost_so_far[
current[0] * 3.141 + current[1] * 2.727]:
# 记录到邻居结点的当前代价是new_cost
cost_so_far[next_one[0] * 3.141 + next_one[1] * 2.727] = new_cost
# 计算 邻居结点的总代价
priority = new_cost + heuristic(target, next_one)
# 如果下一个结点是终点，res_flag设置1，提前退出
if next_one[0] == target[0] and next_one[1] == target[1]:
came_from[target[0] * 3.141 + target[1] * 2.727] = current
res_flag[0] = 1
else:
#不是终点，把这个邻居结点放在frontier中
came_from[next_one[0] * 3.141 + next_one[1] * 2.727] = current
frontier.append((priority, next_one))
#重新排序，确保总代价最小的结点在第一个
frontier.sort(key=lambda x: x[0])
if res_flag[0] == 1:
break
#输出路径
p = target
path.append([target[0], target[1]])
while came_from[p[0] * 3.141 + p[1] * 2.727] != start:
path.append([came_from[p[0] * 3.141 + p[1] * 2.727][0], came_from[p[0] * 3.141 + p[1] * 2.727][1]])
p = came_from[p[0] * 3.141 + p[1] * 2.727]
path.append([start[0], start[1]])
came_from.clear()
cost_so_far.clear()
|
[/zd-plane]
