a*算法 python

以下是A*算法的Python实现示例：

import heapq

class PriorityQueue:
    def __init__(self):
        self.elements = []
    
    def empty(self):
        return len(self.elements) == 0
    
    def put(self, item, priority):
        heapq.heappush(self.elements, (priority, item))
    
    def get(self):
        return heapq.heappop(self.elements)[1]

def heuristic(a, b):
    # 曼哈顿距离
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def astar(graph, start, goal):
    frontier = PriorityQueue()
    frontier.put(start, 0)
    came_from = {}
    cost_so_far = {}
    came_from[start] = None
    cost_so_far[start] = 0
    
    while not frontier.empty():
        current = frontier.get()
        
        if current == goal:
            break
        
        for next in graph.neighbors(current):
            new_cost = cost_so_far[current] + graph.cost(current, next)
            if next not in cost_so_far or new_cost < cost_so_far[next]:
                cost_so_far[next] = new_cost
                priority = new_cost + heuristic(goal, next)
                frontier.put(next, priority)
                came_from[next] = current
    
    return came_from, cost_so_far

需要实现以下接口：

- `graph.neighbors(node)` 返回与给定节点相邻的节点列表。
- `graph.cost(from_node, to_node)` 返回从一个节点到另一个节点的成本。

示例代码中使用了一个优先队列来保存待处理节点，并使用曼哈顿距离作为启发式函数。在主循环中，每次从队列中取出成本最小的节点，并处理它的邻居。如果邻居节点是第一次被发现，将它加入队列，并更新到达它的成本和前驱。如果邻居节点已经被发现，但是新的路径成本更小，则更新成本和前驱。最后，如果目标节点被发现，则返回前驱和到达成本。