diff --git a/README.md b/README.md index 4bb04a5..71770c3 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# LPP pathfinding task +# Pathfinding demo ## TODO @@ -7,11 +7,12 @@ - [x] drawing utility - [x] interface for pathfinding - [x] research methods - - [ ] implement methods + - [x] implement methods - [x] DFS - [x] BFS - [x] Dijsktra - - [ ] A* + - [x] GBFS + - [x] A* - [x] performance measurement: time/visited nodes - [ ] finish text on the page - [x] create a dedicated python script diff --git a/python/pathfinding_demo.py b/python/pathfinding_demo.py index 4efd02b..4998d30 100644 --- a/python/pathfinding_demo.py +++ b/python/pathfinding_demo.py @@ -268,6 +268,7 @@ class DijkstraAlgorithm(PathFinderBase): """ Dijsktra's algorithm (Uniform Cost Search) Like BFS, but takes into account cost of nodes + (priority for the search being the distance from the start) """ name = "Dijkstra's Algorithm" @@ -302,9 +303,8 @@ class DijkstraAlgorithm(PathFinderBase): class GBFS(PathFinderBase): """ - Like Dijsktra's Algorithm, but uses some heuristic - as a priority for the PriorityQueue - Also doesn't care about the cost of the node + Like Dijsktra's Algorithm, but uses some heuristic as a priority + instead of the cost of the node """ name = "Greedy Best First Search" @@ -346,12 +346,48 @@ class GBFS(PathFinderBase): class A_star(PathFinderBase): + """ + Combines Dijsktra's Algorithm and GBFS: + priority is the sum of the heuristic and distance from the start + """ name = "A*" - def _CalculatePath(self, start_point: Point2D, end_point: Point2D) -> Optional[Path]: - ... + @staticmethod + def heuristic(a: Point2D, b: Point2D) -> float: + # for now we use Manhattan distance, although + # it is probably not entirely correct, given that + # we can also move diagonally in the grid + # TODO a problem for future me + x_a, y_a = a + x_b, y_b = b + return abs(x_a - x_b) + abs(y_a - y_b) + def _CalculatePath(self, start_point: Point2D, end_point: Point2D) -> Optional[Path]: + frontier: PriorityQueue[PrioritizedItem] = PriorityQueue() + came_from: dict[Point2D, Optional[Point2D]] = { end_point: None } + cost_so_far: dict[Point2D, float] = { end_point: 0.0 } + + frontier.put(PrioritizedItem(end_point, 0.0)) + while not frontier.empty(): + current = frontier.get().item + if current == start_point: + # early exit + break + for next_point in self._map.GetNeighbours(current): + new_cost = cost_so_far[current] + self._map.Visit(next_point) + if next_point not in cost_so_far or new_cost < cost_so_far[next_point]: + cost_so_far[next_point] = new_cost + priority = new_cost + self.heuristic(start_point, next_point) + frontier.put(PrioritizedItem(next_point, priority)) + came_from[next_point] = current + # create the actual path + path: Path = [start_point] + current = start_point + while came_from[current] is not None: + current = came_from[current] + path.append(current) + return path # # Calculate paths using various methods and visualize them @@ -382,7 +418,7 @@ def main(): elapsed_time, visited_nodes = path_finder.GetStats() if path is not None: cost = m.GetPathCost(path) - print(f"{path_finder.name:22}: took {elapsed_time/1e6:.3f} ms, visited {visited_nodes} nodes, cost {cost:.2f}") + print(f"{path_finder.name:24}: took {elapsed_time/1e6:.3f} ms, visited {visited_nodes} nodes, cost {cost:.2f}") v.DrawPath(path) else: print(f"{path_finder.name}: No path found")