import heapq
def a_star_search(graph, start, goal, heuristic):
# Priority queue to hold nodes and their f-scores
priority_queue = [(0, start)] # (f-score, node)
came_from = {} # To reconstruct the path
g_score = {start: 0} # Cost to reach each node from start
f_score = {start: heuristic[start]} # Estimated total cost (g + h)
while priority_queue:
_, current = heapq.heappop(priority_queue)
# If goal is reached, reconstruct and return the path and cost
if current == goal:
path = []
while current in came_from:
path.append(current)
current = came_from[current]
path.append(start)
path.reverse()
return path, g_score[goal]
# Explore neighbors
for neighbor, cost in graph.get(current, []):
tentative_g_score = g_score[current] + cost
if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
# Update the path and scores
came_from[neighbor] = current
g_score[neighbor] = tentative_g_score
f_score[neighbor] = tentative_g_score + heuristic[neighbor]
heapq.heappush(priority_queue, (f_score[neighbor], neighbor))
return None, float('inf') # If no path is found
# Example usage:
graph = {
'A': [('B', 1), ('C', 4)],
'B': [('A', 1), ('D', 2), ('E', 5)],
'C': [('A', 4), ('F', 3)],
'D': [('B', 2), ('E', 1)],
'E': [('B', 5), ('D', 1), ('F', 2)],
'F': [('C', 3), ('E', 2)]
}
# Heuristic values (straight-line distance to goal)
heuristic = {
'A': 7,
'B': 6,
'C': 2,
'D': 1,
'E': 1,
'F': 0
}
start_node = 'A'
goal_node = 'F'
path, cost = a_star_search(graph, start_node, goal_node, heuristic)
print(f"Path: {path}")
print(f"Cost: {cost}")
Comments
Post a Comment