Python实现A*（Astar）算法寻找最短路径

使用

for item in M:

只可获取副本，无法改变M中值

应使用

for index, item in enumerate(M):
        M[index]=xxxxxxx

改变item即可改变M中值

import math

a = 5  # 矩阵长度
b = 5  # 矩阵宽度


def init(listx):
    # a=int(input("x of target:"))
    # b=int(input("y of target:"))
    for i in range(0, a):
        listx.append(b * [0])
    pass


def barrier(listx):
    print("请输入障碍的坐标，以负值结束输入")
    while 1:
        barx = int(input("请输入障碍的x坐标:"))
        bary = int(input("请输入障碍的y坐标:"))
        if barx >= 0 and bary >= 0:
            listx[barx][bary] = 1
        else:
            break
    pass


def get_h(now_node, end_node):
    return math.sqrt(pow((end_node.x - now_node.x), 2) + pow((end_node.y - now_node.y), 2))


def path_out(s_node, G):
    print("path", end=":")

    for item in G:
        if item.x == s_node.x and item.y == s_node.y:
            item.p_out()
    pass


def A_star(listx, beg_node, end_node):
    beg_node.h = get_h(beg_node, end_node)
    beg_node.get_f()  # 初始化开始节点，赋f值
    open = [beg_node]  # open表
    close = []  # close表

    class search_node:
        def __init__(self, in_x, in_y, in_pre):
            self.x = in_x
            self.y = in_y
            self.pre = in_pre

        def p_out(self):
            print("<-(%d,%d)" % (self.x, self.y), end="")
            if self.pre != None:
                self.pre.p_out()
            else:
                print("\n")
            pass

    G = [search_node(beg_node.x, beg_node.y, None)]  # 搜索过的点
    M = []  # 产生的新节点集合
    while len(open) != 0:
        min_f = open[0].f
        min_node = open[0]

        print("-----new-------cycle-----\nopen:")
        for item in open:
            item.n_out()
        print("finish open")  # 打印open表

        for item in open:
            if item.f < min_f:
                min_f = item.f
                min_node = item

        print("find min_node:", end=" ")  # 寻找f最小的节点并打印
        min_node.n_out()

        open.remove(min_node)
        close.append(min_node)
        min_s_node = search_node(min_node.x, min_node.y, None)

        if min_node.x == end_node.x and min_node.y == end_node.y:
            path_out(min_s_node, G)  # 找到目标节点，输出路径
            return

        if min_node.x - 1 >= 0 and listx[min_node.x - 1][min_node.y] == 0:  # 上节点
            new_node = node(min_node.x - 1, min_node.y, min_node)
            new_node.g = min_node.g + 1
            new_node.h = get_h(new_node, end_node)
            new_node.get_f()
            if not (new_node.x == min_node.father.x and new_node.y == min_node.father.y):  # 子节点不与父节点相同
                M.append(new_node)
        if min_node.y - 1 >= 0 and listx[min_node.x][min_node.y - 1] == 0:  # 左节点
            new_node = node(min_node.x, min_node.y - 1, min_node)
            new_node.g = min_node.g + 1
            new_node.h = get_h(new_node, end_node)
            new_node.get_f()
            if not (new_node.x == min_node.father.x and new_node.y == min_node.father.y):
                M.append(new_node)
        if min_node.x + 1 <= a - 1 and listx[min_node.x + 1][min_node.y] == 0:  # 下节点
            new_node = node(min_node.x + 1, min_node.y, min_node)
            new_node.g = min_node.g + 1
            new_node.h = get_h(new_node, end_node)
            new_node.get_f()
            if not (new_node.x == min_node.father.x and new_node.y == min_node.father.y):
                M.append(new_node)
        if min_node.y + 1 <= b - 1 and listx[min_node.x][min_node.y + 1] == 0:  # 右节点
            new_node = node(min_node.x, min_node.y + 1, min_node)
            new_node.g = min_node.g + 1
            new_node.h = get_h(new_node, end_node)
            new_node.get_f()
            if not (new_node.x == min_node.father.x and new_node.y == min_node.father.y):
                M.append(new_node)

        print("M:")  # 打印M表
        for item in M:
            item.n_out()
        print("finish M")

        for index, item in enumerate(M):
            find = False
            for g_index, g_item in enumerate(G):  # 对M中每个元素，在G中查找
                if g_item.x == item.x and g_item.y == item.y:
                    find = True
                    if min_node.g + 1 < item.g:
                        g_item.pre = min_s_node
                        item.g = min_node.g + 1
                        item.get_f()  # 找到:比较f值，取较小者
            if not find:
                open.append(item)  # 未找到：入open表
                for g_index, g_item in enumerate(G):
                    if g_item.x == min_s_node.x and g_item.y == min_s_node.y:
                        new_search_node = search_node(item.x, item.y, g_item)  # 生成路径点
                        G.append(new_search_node)

        print("G:")  # 打印G表
        for item in G:
            print("(%d,%d)" % (item.x, item.y))
        print("finish G")

        M.clear()
    print("找不到路径！")
    return


class node:
    def __init__(self, in_x, in_y, in_father):
        self.x = in_x
        self.y = in_y
        self.g = 0
        self.h = 0.0
        self.f = self.g + self.h
        self.father = in_father

    def get_f(self):
        self.f = self.g + self.h  # 计算节点f值

    def n_out(self):
        print("(%d,%d),f=%f" % (self.x, self.y, self.f))  # 打印节点属性
        return


listx = []
init(listx)  # 初始化，得5x5矩阵
barrier(listx)  # 初始化，矩阵中加入障碍物
print(listx)

beg_node = node(int(input("请输入起点的x坐标:")), int(input("请输入起点的y坐标:")), None)
beg_node.father = beg_node
end_node = node(int(input("请输入终点的x坐标:")), int(input("请输入终点的y坐标:")), None)

A_star(listx, beg_node, end_node)