python实现插队算法

插队算法（Jump Point Search）是一种用于寻找网格图中最短路径的优化算法，可以减少搜索的时间和空间复杂度。以下是Python实现：

首先，定义节点类：

class Node:
    def __init__(self, x, y, g=0, h=0):
        self.x = x
        self.y = y
        self.g = g  # 实际代价
        self.h = h  # 启发式代价
        self.parent = None

    def f(self):
        return self.g + self.h

然后，实现插队算法：

def jump_point_search(grid, start, end):
    open_list = PriorityQueue()
    open_list.put(start)
    closed_list = set()
    while not open_list.empty():
        current = open_list.get()
        if current == end:
            path = []
            while current:
                path.append(current)
                current = current.parent
            return path[::-1]

        closed_list.add(current)

        # 找到当前节点的所有邻居节点，并对其进行处理
        for neighbor in neighbors(grid, current):
            if neighbor in closed_list:
                continue

            # 计算邻居节点的实际代价和启发式代价
            new_g = current.g + cost(current, neighbor)
            new_h = heuristic(neighbor, end)

            # 如果邻居节点已经在open_list中，则更新其代价和父节点
            if neighbor in open_list.queue:
                for n in open_list.queue:
                    if n == neighbor and n.f() > new_g + new_h:
                        n.g = new_g
                        n.h = new_h
                        n.parent = current
                        open_list = PriorityQueue(sorted(open_list.queue, key=lambda x: x.f()))
                        break
            else:
                neighbor.g = new_g
                neighbor.h = new_h
                neighbor.parent = current
                open_list.put(neighbor)

    return None

其中，`neighbors()`函数用于获取当前节点的所有邻居节点，`cost()`函数用于计算两个节点之间的实际代价，`heuristic()`函数用于计算两个节点之间的启发式代价。

完整代码如下所示：

from queue import PriorityQueue
import math


class Node:
    def __init__(self, x, y, g=0, h=0):
        self.x = x
        self.y = y
        self.g = g  # 实际代价
        self.h = h  # 启发式代价
        self.parent = None

    def f(self):
        return self.g + self.h


def neighbors(grid, node):
    """
    获取当前节点的所有邻居节点
    """
    neighbors = []
    for dx in [-1, 0, 1]:
        for dy in [-1, 0, 1]:
            if dx == dy == 0:
                continue

            x, y = node.x + dx, node.y + dy

            if x < 0 or x >= len(grid) or y < 0 or y >= len(grid[0]) or grid[x][y]:
                continue

            if dx != 0 and dy != 0:  # 斜向移动
                if grid[x - dx][y] or grid[x][y - dy]:
                    continue

            neighbors.append(Node(x, y))

    return neighbors


def cost(node1, node2):
    """
    计算两个节点之间的实际代价
    """
    dx = abs(node1.x - node2.x)
    dy = abs(node1.y - node2.y)
    return math.sqrt(dx ** 2 + dy ** 2)


def heuristic(node1, node2):
    """
    计算两个节点之间的启发式代价
    """
    dx = abs(node1.x - node2.x)
    dy = abs(node1.y - node2.y)
    return min(dx, dy) * math.sqrt(2) + abs(dx - dy)


def jump_point_search(grid, start, end):
    open_list = PriorityQueue()
    open_list.put(start)
    closed_list = set()
    while not open_list.empty():
        current = open_list.get()
        if current == end:
            path = []
            while current:
                path.append(current)
                current = current.parent
            return path[::-1]

        closed_list.add(current)

        # 找到当前节点的所有邻居节点，并对其进行处理
        for neighbor in neighbors(grid, current):
            if neighbor in closed_list:
                continue

            # 计算邻居节点的实际代价和启发式代价
            new_g = current.g + cost(current, neighbor)
            new_h = heuristic(neighbor, end)

            # 如果邻居节点已经在open_list中，则更新其代价和父节点
            if neighbor in open_list.queue:
                for n in open_list.queue:
                    if n == neighbor and n.f() > new_g + new_h:
                        n.g = new_g
                        n.h = new_h
                        n.parent = current
                        open_list = PriorityQueue(sorted(open_list.queue, key=lambda x: x.f()))
                        break
            else:
                neighbor.g = new_g
                neighbor.h = new_h
                neighbor.parent = current
                open_list.put(neighbor)

    return None


if __name__ == '__main__':
    grid = [
        [0, 0, 0, 0, 1, 0],
        [1, 1, 0, 0, 0, 0],
        [0, 0, 0, 1, 0, 0],
        [0, 0, 0, 1, 0, 1],
        [0, 0, 0, 0, 0, 0],
        [0, 0, 0, 0, 1, 0]
    ]

    start = Node(0, 0)
    end = Node(5, 5)

    path = jump_point_search(grid, start, end)
    if path:
        for node in path:
            print((node.x, node.y))
    else:
        print("No path found.")

这是一个6x6的网格图，其中0表示可以通过的点，1表示障碍物。程序会输出起点到终点的最短路径。