107 lines
2.4 KiB
Python
107 lines
2.4 KiB
Python
import heapq
|
|
from itertools import count
|
|
from strategies.strategy import PathFindingStrategy
|
|
|
|
|
|
class AStarStrategy(PathFindingStrategy):
|
|
|
|
def _estimate(self, current, target):
|
|
return (
|
|
abs(current.x - target.x)
|
|
+ abs(current.y - target.y)
|
|
)
|
|
|
|
def findPath(self, maze, start, exit):
|
|
|
|
frontier = []
|
|
|
|
sequence = count()
|
|
|
|
heapq.heappush(
|
|
frontier,
|
|
(
|
|
self._estimate(start, exit),
|
|
next(sequence),
|
|
0,
|
|
start
|
|
)
|
|
)
|
|
|
|
ancestry = {}
|
|
|
|
travel_cost = {
|
|
start.getPosition(): 0
|
|
}
|
|
|
|
explored = set()
|
|
|
|
explored_count = 0
|
|
|
|
while frontier:
|
|
|
|
_, _, spent, current = heapq.heappop(
|
|
frontier
|
|
)
|
|
|
|
current_mark = current.getPosition()
|
|
|
|
if current_mark in explored:
|
|
continue
|
|
|
|
explored.add(current_mark)
|
|
explored_count += 1
|
|
|
|
if current == exit:
|
|
break
|
|
|
|
for neighbor in maze.getNeighbors(current):
|
|
|
|
mark = neighbor.getPosition()
|
|
|
|
new_cost = spent + 1
|
|
|
|
if (
|
|
mark not in travel_cost
|
|
or new_cost < travel_cost[mark]
|
|
):
|
|
|
|
travel_cost[mark] = new_cost
|
|
ancestry[mark] = current
|
|
|
|
priority = (
|
|
new_cost
|
|
+ self._estimate(
|
|
neighbor,
|
|
exit
|
|
)
|
|
)
|
|
|
|
heapq.heappush(
|
|
frontier,
|
|
(
|
|
priority,
|
|
next(sequence),
|
|
new_cost,
|
|
neighbor
|
|
)
|
|
)
|
|
|
|
if (
|
|
exit.getPosition() not in ancestry
|
|
and exit != start
|
|
):
|
|
return [], explored_count
|
|
|
|
route = []
|
|
cursor = exit
|
|
|
|
while cursor != start:
|
|
route.append(cursor)
|
|
cursor = ancestry[
|
|
cursor.getPosition()
|
|
]
|
|
|
|
route.append(start)
|
|
route.reverse()
|
|
|
|
return route, explored_count |