added line of sight check utility and test.

Co-authored-by: jishnuperiya <[email protected]>

pathfinding3d/core/util.py

@@ -263,3 +263,85 @@ def smoothen_path(grid: Grid, path: List[Coords], use_raytrace: bool = False) ->
 
     new_path.append(path[-1])
     return new_path
+
+
+def line_of_sight(grid: Grid, node1: GridNode, node2: GridNode) -> bool:
+    """
+    Check if there is a line of sight between two nodes using Bresenham's algorithm
+
+    Parameters
+    ----------
+    grid : Grid
+        The grid on which the nodes exist
+    node1 : GridNode
+        The first node
+    node2 : GridNode
+        The second node
+
+    Returns
+    -------
+    bool
+        True if there is a line of sight between the two nodes, False otherwise
+    """
+    x0, y0, z0 = node1.x, node1.y, node1.z
+    x1, y1, z1 = node2.x, node2.y, node2.z
+
+    dx = abs(x1 - x0)
+    dy = abs(y1 - y0)
+    dz = abs(z1 - z0)
+    sx = 1 if x0 < x1 else -1
+    sy = 1 if y0 < y1 else -1
+    sz = 1 if z0 < z1 else -1
+
+    # Driving axis is X-axis
+    if dx >= dy and dx >= dz:
+        err_1 = 2 * dy - dx
+        err_2 = 2 * dz - dx
+        while x0 != x1:
+            x0 += sx
+            if err_1 > 0:
+                y0 += sy
+                err_1 -= 2 * dx
+            if err_2 > 0:
+                z0 += sz
+                err_2 -= 2 * dx
+            err_1 += 2 * dy
+            err_2 += 2 * dz
+            if not grid.walkable(x0, y0, z0):
+                return False
+
+    # Driving axis is Y-axis
+    elif dy >= dx and dy >= dz:
+        err_1 = 2 * dx - dy
+        err_2 = 2 * dz - dy
+        while y0 != y1:
+            y0 += sy
+            if err_1 > 0:
+                x0 += sx
+                err_1 -= 2 * dy
+            if err_2 > 0:
+                z0 += sz
+                err_2 -= 2 * dy
+            err_1 += 2 * dx
+            err_2 += 2 * dz
+            if not grid.walkable(x0, y0, z0):
+                return False
+
+    # Driving axis is Z-axis
+    else:
+        err_1 = 2 * dy - dz
+        err_2 = 2 * dx - dz
+        while z0 != z1:
+            z0 += sz
+            if err_1 > 0:
+                y0 += sy
+                err_1 -= 2 * dz
+            if err_2 > 0:
+                x0 += sx
+                err_2 -= 2 * dz
+            err_1 += 2 * dy
+            err_2 += 2 * dx
+            if not grid.walkable(x0, y0, z0):
+                return False
+
+    return True

test/test_util.py

@@ -1,5 +1,8 @@
+import pytest
+
 from pathfinding3d.core.grid import Grid
-from pathfinding3d.core.util import bresenham, expand_path, raytrace, smoothen_path
+from pathfinding3d.core.node import GridNode
+from pathfinding3d.core.util import bresenham, expand_path, line_of_sight, raytrace, smoothen_path
 
 
 def test_bresenham():
@@ -80,10 +83,10 @@ def test_expand_path():
     test expand_path function
     """
     # Test with empty path
-    assert expand_path([]) == []
+    assert not expand_path([])
 
     # Test with one point path
-    assert expand_path([[0, 0, 0]]) == []
+    assert not expand_path([[0, 0, 0]])
 
     # Test with two points path
     assert expand_path([[0, 0, 0], [1, 1, 1]]) == [[0, 0, 0], [1, 1, 1]]
@@ -96,3 +99,44 @@ def test_expand_path():
         [3, 2, 2],
         [4, 2, 2],
     ]
+
+
+@pytest.fixture
+def grid():
+    # Create a 5x5x5 grid with all nodes walkable
+    grid = Grid(5, 5, 5)
+    for x in range(5):
+        for y in range(5):
+            for z in range(5):
+                grid.nodes[x][y][z].walkable = True
+    return grid
+
+
+def test_line_of_sight_self(grid):
+    """
+    test line_of_sight function with self
+    """
+    # Test with self
+    node = GridNode(0, 0, 0)
+    assert line_of_sight(grid, node, node)
+
+
+def test_line_of_sight_clear(grid):
+    """
+    test line_of_sight function with clear line of sight
+    """
+    # Test with clear line of sight
+    node1 = GridNode(0, 0, 0)
+    node2 = GridNode(4, 4, 4)
+    assert line_of_sight(grid, node1, node2)
+
+
+def test_line_of_sight_obstacle(grid):
+    """
+    test line_of_sight function with obstacle
+    """
+    # Test with obstacle
+    node1 = GridNode(0, 0, 0)
+    node2 = GridNode(4, 4, 4)
+    grid.nodes[2][2][2].walkable = False
+    assert not line_of_sight(grid, node1, node2)