37 lines
1.1 KiB
Python
37 lines
1.1 KiB
Python
|
from typing import Optional, Dict, Tuple
|
||
|
|
|
||
|
|
from util.PathDefinitions import WeightedGraph, Location
|
||
|
|
from util.PriorityQueue import PriorityQueue
|
||
|
|
|
||
|
|
|
||
|
|
def heuristic(a: Tuple[int, int], b: Tuple[int, int]) -> float:
|
||
|
|
(x1, y1) = a
|
||
|
|
|
||
|
|
(x2, y2) = b
|
||
|
|
return abs(x1 - x2) + abs(y1 - y2)
|
||
|
|
|
||
|
|
|
||
|
|
def a_star_search(graph: WeightedGraph, start: Tuple[int, int], goal: Tuple[int, int]):
|
||
|
|
frontier = PriorityQueue()
|
||
|
|
frontier.put(start, 0)
|
||
|
|
came_from: Dict[Location, Optional[Location]] = {}
|
||
|
|
cost_so_far: Dict[Location, float] = {}
|
||
|
|
came_from[start] = None
|
||
|
|
cost_so_far[start] = 0
|
||
|
|
|
||
|
|
while not frontier.empty():
|
||
|
|
current: Location = frontier.get()
|
||
|
|
|
||
|
|
if current == goal:
|
||
|
|
break
|
||
|
|
|
||
|
|
for next in graph.neighbors(current):
|
||
|
|
new_cost = cost_so_far[current] + graph.cost(current, next)
|
||
|
|
if next not in cost_so_far or new_cost < cost_so_far[next]:
|
||
|
|
cost_so_far[next] = new_cost
|
||
|
|
priority = new_cost + heuristic(next, goal)
|
||
|
|
frontier.put(next, priority)
|
||
|
|
came_from[next] = current
|
||
|
|
|
||
|
|
return came_from, cost_so_far.keys()
|