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]