added theta star pathfinder

pathfinding3d/finder/__init__.py

@@ -7,4 +7,5 @@
     "finder",
     "ida_star",
     "msp",
+    "theta_star",
 ]

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)

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()

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()

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():