def find_path(self, environment: GridEnvironment, start: Tuple[int, int],
end: Tuple[int, int]) -> Union[List[Tuple[int, int]], None]:
# Use heapq;the thread safety provided by ProrityQueue is not needed, as we only exec on a single thread
open = []
heappush(open, (0, start))
closed = {start: None}
costs = np.full(environment.grid.shape, np.inf)
costs[start[0], start[1]] = 0
while open:
node = heappop(open)[1]
if node == end:
import matplotlib.pyplot as mpl
mpl.matshow(costs)
mpl.show()
return self._reconstruct_path(end, closed, environment.grid)
current_cost = costs[node[0], node[1]]
for neighbour in environment.get_neighbours(node):
x, y = neighbour[1], neighbour[0]
cost = current_cost + environment.f_cost(node, neighbour)
if costs[y, x] > cost:
costs[y, x] = cost
heappush(
open,
(cost + self.heuristic(neighbour, end), neighbour))
closed[neighbour] = node
return None
def find_path(self, environment: GridEnvironment, start: Tuple[int, int],
end: Tuple[int, int]) -> Union[List[Tuple[int, int]], None]:
if not environment.diagonals:
raise ValueError('JPS relies on a grid environment with diagonals')
self.environment = environment
self._max_y, self._max_x = self.environment.grid.shape[
0] - 1, self.environment.grid.shape[1] - 1
self.goal = end
# Use heapq;the thread safety provided by ProrityQueue is not needed, as we only exec on a single thread
open = []
heappush(open, (0, start))
closed = {start: None}
costs = np.full(environment.grid.shape, np.inf)
costs[start[0], start[1]] = 0
# if __debug__:
# debug_heuristic_cost = np.full(environment.grid.shape, np.inf)
while open:
node = heappop(open)[1]
if node == end:
# if __debug__:
# import matplotlib.pyplot as mpl
# mpl.matshow(costs)
# mpl.matshow(debug_heuristic_cost)
# mpl.show()
return self._reconstruct_path(end, closed, environment.grid)
cy, cx = node[0], node[1]
current_cost = costs[cy, cx]
successors = []
for neighbour in environment.get_neighbours(node):
dx, dy = neighbour[1] - cx, neighbour[0] - cy
jumpPoint = self._jump(cy, cx, dy, dx)
if jumpPoint:
successors.append(jumpPoint)
for successor in successors:
x, y = successor[1], successor[0]
cost = current_cost + environment.f_cost(node, successor)
if costs[y, x] > cost:
costs[y, x] = cost
h = self.heuristic(successor, end)
heappush(open, (cost + h, successor))
closed[successor] = node
# if __debug__:
# debug_heuristic_cost[y, x] = h
return None