A*算法【python】动画演示

import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import matplotlib.animation as animation
a = []
b = []
path_len = 0
def update_points(num):
    point_ani.set_data(a[num], b[num])
    xdata.append(a[num])
    ydata.append(b[num])
    if  num % path_len==0:
        xdata.clear()
        ydata.clear()

    # if num % 5 == 0:
    #     # point_ani.set_marker("*")
    #     point_ani.set_markersize(12)
    # elif num % 2== 0:
    #     # point_ani.set_marker("o")
    #     point_ani.set_markersize(8)
    # else:
    #     point_ani.set_markersize(5)

    #text_pt.set_position((a[num], b[num]))
    #text_pt.set_text("x=%.3f, y=%.3f" % (a[num], b[num]))
    point_ani.set_data(ydata, xdata)
    return point_ani, #text_pt,




import numpy as np
from heapq import heappush, heappop

import random
from PIL import Image
import matplotlib.pyplot as plt
def heuristic_cost_estimate(neighbor, goal):
    x = neighbor[0] - goal[0]
    y = neighbor[1] - goal[1]
    return abs(x) + abs(y)


def dist_between(a, b):
    return (b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2


def reconstruct_path(came_from, current):
    path = [current]
    while current in came_from:
        current = came_from[current]
        path.append(current)
    return path


# astar function returns a list of points (shortest path)
def astar(array, start, goal):
    directions = [(0, 1), (0, -1), (1, 0), (-1, 0)]#, (1, 1), (1, -1), (-1, 1), (-1, -1)]  # 8个方向

    close_set = set()
    came_from = {}
    gscore = {start: 0}
    fscore = {start: heuristic_cost_estimate(start, goal)}

    openSet = []
    heappush(openSet, (fscore[start], start))  # 往堆中插入一条新的值

    # while openSet is not empty
    while openSet:
        # current := the node in openSet having the lowest fScore value
        current = heappop(openSet)[1]  # 从堆中弹出fscore最小的节点

        if current == goal:
            return reconstruct_path(came_from, current)

        close_set.add(current)

        for i, j in directions:  # 对当前节点的 8 个相邻节点一一进行检查
            neighbor = current[0] + i, current[1] + j

            ## 判断节点是否在地图范围内，并判断是否为障碍物
            if 0 <= neighbor[0] < array.shape[0]:
                if 0 <= neighbor[1] < array.shape[1]:
                    if array[neighbor[0]][neighbor[1]] == 1:  # 1为障碍物
                        continue
                else:
                    # array bound y walls
                    continue
            else:
                # array bound x walls
                continue

            # Ignore the neighbor which is already evaluated.
            if neighbor in close_set:
                continue

            #  The distance from start to a neighbor via current
            tentative_gScore = gscore[current] + dist_between(current, neighbor)

            if neighbor not in [i[1] for i in openSet]:  # Discover a new node
                heappush(openSet, (fscore.get(neighbor, np.inf), neighbor))
            elif tentative_gScore >= gscore.get(neighbor, np.inf):  # This is not a better path.
                continue

                # This path is the best until now. Record it!
            came_from[neighbor] = current
            gscore[neighbor] = tentative_gScore
            fscore[neighbor] = tentative_gScore + heuristic_cost_estimate(neighbor, goal)

    return False


if __name__ == "__main__":
    #mapsize = tuple(map(int, input('请输入地图大小，以逗号隔开：').split(',')))
    mapsize = (24,20)
    nmap = np.zeros(mapsize, dtype=np.int)
    map_weight = nmap.shape[0]
    map_height = nmap.shape[1]
    # 定义起点和重点
    start_node = (0,0)
    end_node = (map_weight-1,map_height-1)

    # 随机生成地图
    for i in range(int(0.25*map_weight*map_height)):
        xi = random.randint(0, map_weight-1)
        yi = random.randint(0, map_height-1)
        nmap[xi][yi] = 1
    print(nmap)
    nmap[start_node] = 0
    nmap[end_node] = 0

    path = astar(nmap, start_node, end_node) # 用A星算法生成路径
    path_len = len(path)
    for i in range(path_len):
        # nmap[path[i]]  = 4 #设置路径的颜色
        a.append(path[i][0])
        b.append(path[i][1])

    nmap[start_node] = 3 #设置开始点的颜色
    nmap[end_node] = 2  # 设置终点的颜色
    print(a)
    print(b)

    plt.scatter(b,a)
    # 绘制路径
    img = np.array(nmap)
    plt.imshow(img)
 #   plt.axis('off')


# 动画演示
xdata = []
ydata = [] 

fig = plt.figure(tight_layout=True)

point_ani = plt.axes(xlim=(-1, mapsize[1]), ylim=(mapsize[0], -1))


point_ani, = plt.plot(ydata,xdata, "ro")

#text_pt = plt.text(4, 0.8, '', fontsize=16)
im = plt.imshow(img)
ani = animation.FuncAnimation(fig, update_points, np.arange(0,len(a)-1), interval=100, blit=True)

# ani.save('sin_test3.gif', writer='imagemagick', fps=10)
plt.show()