From eb075799d65eaa15f908a748b2bba2a94d3f6665 Mon Sep 17 00:00:00 2001 From: Hari Date: Sat, 27 Jan 2024 20:46:30 +0100 Subject: [PATCH] added theta star pathfinder --- pathfinding3d/finder/__init__.py | 1 + pathfinding3d/finder/theta_star.py | 100 +++++++++++++++++++++++++++++ test/test_finder.py | 13 ---- test/test_path.py | 24 ++++++- test/test_util.py | 8 ++- 5 files changed, 131 insertions(+), 15 deletions(-) create mode 100644 pathfinding3d/finder/theta_star.py diff --git a/pathfinding3d/finder/__init__.py b/pathfinding3d/finder/__init__.py index c319c97..0b3e84d 100644 --- a/pathfinding3d/finder/__init__.py +++ b/pathfinding3d/finder/__init__.py @@ -7,4 +7,5 @@ "finder", "ida_star", "msp", + "theta_star", ] diff --git a/pathfinding3d/finder/theta_star.py b/pathfinding3d/finder/theta_star.py new file mode 100644 index 0000000..1f41fee --- /dev/null +++ b/pathfinding3d/finder/theta_star.py @@ -0,0 +1,100 @@ +import logging +from typing import Callable, List, Union + +from pathfinding3d.core.diagonal_movement import DiagonalMovement +from pathfinding3d.core.grid import Grid +from pathfinding3d.core.node import GridNode +from pathfinding3d.core.util import line_of_sight +from pathfinding3d.finder.a_star import AStarFinder +from pathfinding3d.finder.finder import MAX_RUNS, TIME_LIMIT + + +class ThetaStarFinder(AStarFinder): + def __init__( + self, + heuristic: Callable = None, + weight: int = 1, + diagonal_movement: int = DiagonalMovement.always, + time_limit: float = TIME_LIMIT, + max_runs: Union[int, float] = MAX_RUNS, + ): + """ + Find shortest path using Theta* algorithm + Diagonal movement is forced to always. + + Parameters + ---------- + heuristic : Callable + heuristic used to calculate distance of 2 points + weight : int + weight for the edges + diagonal_movement : int + if diagonal movement is allowed + (see enum in diagonal_movement) + time_limit : float + max. runtime in seconds + max_runs : int + max. amount of tries until we abort the search + (optional, only if we enter huge grids and have time constrains) + <=0 means there are no constrains and the code might run on any + large map. + """ + + if diagonal_movement != DiagonalMovement.always: + logging.warning("Diagonal movement is forced to always for Theta*") + diagonal_movement = DiagonalMovement.always + + super().__init__( + heuristic=heuristic, + weight=weight, + diagonal_movement=diagonal_movement, + time_limit=time_limit, + max_runs=max_runs, + ) + + def process_node( + self, + grid: Grid, + node: GridNode, + parent: GridNode, + end: GridNode, + open_list: List, + open_value: int = 1, + ): + """ + Check if we can reach the grandparent node directly from the current node + and if so, skip the parent. + + Parameters + ---------- + grid : Grid + grid that stores all possible steps/tiles as 3D-list + node : GridNode + the node we like to test + parent : GridNode + the parent node (of the current node we like to test) + end : GridNode + the end point to calculate the cost of the path + open_list : List + the list that keeps track of our current path + open_value : bool + needed if we like to set the open list to something + else than True (used for bi-directional algorithms) + """ + # Check for line of sight to the grandparent + if parent and parent.parent and line_of_sight(grid, node, parent.parent): + ng = parent.parent.g + grid.calc_cost(parent.parent, node, self.weighted) + if not node.opened or ng < node.g: + old_f = node.f + node.g = ng + node.h = node.h or self.apply_heuristic(node, end) + node.f = node.g + node.h + node.parent = parent.parent + if not node.opened: + open_list.push_node(node) + node.opened = open_value + else: + open_list.remove_node(node, old_f) + open_list.push_node(node) + else: + super().process_node(grid, node, parent, end, open_list) diff --git a/test/test_finder.py b/test/test_finder.py index 8a450a9..6fe490f 100644 --- a/test/test_finder.py +++ b/test/test_finder.py @@ -20,16 +20,3 @@ def test_check_neighbors_raises_exception(): with pytest.raises(NotImplementedError): finder.check_neighbors(start, end, grid, open_list) - - -def test_msp(): - """ - Test that the minimum spanning tree finder returns all nodes. - """ - matrix = np.array(np.ones((3, 3, 3))) - grid = Grid(matrix=matrix) - - start = grid.node(0, 0, 0) - - finder = MinimumSpanningTree() - assert finder.tree(grid, start).sort() == [node for row in grid.nodes for col in row for node in col].sort() diff --git a/test/test_path.py b/test/test_path.py index f429883..4a6d821 100644 --- a/test/test_path.py +++ b/test/test_path.py @@ -4,6 +4,7 @@ from pathfinding3d.core.diagonal_movement import DiagonalMovement from pathfinding3d.core.grid import Grid from pathfinding3d.core.node import GridNode +from pathfinding3d.core.util import expand_path from pathfinding3d.finder.a_star import AStarFinder from pathfinding3d.finder.best_first import BestFirst from pathfinding3d.finder.bi_a_star import BiAStarFinder @@ -12,6 +13,7 @@ from pathfinding3d.finder.finder import ExecutionRunsException, ExecutionTimeException from pathfinding3d.finder.ida_star import IDAStarFinder from pathfinding3d.finder.msp import MinimumSpanningTree +from pathfinding3d.finder.theta_star import ThetaStarFinder finders = [ AStarFinder, @@ -21,6 +23,7 @@ IDAStarFinder, BreadthFirstFinder, MinimumSpanningTree, + ThetaStarFinder, ] TIME_LIMIT = 10 # give it a 10 second limit. @@ -29,6 +32,7 @@ BiAStarFinder, DijkstraFinder, MinimumSpanningTree, + ThetaStarFinder, ] SIMPLE_MATRIX = np.zeros((5, 5, 5)) @@ -64,6 +68,8 @@ def test_path(): start = grid.node(0, 0, 0) end = grid.node(4, 4, 0) for find in finders: + if find == ThetaStarFinder: + continue grid.cleanup() finder = find(time_limit=TIME_LIMIT) path_, runs = finder.find_path(start, end, grid) @@ -84,6 +90,8 @@ def test_weighted_path(): start = grid.node(0, 0, 0) end = grid.node(4, 4, 0) for find in weighted_finders: + if find == ThetaStarFinder: + continue grid.cleanup() finder = find(time_limit=TIME_LIMIT) path_, runs = finder.find_path(start, end, grid) @@ -114,10 +122,11 @@ def test_path_diagonal(): path.append((node.x, node.y, node.z)) elif isinstance(node, tuple): path.append((node[0], node[1], node[2])) - print(find.__name__) print(f"path: {path}") print(f"length: {len(path)}, runs: {runs}") + if find == ThetaStarFinder: + path = expand_path(path) assert len(path) == 5 @@ -149,3 +158,16 @@ def test_time(): print(f"path: {path}") msg = f"{finder.__class__.__name__} took too long" assert finder.runs == 1, msg + + +def test_msp(): + """ + Test that the minimum spanning tree finder returns all nodes. + """ + matrix = np.array(np.ones((3, 3, 3))) + grid = Grid(matrix=matrix) + + start = grid.node(0, 0, 0) + + finder = MinimumSpanningTree() + assert finder.tree(grid, start).sort() == [node for row in grid.nodes for col in row for node in col].sort() diff --git a/test/test_util.py b/test/test_util.py index eeb43fd..f5ef53a 100644 --- a/test/test_util.py +++ b/test/test_util.py @@ -2,7 +2,13 @@ from pathfinding3d.core.grid import Grid from pathfinding3d.core.node import GridNode -from pathfinding3d.core.util import bresenham, expand_path, line_of_sight, raytrace, smoothen_path +from pathfinding3d.core.util import ( + bresenham, + expand_path, + line_of_sight, + raytrace, + smoothen_path, +) def test_bresenham():