[Python] 【A*/A star】启发式路径搜索算法

'''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()