a-star algorithm in python

main.py
import heapq

class Graph:
    def __init__(self):
        self.nodes = set()
        self.edges = {}

    def add_node(self, node):
        self.nodes.add(node)
        self.edges[node] = {}

    def add_edge(self, u, v, weight):
        self.edges[u][v] = weight
        self.edges[v][u] = weight

def A_star(graph, start, goal):
    # Create dicts for distances, heuristic estimates, and previous nodes
    distances = {node: float('inf') for node in graph.nodes}
    heuristic = {node: heuristic_estimate(node, goal) for node in graph.nodes}
    previous = {node: None for node in graph.nodes}

    # Set up priority queue
    heap = []

    # Set up start node
    distances[start] = 0
    heapq.heappush(heap, (heuristic[start], start))

    # Loop until goal is found or queue is empty
    while heap:
        # Get node with lowest heuristic estimate
        current_heuristic, current_node = heapq.heappop(heap)

        # Check if goal has been reached
        if current_node == goal:
            path = []
            while current_node:
                path.append(current_node)
                current_node = previous[current_node]
            return path[::-1]

        # Loop through neighbors of current node
        for neighbor, weight in graph.edges[current_node].items():
            # Calculate tentative distance to neighbor
            tentative_distance = distances[current_node] + weight

            # Check if tentative distance is better than current distance
            if tentative_distance < distances[neighbor]:
                distances[neighbor] = tentative_distance
                previous[neighbor] = current_node
                priority = tentative_distance + heuristic[neighbor]
                heapq.heappush(heap, (priority, neighbor))

    # If this point is reached, no path was found
    return None

def heuristic_estimate(node, goal):
    # Calculate Manhattan distance as heuristic estimate
    dx = abs(node[0] - goal[0])
    dy = abs(node[1] - goal[1])
    return dx + dy

# Example usage
graph = Graph()
graph.add_node((0,0))
graph.add_node((0,1))
graph.add_node((1,0))
graph.add_node((1,1))
graph.add_edge((0,0), (0,1), 1)
graph.add_edge((0,0), (1,0), 1)
graph.add_edge((0,1), (1,1), 3)
graph.add_edge((1,0), (1,1), 1)
print(A_star(graph, (0,0), (1,1)))  # Prints [(0, 0), (1, 0), (1, 1)]
2343 chars
74 lines