Implements Dijkstra's algorithm on a binary grid. Args: grid (np.ndarray): A 2D numpy array representing the grid. 1 represents a walkable node and 0 represents an obstacle. source (Tuple[int, int]): A tuple representing the start node. destination (Tuple[in
(
grid: np.ndarray,
source: tuple[int, int],
destination: tuple[int, int],
allow_diagonal: bool,
)
| 12 | |
| 13 | |
| 14 | def dijkstra( |
| 15 | grid: np.ndarray, |
| 16 | source: tuple[int, int], |
| 17 | destination: tuple[int, int], |
| 18 | allow_diagonal: bool, |
| 19 | ) -> tuple[float | int, list[tuple[int, int]]]: |
| 20 | """ |
| 21 | Implements Dijkstra's algorithm on a binary grid. |
| 22 | |
| 23 | Args: |
| 24 | grid (np.ndarray): A 2D numpy array representing the grid. |
| 25 | 1 represents a walkable node and 0 represents an obstacle. |
| 26 | source (Tuple[int, int]): A tuple representing the start node. |
| 27 | destination (Tuple[int, int]): A tuple representing the |
| 28 | destination node. |
| 29 | allow_diagonal (bool): A boolean determining whether |
| 30 | diagonal movements are allowed. |
| 31 | |
| 32 | Returns: |
| 33 | Tuple[Union[float, int], List[Tuple[int, int]]]: |
| 34 | The shortest distance from the start node to the destination node |
| 35 | and the shortest path as a list of nodes. |
| 36 | |
| 37 | >>> dijkstra(np.array([[1, 1, 1], [0, 1, 0], [0, 1, 1]]), (0, 0), (2, 2), False) |
| 38 | (4.0, [(0, 0), (0, 1), (1, 1), (2, 1), (2, 2)]) |
| 39 | |
| 40 | >>> dijkstra(np.array([[1, 1, 1], [0, 1, 0], [0, 1, 1]]), (0, 0), (2, 2), True) |
| 41 | (2.0, [(0, 0), (1, 1), (2, 2)]) |
| 42 | |
| 43 | >>> dijkstra(np.array([[1, 1, 1], [0, 0, 1], [0, 1, 1]]), (0, 0), (2, 2), False) |
| 44 | (4.0, [(0, 0), (0, 1), (0, 2), (1, 2), (2, 2)]) |
| 45 | """ |
| 46 | rows, cols = grid.shape |
| 47 | dx = [-1, 1, 0, 0] |
| 48 | dy = [0, 0, -1, 1] |
| 49 | if allow_diagonal: |
| 50 | dx += [-1, -1, 1, 1] |
| 51 | dy += [-1, 1, -1, 1] |
| 52 | |
| 53 | queue, visited = [(0, source)], set() |
| 54 | matrix = np.full((rows, cols), np.inf) |
| 55 | matrix[source] = 0 |
| 56 | predecessors = np.empty((rows, cols), dtype=object) |
| 57 | predecessors[source] = None |
| 58 | |
| 59 | while queue: |
| 60 | (dist, (x, y)) = heappop(queue) |
| 61 | if (x, y) in visited: |
| 62 | continue |
| 63 | visited.add((x, y)) |
| 64 | |
| 65 | if (x, y) == destination: |
| 66 | path = [] |
| 67 | while (x, y) != source: |
| 68 | path.append((x, y)) |
| 69 | x, y = predecessors[x, y] |
| 70 | path.append(source) # add the source manually |
| 71 | path.reverse() |