编辑代码

from datetime import datetime
 
class Node:
    def __init__(self, data, level, parent, score):
        self.data = data  # 节点坐标 (row, col)
        self.level = level  # 节点深度
        self.parent = parent  # 父节点
        self.score = score  # 评估函数值 f = g + h
 
# 计算S0位置启发函数的值，曼哈顿距离
def Atest_g(S0, Sg):
    x0, y0 = S0
    xg, yg = Sg
    g = abs(x0 - xg) + abs(y0 - yg)
    return g
 
# 冒泡排序，将open表中的节点根据评估函数的值进行排序，从小到大排序
def Asort(open):
    n = len(open)
    for i in range(n):
        for j in range(0, n-i-1):
            if open[j].score > open[j+1].score:
                open[j], open[j+1] = open[j+1], open[j]
    return open
 
if __name__ == "__main__":
    # 初始化迷宫
    maze = [
        [1, 0, 1, 0, 1],
        [0, 0, 0, 0, 1],
        [1, 0, 0, 0, 0],
        [0, 0, 0, 1, 0]
    ]
 
    rows = len(maze)
    cols = len(maze[0])
 
    # 初始化起点和终点
    S0 = (0, 0)
    Sg = (4, 4)
 
    # 初始化起点节点
    Node0 = Node(S0, 0, None, Atest_g(S0, Sg))
    open_list = [Node0]
    close_list = []
    step = 0
    start = datetime.now()
 
    while len(open_list) > 0:
        step += 1
        n = open_list.pop(0)  # 取出open表中的第1个节点并删除
        row, col = n.data
        close_list.append(n)  # 将节点n添加到close表中
 
        if n.data == Sg:
            print(n.data, '搜索完毕')
            result = []
            result.append(n)
            while n.parent is not None:
                result.append(n.parent.data)
                n = n.parent
 
            # 输出解路径
            print("路径:", result[::-1])
            print("--------结束搜索--------")
            break
        else:
            # 四个方向的移动 (下, 右, 左, 上)
            directions = [(1, 0), (0, 1), (-1, 0), (0, -1)]
            for dx, dy in directions:
                new_row, new_col = row + dx, col + dy
                if 0 <= new_row < rows and 0 <= new_col < cols and maze[new_row][new_col] == 0:
                    new_node_data = (new_row, new_col)
                    if new_node_data not in [node.data for node in close_list]:  # 不在close表中
                        new_node_parent = n
                        new_node_level = n.level + 1
                        new_node_score = n.level + 1 + Atest_g(new_node_data, Sg)  # f = g + h
                        new_node = Node(new_node_data, new_node_level, new_node_parent, new_node_score)
 
                        # 如果新节点不在open表中，添加到open表中
                        if not any(new_node_data == node.data for node in open_list):
                            open_list.append(new_node)
 
            # 对open表进行排序
            open_list = Asort(open_list)
 
        print("第" + str(step) + "次查找，中间项为：", n.data, "深度为：", n.level, "估价值为：", n.score)
    else:
        print("搜索失败！")
 
    print("扩展节点个数：", len(close_list))
    print("生成节点个数:", len(open_list) + len(close_list))  # 因为open_list在循环结束时可能不为空
    end = datetime.now()
    print('共耗时：', end - start)