A*算法python实现

好的，以下是A*算法的Python实现代码：

import heapq

class Node:
    def __init__(self, x, y, reachable=True):
        self.x = x
        self.y = y
        self.reachable = reachable
        self.parent = None
        self.g = 0
        self.h = 0
        self.f = 0

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

class AStar:
    def __init__(self, grid):
        self.open = []
        heapq.heapify(self.open)
        self.close = set()
        self.grid = grid

    def find_path(self, start, end):
        start_node = Node(*start)
        end_node = Node(*end)
        heapq.heappush(self.open, start_node)
        while len(self.open) > 0:
            current_node = heapq.heappop(self.open)
            self.close.add(current_node)
            if current_node.x == end_node.x and current_node.y == end_node.y:
                path = []
                while current_node.parent:
                    path.append((current_node.x, current_node.y))
                    current_node = current_node.parent
                path.append((start_node.x, start_node.y))
                return path[::-1]
            neighbors = self.get_neighbors(current_node)
            for neighbor in neighbors:
                if neighbor not in self.close:
                    if neighbor not in self.open:
                        neighbor.g = current_node.g + 1
                        neighbor.h = self.get_distance(neighbor, end_node)
                        neighbor.f = neighbor.g + neighbor.h
                        neighbor.parent = current_node
                        heapq.heappush(self.open, neighbor)
                    else:
                        new_g = current_node.g + 1
                        if new_g < neighbor.g:
                            neighbor.g = new_g
                            neighbor.f = neighbor.g + neighbor.h
                            neighbor.parent = current_node
        return None

    def get_distance(self, node, end):
        return abs(node.x - end.x) + abs(node.y - end.y)

    def get_neighbors(self, node):
        neighbors = []
        for x, y in [(0, 1), (0, -1), (1, 0), (-1, 0)]:
            new_x = node.x + x
            new_y = node.y + y
            if new_x < 0 or new_x >= len(self.grid) or new_y < 0 or new_y >= len(self.grid[0]):
                continue
            if not self.grid[new_x][new_y].reachable:
                continue
            neighbors.append(Node(new_x, new_y))
        return neighbors

这里定义了一个`Node`类来表示网格中的一个节点，包含了节点的坐标、是否可以到达、父节点、G值、H值和F值。其中，G值表示从起始点到当前节点的实际距离，H值表示从当前节点到终点的估计距离，F值是G值和H值的和，用来决定下一个要扩展的节点。在实现中，使用了一个`open`列表来存储待扩展的节点，并按F值从小到大排序；使用一个`close`集合来存储已经扩展过的节点，避免重复扩展。在每次扩展时，更新节点的G值、H值和F值，并将其加入到`open`列表中。最后，如果找到了终点，则从终点开始回溯，得到路径。

具体使用方法可以参考以下代码：

grid = [[Node(x, y) for y in range(10)] for x in range(10)]
grid[2][3].reachable = False
grid[3][3].reachable = False
start = (0, 0)
end = (9, 9)
astar = AStar(grid)
path = astar.find_path(start, end)
if path:
    print(path)
else:
    print("No path found.")

这里构造了一个10x10的网格，并将第(2,3)和(3,3)个节点设置为不可达。然后，定义了起始点和终点，并创建了一个`AStar`对象。最后，调用`find_path`方法来查找路径，如果找到了路径，则打印出来，否则打印"No path found."。