基于经纬坐标的双向A星算法的Python实现

以下是基于经纬坐标的双向A星算法的Python实现的代码：

# -*- coding: utf-8 -*-

import math
import heapq

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

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

class AStar:
    def __init__(self, start, end, obstacles):
        self.start = Node(start[0], start[1])
        self.end = Node(end[0], end[1])
        self.obstacles = obstacles

    def heuristic(self, node):
        dx = abs(node.x - self.end.x)
        dy = abs(node.y - self.end.y)
        return math.sqrt(dx*dx + dy*dy)

    def get_neighbors(self, node):
        neighbors = []
        for dx in [-1, 0, 1]:
            for dy in [-1, 0, 1]:
                if dx == 0 and dy == 0:
                    continue
                x = node.x + dx
                y = node.y + dy
                if (x, y) in self.obstacles:
                    continue
                neighbors.append(Node(x, y))
        return neighbors

    def search(self):
        open_list_start = [self.start]
        open_list_end = [self.end]
        closed_list_start = set()
        closed_list_end = set()

        while open_list_start and open_list_end:
            current_start = heapq.heappop(open_list_start)
            current_end = heapq.heappop(open_list_end)

            if current_start in closed_list_end or current_end in closed_list_start:
                return self.reconstruct_path(current_start, current_end)

            closed_list_start.add(current_start)
            closed_list_end.add(current_end)

            for neighbor in self.get_neighbors(current_start):
                if neighbor in closed_list_start:
                    continue
                tentative_g = current_start.g + 1
                if neighbor not in open_list_start:
                    heapq.heappush(open_list_start, neighbor)
                elif tentative_g >= neighbor.g:
                    continue
                neighbor.parent = current_start
                neighbor.g = tentative_g
                neighbor.h = self.heuristic(neighbor)
                neighbor.f = neighbor.g + neighbor.h

            for neighbor in self.get_neighbors(current_end):
                if neighbor in closed_list_end:
                    continue
                tentative_g = current_end.g + 1
                if neighbor not in open_list_end:
                    heapq.heappush(open_list_end, neighbor)
                elif tentative_g >= neighbor.g:
                    continue
                neighbor.parent = current_end
                neighbor.g = tentative_g
                neighbor.h = self.heuristic(neighbor)
                neighbor.f = neighbor.g + neighbor.h

        return None

    def reconstruct_path(self, current_start, current_end):
        path = []
        while current_start:
            path.append((current_start.x, current_start.y))
            current_start = current_start.parent
        path.reverse()
        current_end = current_end.parent
        while current_end:
            path.append((current_end.x, current_end.y))
            current_end = current_end.parent
        return path

# Example usage:
start = (0, 0)
end = (5, 5)
obstacles = [(1, 1), (2, 2), (3, 3)]
astar = AStar(start, end, obstacles)
path = astar.search()
print(path)

希望这个代码能够帮到你！