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)]
This implementation is for a simple undirected graph, but it should be fairly easy to adapt to different types of graphs. The heuristic_estimate
function used in this example is a simple Manhattan distance calculation, but you can substitute any other admissible heuristic function.