Astar算法三维Python实现

以下是实现Astar算法三维Python代码：

import heapq

# 定义节点类
class Node:
    def __init__(self, x, y, z, g=0, h=0):
        self.x = x
        self.y = y
        self.z = z
        self.g = g  # 从起点到该节点的实际代价
        self.h = h  # 从该节点到终点的估计代价
        self.f = g + h  # 节点的总代价
        self.parent = None  # 父节点

    def __lt__(self, other):
        return self.f < other.f

    def __eq__(self, other):
        return self.x == other.x and self.y == other.y and self.z == other.z

# 定义Astar算法函数
def astar(maze, start, end):
    open_list = []  # 未考虑的节点列表
    closed_list = set()  # 已考虑的节点集合

    # 将起点加入open_list
    heapq.heappush(open_list, start)

    # 循环直到open_list为空
    while open_list:
        # 取出f值最小的节点
        current_node = heapq.heappop(open_list)

        # 如果当前节点是终点，则返回路径
        if current_node == end:
            path = []
            while current_node:
                path.append((current_node.x, current_node.y, current_node.z))
                current_node = current_node.parent
            path.reverse()
            return path

        # 将当前节点加入closed_list
        closed_list.add(current_node)

        # 遍历当前节点的相邻节点
        for dx, dy, dz in [(0, 0, 1), (0, 0, -1), (0, 1, 0), (0, -1, 0), (1, 0, 0), (-1, 0, 0)]:
            next_x, next_y, next_z = current_node.x + dx, current_node.y + dy, current_node.z + dz

            # 如果相邻节点不在迷宫内，则忽略
            if not (0 <= next_x < len(maze) and 0 <= next_y < len(maze[0]) and 0 <= next_z < len(maze[0][0])):
                continue

            # 如果相邻节点是障碍物，则忽略
            if maze[next_x][next_y][next_z] == 1:
                continue

            # 计算从起点到相邻节点的实际代价
            g = current_node.g + 1

            # 计算从相邻节点到终点的估计代价
            h = ((next_x - end.x) ** 2 + (next_y - end.y) ** 2 + (next_z - end.z) ** 2) ** 0.5

            # 如果相邻节点已经在closed_list中，则忽略
            next_node = Node(next_x, next_y, next_z, g, h)
            if next_node in closed_list:
                continue

            # 如果相邻节点已经在open_list中，则更新其实际代价和估计代价
            for node in open_list:
                if node == next_node:
                    if node.g > next_node.g:
                        open_list.remove(node)
                        heapq.heapify(open_list)
                    else:
                        next_node = None
                    break

            # 将相邻节点加入open_list
            if next_node:
                next_node.parent = current_node
                heapq.heappush(open_list, next_node)

    # 如果open_list为空且没有找到终点，则返回None
    return None

上述代码中，我们定义了一个`Node`类来表示节点，其中包含了节点的坐标、从起点到该节点的实际代价、从该节点到终点的估计代价、节点的总代价以及父节点等信息。我们还定义了一个`astar`函数来实现Astar算法，其中使用了一个`open_list`列表来存储未考虑的节点，使用了一个`closed_list`集合来存储已考虑的节点。在每次循环中，我们从`open_list`中取出f值最小的节点，将其加入`closed_list`，然后遍历其相邻节点，计算其实际代价和估计代价，并将其加入`open_list`中。如果相邻节点已经在`closed_list`中，则忽略；如果相邻节点已经在`open_list`中，则更新其实际代价和估计代价。最终，如果`open_list`为空且没有找到终点，则返回None。如果找到了终点，则返回路径。