import heapq
def a_star(graph, start, goal, heuristic):
open_set = []
heapq.heappush(open_set, (heuristic[start], start)) # (f = g + h, node)
came_from = {} # To reconstruct the path
g_score = {start: 0} # Cost from start to each node
closed_set = set() # Set of visited nodes
while open_set:
_, current = heapq.heappop(open_set)
if current == goal:
return reconstruct_path(came_from, current)
closed_set.add(current)
for neighbor, cost in graph.get(current, []):
if neighbor in closed_set:
continue
tentative_g = g_score[current] + cost
if neighbor not in g_score or tentative_g < g_score[neighbor]:
came_from[neighbor] = current
g_score[neighbor] = tentative_g
f = tentative_g + heuristic.get(neighbor, float('inf'))
heapq.heappush(open_set, (f, neighbor))
return None # No path found
def reconstruct_path(came_from, node):
path = [node]
while node in came_from:
node = came_from[node]
path.append(node)
path.reverse()
return path
# --- MAIN PROGRAM STARTS HERE ---
# Define the graph (state-space)
graph = {
'A': [('B', 1), ('C', 4)],
'B': [('D', 2), ('E', 5)],
'C': [('F', 3)],
'D': [('G', 1)],
'E': [('G', 2)],
'F': [('G', 5)],
'G': []
}
# Define the heuristic values (h(n))
heuristic = {
'A': 7,
'B': 6,
'C': 5,
'D': 3,
'E': 2,
'F': 5,
'G': 0
}
# Define start and goal nodes
start_node = 'A'
goal_node = 'G'
# Run A* algorithm
path = a_star(graph, start_node, goal_node, heuristic)
# Display result
if path:
print("Optimal path found:", path)
else:
print("No path found from", start_node, "to", goal_node)