From a3715c37197a138e8144abdb283ff6422da5221c Mon Sep 17 00:00:00 2001 From: Hari Date: Sat, 20 Jan 2024 18:04:23 +0100 Subject: [PATCH 01/10] code refactored, tests added for diagonal movements --- pathfinding3d/core/grid.py | 178 ++++++++++++++++---------------- test/test_grid.py | 204 +++++++++++++++++++++++++++++++++++++ 2 files changed, 292 insertions(+), 90 deletions(-) diff --git a/pathfinding3d/core/grid.py b/pathfinding3d/core/grid.py index 07caa1a..007f807 100644 --- a/pathfinding3d/core/grid.py +++ b/pathfinding3d/core/grid.py @@ -228,9 +228,7 @@ def neighbors( list list of neighbor nodes """ - x = node.x - y = node.y - z = node.z + x, y, z = node.x, node.y, node.z neighbors = [] # current plane @@ -240,25 +238,25 @@ def neighbors( # lower plane ls0 = ld0 = ls1 = ld1 = ls2 = ld2 = ls3 = ld3 = lb = False # lb = lower bottom + # -y + if self.walkable(x, y - 1, z): + neighbors.append(self.nodes[x][y - 1][z]) + cs0 = True + # +x if self.walkable(x + 1, y, z): neighbors.append(self.nodes[x + 1][y][z]) cs1 = True - # -x - if self.walkable(x - 1, y, z): - neighbors.append(self.nodes[x - 1][y][z]) - cs3 = True - # +y if self.walkable(x, y + 1, z): neighbors.append(self.nodes[x][y + 1][z]) cs2 = True - # -y - if self.walkable(x, y - 1, z): - neighbors.append(self.nodes[x][y - 1][z]) - cs0 = True + # -x + if self.walkable(x - 1, y, z): + neighbors.append(self.nodes[x - 1][y][z]) + cs3 = True # +z if self.walkable(x, y, z + 1): @@ -278,10 +276,10 @@ def neighbors( return neighbors if diagonal_movement == DiagonalMovement.only_when_no_obstacle: - cd0 = cs0 and cs3 - cd1 = cs0 and cs1 - cd2 = cs1 and cs2 - cd3 = cs2 and cs3 + cd0 = cs0 and cs1 + cd1 = cs1 and cs2 + cd2 = cs2 and cs3 + cd3 = cs3 and cs0 us0 = cs0 and ut us1 = cs1 and ut @@ -294,10 +292,10 @@ def neighbors( ls3 = cs3 and lb elif diagonal_movement == DiagonalMovement.if_at_most_one_obstacle: - cd0 = cs0 or cs3 - cd1 = cs0 or cs1 - cd2 = cs1 or cs2 - cd3 = cs2 or cs3 + cd0 = cs0 or cs1 + cd1 = cs1 or cs2 + cd2 = cs2 or cs3 + cd3 = cs3 or cs0 us0 = cs0 or ut us1 = cs1 or ut @@ -314,41 +312,47 @@ def neighbors( us0 = us1 = us2 = us3 = True ls0 = ls1 = ls2 = ls3 = True - # +x +y - if cd2 and self.walkable(x + 1, y + 1, z): - neighbors.append(self.nodes[x + 1][y + 1][z]) - else: - cd2 = False - # +x -y - if cd1 and self.walkable(x + 1, y - 1, z): + if cd0 and self.walkable(x + 1, y - 1, z): neighbors.append(self.nodes[x + 1][y - 1][z]) + else: + cd0 = False + + # +x +y + if cd1 and self.walkable(x + 1, y + 1, z): + neighbors.append(self.nodes[x + 1][y + 1][z]) else: cd1 = False # -x +y - if cd3 and self.walkable(x - 1, y + 1, z): + if cd2 and self.walkable(x - 1, y + 1, z): neighbors.append(self.nodes[x - 1][y + 1][z]) else: - cd3 = False + cd2 = False # -x -y - if cd0 and self.walkable(x - 1, y - 1, z): + if cd3 and self.walkable(x - 1, y - 1, z): neighbors.append(self.nodes[x - 1][y - 1][z]) else: - cd0 = False + cd3 = False + + # -y +z + if us0 and self.walkable(x, y - 1, z + 1): + neighbors.append(self.nodes[x][y - 1][z + 1]) + else: + us0 = False # +x +z - if us2 and self.walkable(x + 1, y, z + 1): + if us1 and self.walkable(x + 1, y, z + 1): neighbors.append(self.nodes[x + 1][y][z + 1]) else: - us2 = False + us1 = False - # +x -z - if ls2 and self.walkable(x + 1, y, z - 1): - neighbors.append(self.nodes[x + 1][y][z - 1]) + # +y +z + if us2 and self.walkable(x, y + 1, z + 1): + neighbors.append(self.nodes[x][y + 1][z + 1]) else: - ls2 = False + us2 = False # -x +z if us3 and self.walkable(x - 1, y, z + 1): @@ -356,93 +360,87 @@ def neighbors( else: us3 = False - # -x -z - if ls3 and self.walkable(x - 1, y, z - 1): - neighbors.append(self.nodes[x - 1][y][z - 1]) + # -y -z + if ls0 and self.walkable(x, y - 1, z - 1): + neighbors.append(self.nodes[x][y - 1][z - 1]) else: - ls3 = False + ls0 = False - # +y +z - if us0 and self.walkable(x, y + 1, z + 1): - neighbors.append(self.nodes[x][y + 1][z + 1]) + # +x -z + if ls1 and self.walkable(x + 1, y, z - 1): + neighbors.append(self.nodes[x + 1][y][z - 1]) else: - us0 = False + ls1 = False # +y -z - if ls0 and self.walkable(x, y + 1, z - 1): + if ls2 and self.walkable(x, y + 1, z - 1): neighbors.append(self.nodes[x][y + 1][z - 1]) else: - ls0 = False - - # -y +z - if us1 and self.walkable(x, y - 1, z + 1): - neighbors.append(self.nodes[x][y - 1][z + 1]) - else: - us1 = False + ls2 = False - # -y -z - if ls1 and self.walkable(x, y - 1, z - 1): - neighbors.append(self.nodes[x][y - 1][z - 1]) + # -x -z + if ls3 and self.walkable(x - 1, y, z - 1): + neighbors.append(self.nodes[x - 1][y][z - 1]) else: - ls1 = False + ls3 = False # remaining daigonal neighbors if diagonal_movement == DiagonalMovement.only_when_no_obstacle: - ud0 = cd0 and cs0 and cs3 and us0 and us3 and ut - ud1 = cd1 and cs0 and cs1 and us0 and us1 and ut - ud2 = cd2 and cs1 and cs2 and us1 and us2 and ut - ud3 = cd3 and cs2 and cs3 and us2 and us3 and ut + ud0 = cs0 and cd0 and cs1 and us0 and us1 and ut + ud1 = cs1 and cd1 and cs2 and us1 and us2 and ut + ud2 = cs2 and cd2 and cs3 and us2 and us3 and ut + ud3 = cs3 and cd3 and cs0 and us3 and us0 and ut - ld0 = cd0 and cs0 and cs3 and ls0 and ls3 and lb - ld1 = cd1 and cs0 and cs1 and ls0 and ls1 and lb - ld2 = cd2 and cs1 and cs2 and ls1 and ls2 and lb - ld3 = cd3 and cs2 and cs3 and ls2 and ls3 and lb + ld0 = cs0 and cd0 and cs1 and ls0 and ls1 and lb + ld1 = cs1 and cd1 and cs2 and ls1 and ls2 and lb + ld2 = cs2 and cd2 and cs3 and ls2 and ls3 and lb + ld3 = cs3 and cd3 and cs0 and ls3 and ls0 and lb elif diagonal_movement == DiagonalMovement.if_at_most_one_obstacle: - ud0 = sum([cd0, cs0, cs3, us0, us3, ut]) >= 5 - ud1 = sum([cd1, cs0, cs1, us0, us1, ut]) >= 5 - ud2 = sum([cd2, cs1, cs2, us1, us2, ut]) >= 5 - ud3 = sum([cd3, cs2, cs3, us2, us3, ut]) >= 5 + ud0 = sum([cs0, cd0, cs1, us0, us1, ut]) >= 5 + ud1 = sum([cs1, cd1, cs2, us1, us2, ut]) >= 5 + ud2 = sum([cs2, cd2, cs3, us2, us3, ut]) >= 5 + ud3 = sum([cs3, cd3, cs0, us3, us0, ut]) >= 5 - ld0 = sum([cd0, cs0, cs3, ls0, ls3, lb]) >= 5 - ld1 = sum([cd1, cs0, cs1, ls0, ls1, lb]) >= 5 - ld2 = sum([cd2, cs1, cs2, ls1, ls2, lb]) >= 5 - ld3 = sum([cd3, cs2, cs3, ls2, ls3, lb]) >= 5 + ld0 = sum([cs0, cd0, cs1, ls0, ls1, lb]) >= 5 + ld1 = sum([cs1, cd1, cs2, ls1, ls2, lb]) >= 5 + ld2 = sum([cs2, cd2, cs3, ls2, ls3, lb]) >= 5 + ld3 = sum([cs3, cd3, cs0, ls3, ls0, lb]) >= 5 elif diagonal_movement == DiagonalMovement.always: ud0 = ud1 = ud2 = ud3 = True ld0 = ld1 = ld2 = ld3 = True + # +x -y +z + if ud0 and self.walkable(x + 1, y - 1, z + 1): + neighbors.append(self.nodes[x + 1][y - 1][z + 1]) + # +x +y +z - if ud2 and self.walkable(x + 1, y + 1, z + 1): + if ud1 and self.walkable(x + 1, y + 1, z + 1): neighbors.append(self.nodes[x + 1][y + 1][z + 1]) - # +x +y -z - if ld2 and self.walkable(x + 1, y + 1, z - 1): - neighbors.append(self.nodes[x + 1][y + 1][z - 1]) + # -x +y +z + if ud2 and self.walkable(x - 1, y + 1, z + 1): + neighbors.append(self.nodes[x - 1][y + 1][z + 1]) - # +x -y +z - if ud1 and self.walkable(x + 1, y - 1, z + 1): - neighbors.append(self.nodes[x + 1][y - 1][z + 1]) + # -x -y +z + if ud3 and self.walkable(x - 1, y - 1, z + 1): + neighbors.append(self.nodes[x - 1][y - 1][z + 1]) # +x -y -z - if ld1 and self.walkable(x + 1, y - 1, z - 1): + if ld0 and self.walkable(x + 1, y - 1, z - 1): neighbors.append(self.nodes[x + 1][y - 1][z - 1]) - # -x +y +z - if ud3 and self.walkable(x - 1, y + 1, z + 1): - neighbors.append(self.nodes[x - 1][y + 1][z + 1]) + # +x +y -z + if ld1 and self.walkable(x + 1, y + 1, z - 1): + neighbors.append(self.nodes[x + 1][y + 1][z - 1]) # -x +y -z - if ld3 and self.walkable(x - 1, y + 1, z - 1): + if ld2 and self.walkable(x - 1, y + 1, z - 1): neighbors.append(self.nodes[x - 1][y + 1][z - 1]) - # -x -y +z - if ud0 and self.walkable(x - 1, y - 1, z + 1): - neighbors.append(self.nodes[x - 1][y - 1][z + 1]) - # -x -y -z - if ld0 and self.walkable(x - 1, y - 1, z - 1): + if ld3 and self.walkable(x - 1, y - 1, z - 1): neighbors.append(self.nodes[x - 1][y - 1][z - 1]) return neighbors diff --git a/test/test_grid.py b/test/test_grid.py index ccc9f83..edf96ac 100644 --- a/test/test_grid.py +++ b/test/test_grid.py @@ -1,6 +1,7 @@ """ Test Grid class. """ import numpy as np +import pytest from pathfinding3d.core.diagonal_movement import DiagonalMovement from pathfinding3d.core.grid import Grid @@ -34,3 +35,206 @@ def test_numpy(): path.append([node.x, node.y, node.z]) assert path == [[0, 0, 0], [1, 1, 0], [2, 2, 1], [2, 2, 2]] + + +DIRECTIONS = [ + (0, 0, 0), + (0, 1, 0), + (0, 2, 0), + (1, 0, 0), + (1, 1, 0), + (1, 2, 0), + (2, 0, 0), + (2, 1, 0), + (2, 2, 0), + (0, 0, 1), + (0, 1, 1), + (0, 2, 1), + (1, 0, 1), + (1, 2, 1), + (2, 0, 1), + (2, 1, 1), + (2, 2, 1), + (0, 0, 2), + (0, 1, 2), + (0, 2, 2), + (1, 0, 2), + (1, 1, 2), + (1, 2, 2), + (2, 0, 2), + (2, 1, 2), + (2, 2, 2), +] + + +@pytest.fixture +def gen_grid(): + grid = Grid(width=3, height=3, depth=3) + for x in range(3): + for y in range(3): + for z in range(3): + grid.nodes[x][y][z].walkable = True + node = grid.node(1, 1, 1) + return grid, node + + +def test_diagonal_movement_always(gen_grid): + test_grid, node = gen_grid + neighbors = test_grid.neighbors(node, DiagonalMovement.always) + assert len(neighbors) == 26 # All 26 possible neighbors + + +def test_diagonal_movement_never(gen_grid): + test_grid, node = gen_grid + neighbors = test_grid.neighbors(node, DiagonalMovement.never) + assert len(neighbors) == 6 # Only 6 neighbors (no diagonals) + # only the following nodes are considered neighbors: + expected_neighbors = [(0, 1, 1), (1, 0, 1), (1, 2, 1), (2, 1, 1), (1, 1, 0), (1, 1, 2)] + actual_neighbors = [(neighbor.x, neighbor.y, neighbor.z) for neighbor in neighbors] + # assert all neighbors are in the expected list + for neighbor in actual_neighbors: + assert neighbor in expected_neighbors + + +# current plane : cs (z = 1) +# upper plane : us (z = 2) +# lower plane : ls (z = 0) +def test_diagonal_movement_if_at_most_one_obstacle_cs(gen_grid): + test_grid, node = gen_grid + test_grid.nodes[1][0][1].walkable = False # Create an obstacle + neighbors = test_grid.neighbors(node, DiagonalMovement.if_at_most_one_obstacle) + neighbors = [(neighbor.x, neighbor.y, neighbor.z) for neighbor in neighbors] + expected_neighbors_count = 25 + assert len(neighbors) == expected_neighbors_count + expected_neighbors = DIRECTIONS.copy() + expected_neighbors.remove((1, 0, 1)) + # assert all neighbors are in the expected list + for neighbor in neighbors: + assert neighbor in expected_neighbors + + test_grid.nodes[0][1][1].walkable = False # Create another obstacle + neighbors = test_grid.neighbors(node, DiagonalMovement.if_at_most_one_obstacle) + neighbors = [(neighbor.x, neighbor.y, neighbor.z) for neighbor in neighbors] + expected_neighbors_count = 21 + assert len(neighbors) == expected_neighbors_count + expected_neighbors.remove((0, 1, 1)) + expected_neighbors.remove((0, 0, 1)) + expected_neighbors.remove((0, 0, 0)) + expected_neighbors.remove((0, 0, 2)) + # assert all neighbors are in the expected list + for neighbor in neighbors: + assert neighbor in expected_neighbors + + +def test_diagonal_movement_if_at_most_one_obstacle_us(gen_grid): + test_grid, node = gen_grid + test_grid.nodes[1][1][2].walkable = False # Create an obstacle + test_grid.nodes[1][2][1].walkable = False # Create another obstacle + neighbors = test_grid.neighbors(node, DiagonalMovement.if_at_most_one_obstacle) + neighbors = [(neighbor.x, neighbor.y, neighbor.z) for neighbor in neighbors] + expected_neighbors_count = 21 + assert len(neighbors) == expected_neighbors_count + expected_neighbors = DIRECTIONS.copy() + expected_neighbors.remove((1, 1, 2)) + expected_neighbors.remove((1, 2, 1)) + expected_neighbors.remove((1, 2, 2)) + expected_neighbors.remove((2, 2, 2)) + expected_neighbors.remove((0, 2, 2)) + # assert all neighbors are in the expected list + for neighbor in neighbors: + assert neighbor in expected_neighbors + + +def test_diagonal_movement_if_at_most_one_obstacle_ls(gen_grid): + test_grid, node = gen_grid + test_grid.nodes[1][1][0].walkable = False # Create an obstacle + test_grid.nodes[1][0][1].walkable = False # Create another obstacle + neighbors = test_grid.neighbors(node, DiagonalMovement.if_at_most_one_obstacle) + neighbors = [(neighbor.x, neighbor.y, neighbor.z) for neighbor in neighbors] + expected_neighbors_count = 21 + assert len(neighbors) == expected_neighbors_count + expected_neighbors = DIRECTIONS.copy() + expected_neighbors.remove((1, 1, 0)) + expected_neighbors.remove((1, 0, 1)) + expected_neighbors.remove((1, 0, 0)) + expected_neighbors.remove((2, 0, 0)) + expected_neighbors.remove((0, 0, 0)) + # assert all neighbors are in the expected list + for neighbor in neighbors: + assert neighbor in expected_neighbors + + +def test_diagonal_movement_only_when_no_obstacle_cs(gen_grid): + test_grid, node = gen_grid + test_grid.nodes[1][0][1].walkable = False # Create an obstacle + neighbors = test_grid.neighbors(node, DiagonalMovement.only_when_no_obstacle) + neighbors = [(neighbor.x, neighbor.y, neighbor.z) for neighbor in neighbors] + expected_neighbors_count = 17 + assert len(neighbors) == expected_neighbors_count + expected_neighbors = DIRECTIONS.copy() + expected_neighbors.remove((1, 0, 1)) + expected_neighbors.remove((0, 0, 1)) + expected_neighbors.remove((2, 0, 1)) + expected_neighbors.remove((1, 0, 0)) + expected_neighbors.remove((0, 0, 0)) + expected_neighbors.remove((2, 0, 0)) + expected_neighbors.remove((1, 0, 2)) + expected_neighbors.remove((0, 0, 2)) + expected_neighbors.remove((2, 0, 2)) + # assert all neighbors are in the expected list + for neighbor in neighbors: + assert neighbor in expected_neighbors + + +def test_diagonal_movement_only_when_no_obstacle_cd(gen_grid): + test_grid, node = gen_grid + test_grid.nodes[0][0][1].walkable = False # Create an obstacle + neighbors = test_grid.neighbors(node, DiagonalMovement.only_when_no_obstacle) + neighbors = [(neighbor.x, neighbor.y, neighbor.z) for neighbor in neighbors] + expected_neighbors_count = 23 + assert len(neighbors) == expected_neighbors_count + expected_neighbors = DIRECTIONS.copy() + expected_neighbors.remove((0, 0, 1)) + expected_neighbors.remove((0, 0, 0)) + expected_neighbors.remove((0, 0, 2)) + # assert all neighbors are in the expected list + for neighbor in neighbors: + assert neighbor in expected_neighbors + + +def test_diagonal_movement_only_when_no_obstacle_us(gen_grid): + test_grid, node = gen_grid + test_grid.nodes[1][2][2].walkable = False # Create an obstacle + neighbors = test_grid.neighbors(node, DiagonalMovement.only_when_no_obstacle) + neighbors = [(neighbor.x, neighbor.y, neighbor.z) for neighbor in neighbors] + expected_neighbors_count = 23 + assert len(neighbors) == expected_neighbors_count + expected_neighbors = DIRECTIONS.copy() + expected_neighbors.remove((1, 2, 2)) + expected_neighbors.remove((0, 2, 2)) + expected_neighbors.remove((2, 2, 2)) + # assert all neighbors are in the expected list + for neighbor in neighbors: + assert neighbor in expected_neighbors + + +def test_diagonal_movement_only_when_no_obstacle_ud(gen_grid): + test_grid, node = gen_grid + test_grid.nodes[1][1][0].walkable = False # Create an obstacle + neighbors = test_grid.neighbors(node, DiagonalMovement.only_when_no_obstacle) + neighbors = [(neighbor.x, neighbor.y, neighbor.z) for neighbor in neighbors] + expected_neighbors_count = 17 + assert len(neighbors) == expected_neighbors_count + expected_neighbors = DIRECTIONS.copy() + expected_neighbors.remove((1, 1, 0)) + expected_neighbors.remove((0, 1, 0)) + expected_neighbors.remove((2, 1, 0)) + expected_neighbors.remove((1, 0, 0)) + expected_neighbors.remove((0, 0, 0)) + expected_neighbors.remove((2, 0, 0)) + expected_neighbors.remove((1, 2, 0)) + expected_neighbors.remove((0, 2, 0)) + expected_neighbors.remove((2, 2, 0)) + # assert all neighbors are in the expected list + for neighbor in neighbors: + assert neighbor in expected_neighbors From a43d6c3556f22cc9e62670a8c1e3e09913ad8b9c Mon Sep 17 00:00:00 2001 From: Hari Date: Sat, 20 Jan 2024 21:36:18 +0100 Subject: [PATCH 02/10] lru caching for heuristics --- pathfinding3d/core/heuristic.py | 23 +++++++++-------------- pathfinding3d/core/util.py | 22 ++++++++-------------- 2 files changed, 17 insertions(+), 28 deletions(-) diff --git a/pathfinding3d/core/heuristic.py b/pathfinding3d/core/heuristic.py index a483933..ed8f6df 100644 --- a/pathfinding3d/core/heuristic.py +++ b/pathfinding3d/core/heuristic.py @@ -1,7 +1,8 @@ import math +from functools import lru_cache from typing import Union -from .util import SQRT2, SQRT3 +from .util import SQRT2_MINUS_1, SQRT3_MINUS_SQRT2 def null(dx: Union[int, float], dy: Union[int, float], dz: Union[int, float]) -> float: @@ -28,9 +29,7 @@ def null(dx: Union[int, float], dy: Union[int, float], dz: Union[int, float]) -> return 0.0 -def manhattan( - dx: Union[int, float], dy: Union[int, float], dz: Union[int, float] -) -> float: +def manhattan(dx: Union[int, float], dy: Union[int, float], dz: Union[int, float]) -> float: """Manhattan heuristics Parameters @@ -50,9 +49,8 @@ def manhattan( return dx + dy + dz -def euclidean( - dx: Union[int, float], dy: Union[int, float], dz: Union[int, float] -) -> float: +@lru_cache(maxsize=128) +def euclidean(dx: Union[int, float], dy: Union[int, float], dz: Union[int, float]) -> float: """Euclidean distance heuristics Parameters @@ -72,9 +70,7 @@ def euclidean( return math.sqrt(dx * dx + dy * dy + dz * dz) -def chebyshev( - dx: Union[int, float], dy: Union[int, float], dz: Union[int, float] -) -> float: +def chebyshev(dx: Union[int, float], dy: Union[int, float], dz: Union[int, float]) -> float: """Chebyshev distance. Parameters @@ -94,9 +90,8 @@ def chebyshev( return max(dx, dy, dz) -def octile( - dx: Union[int, float], dy: Union[int, float], dz: Union[int, float] -) -> float: +@lru_cache(maxsize=128) +def octile(dx: Union[int, float], dy: Union[int, float], dz: Union[int, float]) -> float: """Octile distance. Parameters @@ -117,4 +112,4 @@ def octile( dmin = min(dx, dy, dz) dmid = dx + dy + dz - dmax - dmin - return dmax + (SQRT2 - 1) * dmid + (SQRT3 - SQRT2) * dmin + return dmax + SQRT2_MINUS_1 * dmid + SQRT3_MINUS_SQRT2 * dmin diff --git a/pathfinding3d/core/util.py b/pathfinding3d/core/util.py index 2c60387..584b975 100644 --- a/pathfinding3d/core/util.py +++ b/pathfinding3d/core/util.py @@ -9,6 +9,10 @@ SQRT2 = math.sqrt(2) # square root of 3 for octile distance SQRT3 = math.sqrt(3) +# square root of 2 minus 1 for octile distance +SQRT2_MINUS_1 = SQRT2 - 1 +# square root of 3 minus square root of 2 for octile distance +SQRT3_MINUS_SQRT2 = SQRT3 - SQRT2 Coords = Tuple[float, float, float] @@ -105,15 +109,9 @@ def raytrace(coords_a: Coords, coords_b: Coords) -> List[List[float]]: while t <= 1.0: line.append(copy.copy(grid_pos)) index = None - if ( - t_for_next_border[0] <= t_for_next_border[1] - and t_for_next_border[0] <= t_for_next_border[2] - ): + if t_for_next_border[0] <= t_for_next_border[1] and t_for_next_border[0] <= t_for_next_border[2]: index = 0 - elif ( - t_for_next_border[1] <= t_for_next_border[2] - and t_for_next_border[1] <= t_for_next_border[0] - ): + elif t_for_next_border[1] <= t_for_next_border[2] and t_for_next_border[1] <= t_for_next_border[0]: index = 1 else: index = 2 @@ -226,9 +224,7 @@ def expand_path(path: List[Coords]) -> List[Coords]: return expanded -def smoothen_path( - grid: Grid, path: List[Coords], use_raytrace: bool = False -) -> List[List[float]]: +def smoothen_path(grid: Grid, path: List[Coords], use_raytrace: bool = False) -> List[List[float]]: """ Given an uncompressed path, return a new path that has less turnings and looks more natural. @@ -256,9 +252,7 @@ def smoothen_path( line = interpolate((sx, sy, sz), coord) blocked = False for test_coord in line[1:]: - if not grid.walkable( - int(test_coord[0]), int(test_coord[1]), int(test_coord[2]) - ): + if not grid.walkable(int(test_coord[0]), int(test_coord[1]), int(test_coord[2])): blocked = True break if not blocked: From 4b0700f1177d1bec795f0d2be25a4e2ae9765581 Mon Sep 17 00:00:00 2001 From: Hari Date: Sat, 20 Jan 2024 22:04:42 +0100 Subject: [PATCH 03/10] Update CONTRIBUTING.md --- CONTRIBUTING.md | 67 +++++++++++++++++++++++++++++++++++-------------- 1 file changed, 48 insertions(+), 19 deletions(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index bf37607..84000aa 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -5,42 +5,71 @@ feature developments, extensions, or bugfixes that you are working on. An easy way is to open an issue or a pull request in which you explain what you are trying to do. +Before starting your work, please check the existing issues and discussions to avoid duplicate efforts. Engaging in discussions on issues can provide better clarity and enhance community interaction. + ## Pull Requests -The preferred way to contribute to *pathfinding3D* is to fork the main repository on Github, then submit a "pull request" -(PR): +The preferred way to contribute to *pathfinding3D* is to fork the main repository on Github, then submit a "pull request" (PR). Follow this checklist before submitting your PR: 1. Fork the [project repository](https://github.com/harisankar95/pathfinding3D): - click on the 'Fork' button near the top of the page. This creates a copy of - the code under your account on the Github server. + click on the ``Fork`` button near the top of the page. This creates a copy of + the code under your namespace on the Github servers. + +2. Clone this repository to your local disk: + + ```bash + git clone git@github.com:YourLogin/pathfinding3D.git + cd pathfinding3D + ``` + +3. Prepare the development environment: -2. Clone this copy to your local disk: + ```bash + pip install numpy pytest sphinx + ``` - git clone git@github.com:YourLogin/pathfinding3D.git +4. Create a new branch to hold your changes, for example (replace + ``my-feature`` with a short but descriptive name of your feature, bugfix, etc.): -3. Create a branch to hold your changes: + ```bash + git checkout -b my-feature + ``` - git checkout -b my-feature + and start making changes. - and start making changes. Never work in the ``main`` branch! +5. When you're done making changes, add changed files to the index with ``git add`` and + ``git commit`` them with a descriptive message. -4. Work on this copy, on your computer, using Git to do the version - control. When you're done editing, do:: + ```bash + git add modified_files + git commit -m "A short description of the commit" + ``` - git add modified_files - git commit +6. Run the tests with ``pytest``. If they pass, you are ready to submit a pull request. - to record your changes in Git, then push them to Github with:: + ```bash + pytest test + ``` - git push -u origin my-feature +7. Push your changes to Github with: -Finally, go to the web page of the your fork of the repo, -and click 'Pull request' to send your changes to the maintainers for review. + ```bash + git push -u origin my-feature + ``` + +Finally, go to the web page of your fork of the repo, +and click ``Pull request`` to send your changes to the maintainers for review. ## Merge Policy Summary: maintainer can push minor changes directly, pull request + 1 reviewer for everything else. -* Usually it is not possible to push directly to the `main` branch of *pathfinding3D* for anyone. Only tiny changes, urgent bugfixes, and maintenance commits can be pushed directly to the `main` branch by the maintainer without a review. "Tiny" means backwards compatibility is mandatory and all tests must succeed. No new feature must be added. +Usually, direct push to the main branch of pathfinding3D is restricted. Only minor changes, urgent bugfixes, and maintenance commits can be pushed directly to the main branch by the maintainer without a review. "Minor" implies backwards compatibility and successful completion of all tests. No new features must be added. + +Developers should submit pull requests for their changes. PRs should be reviewed by at least one other developer and merged by the maintainer. New features must be documented and tested. Breaking changes must be discussed and announced in advance with deprecation warnings. + +Contributors are encouraged to engage in the feedback and review process. If you haven't received any response on your PR within a reasonable timeframe, feel free to reach out for an update. + +All contributions will be acknowledged, either through mentions in release notes, the contributor list, or other appropriate channels. -* Developers should submit pull requests. Those will ideally be reviewed by at least one other developer and merged by the maintainer. New features must be documented and tested. Breaking changes must be discussed and announced in advance with deprecation warnings. +By following these guidelines, you help maintain the quality and consistency of the pathfinding3D project. We appreciate your contributions and look forward to your active participation in our community. From 5d5f85015e2d6b76abb4bcd21331ccf2405bd9b8 Mon Sep 17 00:00:00 2001 From: Hari Date: Sat, 20 Jan 2024 22:05:23 +0100 Subject: [PATCH 04/10] Update LICENSE --- LICENSE | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/LICENSE b/LICENSE index abc23fc..1c0c4d3 100644 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,6 @@ The MIT License (MIT) -Copyright (c) 2023 Harisankar Babu +Copyright (c) 2024 Harisankar Babu Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal From be7d9adeb17d02241cfc0f603b75e97c3bcb0467 Mon Sep 17 00:00:00 2001 From: Hari Date: Sat, 20 Jan 2024 23:45:01 +0100 Subject: [PATCH 05/10] Update README.md --- README.md | 102 ++++++++++++++++++++++++++++++++++++++++-------------- 1 file changed, 76 insertions(+), 26 deletions(-) diff --git a/README.md b/README.md index 4edcb1f..ff62bde 100644 --- a/README.md +++ b/README.md @@ -8,74 +8,124 @@ Pathfinding algorithms for python3 froked from [python-pathfinding](https://github.com/brean/python-pathfinding) by [@brean](https://github.com/brean). +Pathfinding3D is a comprehensive library designed for 3D pathfinding applications. + Currently there are 7 path-finders bundled in this library, namely: -- A* -- Dijkstra +- A\*: Versatile and most widely used algorithm. +- Dijkstra: A\* without heuristic. - Best-First -- Bi-directional A* +- Bi-directional A\*: Efficient for large graphs with a known goal. - Breadth First Search (BFS) -- Iterative Deeping A\* (IDA*) +- Iterative Deeping A\* (IDA\*): Memory efficient algorithm for large graphs. - Minimum Spanning Tree (MSP) Dijkstra, A\* and Bi-directional A\* take the weight of the fields on the map into account. ## Installation -The package is available on pypi, so you can install it with pip: +### Requirements + +- python >= 3.8 +- numpy + +To install Pathfinding3D, use pip: ```bash pip install pathfinding3d ``` -see [pathfinding3d on pypi](https://pypi.org/project/pathfinding3d/) +For more details, see [pathfinding3d on pypi](https://pypi.org/project/pathfinding3d/) ## Usage examples -For usage examples with detailed descriptions take a look at the [examples](examples/) folder, also take a look at the [test/](test/) folder for more examples. +For a quick start, here's a basic example: + + ```python + import numpy as np + + from pathfinding3d.core.diagonal_movement import DiagonalMovement + from pathfinding3d.core.grid import Grid + from pathfinding3d.finder.a_star import AStarFinder + + # Create a 3D numpy array with 0s as obstacles and 1s as walkable paths + matrix = np.ones((10, 10, 10), dtype=np.int8) + # mark the center of the grid as an obstacle + matrix[5, 5, 5] = 0 + + # Create a grid object from the numpy array + grid = Grid(matrix=matrix) + + # Mark the start and end points + start = grid.node(0, 0, 0) + end = grid.node(9, 9, 9) + + # Create an instance of the A* finder with diagonal movement allowed + finder = AStarFinder(diagonal_movement=DiagonalMovement.always) + path, runs = finder.find_path(start, end, grid) -## Rerun the algorithm + # Path will be a list with all the waypoints as nodes + # Convert it to a list of coordinate tuples + path = [p.identifier for p in path] -While running the pathfinding algorithm it might set values on the nodes. Depending on your path finding algorithm things like calculated distances or visited flags might be stored on them. So if you want to run the algorithm in a loop you need to clean the grid first (see `Grid.cleanup`). Please note that because cleanup looks at all nodes of the grid it might be an operation that can take a bit of time! + print("operations:", runs, "path length:", len(path)) + print("path:", path) + ``` + +For usage examples with detailed descriptions take a look at the [examples](examples/) folder. + +## Rerun the Algorithm + +When rerunning the algorithm, remember to clean the grid first using `Grid.cleanup`. This will reset the grid to its original state. + + ```python + grid.cleanup() + ``` + +Please note that this operation can be time-consuming but is usally faster than creating a new grid object. ## Implementation details -All pathfinding algorithms in this library are inheriting the Finder class. It has some common functionality that can be overwritten by the implementation of a path finding algorithm. +All pathfinding algorithms in this library inherit from the `Finder` class. This class provides common functionality that can be overridden by specific pathfinding algorithm implementations. -The normal process works like this: +General Process: 1. You call `find_path` on one of your finder implementations. -1. `init_find` instantiates the `open_list` and resets all values and counters. -1. The main loop starts on the `open_list`. This list gets filled with all nodes that will be processed next (e.g. all current neighbors that are walkable). For this you need to implement `check_neighbors` in your own finder implementation. -1. For example in A\*s implementation of `check_neighbors` you first want to get the next node closest from the current starting point from the open list. the `next_node` method in Finder does this by giving you the node with a minimum `f`-value from the open list, it closes it and removes it from the `open_list`. -1. if this node is not the end node we go on and get its neighbors by calling `find_neighbors`. This just calls `grid.neighbors` for most algorithms. -1. If none of the neighbors are the end node we want to process the neighbors to calculate their distances in `process_node` -1. `process_node` calculates the cost `f` from the start to the current node using the `calc_cost` method and the cost after calculating `h` from `apply_heuristic`. -1. finally `process_node` updates the open list so `find_path` can run `check_neighbors` on it in the next node in the next iteration of the main loop. +2. `init_find` instantiates the `open_list` and resets all values and counters. The `open_list` is a priority queue that keeps track of nodes to be explored. +3. The main loop starts on the `open_list` which contains all nodes to be processed next (e.g. all current neighbors that are walkable). You need to implement `check_neighbors` in your finder implementation to fill this list. +4. For example in A\* implementation (`AStarFinder`), `check_neighbors` pops the node with the minimum 'f' value from the open list and marks it as closed. It then either returns the path (if the end node is reached) or continues processing neighbors. +5. If the end node is not reached, `check_neighbors` calls `find_neighbors` to get all adjacent walkable nodes. For most algorithms, this calls `grid.neighbors`. +6. If none of the neighbors are walkable, the algorithm terminates. Otherwise, `check_neighbors` calls `process_node` on each neighbor. It calculates the cost `f` for each neighbor node. This involves computing `g` (the cost from the start node to the current node) and `h` (the estimated cost from the current node to the end node, calculated by `apply_heuristic`). +7. Finally `process_node` updates the open list so `find_path` with new or updated nodes. This allows `find_path` to continue the process with the next node in the subsequent iteration. flow: ```pseudo find_path init_find # (re)set global values and open list - check_neighbors # for every node in open list - next_node # closest node to start in open list - find_neighbors # get neighbors - process_node # calculate new cost for neighboring node + while open_list not empty: + check_neighbors # for the node with min 'f' value in open list + pop_node # node with min 'f' value + find_neighbors # get neighbors + process_node # calculate new cost for each neighbor ``` ## Testing -You can run the tests locally using pytest. Take a look at the `test`-folder +Run the tests locally using pytest. For detailed instructions, see the `test` folder: + +```bash +pytest test +``` ## Contributing -Please use the [issue tracker](https://github.com/harisankar95/pathfinding3D/issues) to submit bug reports and feature requests. Please use merge requests as described [here](/CONTRIBUTING.md) to add/adapt functionality. +We welcome contributions of all sizes and levels! For bug reports and feature requests, please use the [issue tracker](https://github.com/harisankar95/pathfinding3D/issues) to submit bug reports and feature requests. Please Follow the guidelines in [CONTRIBUTING.md](/CONTRIBUTING.md) for submitting merge requests. ## License -python-pathfinding is distributed under the [MIT license](https://opensource.org/licenses/MIT). +Pathfinding3D is distributed under the [MIT license](https://opensource.org/licenses/MIT). ## Authors / Contributers -Authors and contributers are [listed on github](https://github.com/harisankar95/pathfinding3D/graphs/contributors). +Find a list of contributors [here](https://github.com/harisankar95/pathfinding3D/graphs/contributors). From e36bfb84059352e3fa1f770d74a219b3472f3b1b Mon Sep 17 00:00:00 2001 From: Hari Date: Sat, 20 Jan 2024 23:49:52 +0100 Subject: [PATCH 06/10] formated with black --- pathfinding3d/core/world.py | 8 ++------ pathfinding3d/finder/best_first.py | 4 +--- test/test_grid.py | 9 ++++++++- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/pathfinding3d/core/world.py b/pathfinding3d/core/world.py index e5ca641..0f033fb 100644 --- a/pathfinding3d/core/world.py +++ b/pathfinding3d/core/world.py @@ -27,13 +27,9 @@ def neighbors(self, node: GridNode, diagonal_movement: int) -> List[GridNode]: neighbors of the given node """ - return self.grids[node.grid_id].neighbors( - node, diagonal_movement=diagonal_movement - ) + return self.grids[node.grid_id].neighbors(node, diagonal_movement=diagonal_movement) - def calc_cost( - self, node_a: GridNode, node_b: GridNode, weighted: bool = False - ) -> float: + def calc_cost(self, node_a: GridNode, node_b: GridNode, weighted: bool = False) -> float: """ Calculate the cost between two nodes. diff --git a/pathfinding3d/finder/best_first.py b/pathfinding3d/finder/best_first.py index e50519f..ee725b3 100644 --- a/pathfinding3d/finder/best_first.py +++ b/pathfinding3d/finder/best_first.py @@ -48,9 +48,7 @@ def __init__( self.weighted = False - def apply_heuristic( - self, node_a: GridNode, node_b: GridNode, heuristic: Optional[Callable] = None - ) -> float: + def apply_heuristic(self, node_a: GridNode, node_b: GridNode, heuristic: Optional[Callable] = None) -> float: """ Helper function to apply heuristic diff --git a/test/test_grid.py b/test/test_grid.py index edf96ac..fa9f99c 100644 --- a/test/test_grid.py +++ b/test/test_grid.py @@ -89,7 +89,14 @@ def test_diagonal_movement_never(gen_grid): neighbors = test_grid.neighbors(node, DiagonalMovement.never) assert len(neighbors) == 6 # Only 6 neighbors (no diagonals) # only the following nodes are considered neighbors: - expected_neighbors = [(0, 1, 1), (1, 0, 1), (1, 2, 1), (2, 1, 1), (1, 1, 0), (1, 1, 2)] + expected_neighbors = [ + (0, 1, 1), + (1, 0, 1), + (1, 2, 1), + (2, 1, 1), + (1, 1, 0), + (1, 1, 2), + ] actual_neighbors = [(neighbor.x, neighbor.y, neighbor.z) for neighbor in neighbors] # assert all neighbors are in the expected list for neighbor in actual_neighbors: From 53af2a5855c7f422b08fb6edb921ab8514bf9c31 Mon Sep 17 00:00:00 2001 From: Hari Date: Sun, 21 Jan 2024 00:38:31 +0100 Subject: [PATCH 07/10] updated documentation for better understanding --- docs/INSTALL.md | 8 +- docs/INTRO.md | 10 +- docs/USAGE.md | 188 ++++++++++++++++++++++++++------ examples/01_basic_usage.md | 69 ++++++------ examples/02_connecting_grids.md | 133 +++++++++++++++++----- 5 files changed, 310 insertions(+), 98 deletions(-) diff --git a/docs/INSTALL.md b/docs/INSTALL.md index 75bf9c8..dba01e9 100644 --- a/docs/INSTALL.md +++ b/docs/INSTALL.md @@ -1,5 +1,10 @@ # Installation +## Requirements + +- python - 3.8 or higher +- numpy + ## PyPI The package is available on pypi, so you can install it with pip: @@ -8,10 +13,11 @@ The package is available on pypi, so you can install it with pip: pip install pathfinding3d ``` -## For development purpose use editable mode +## For development purposes please use editable mode ```bash git clone https://github.com/harisankar95/pathfinding3D cd pathfinding3D pip install -e .[dev] +git checkout -b ``` diff --git a/docs/INTRO.md b/docs/INTRO.md index 96b95f0..dff411f 100644 --- a/docs/INTRO.md +++ b/docs/INTRO.md @@ -4,14 +4,16 @@ Pathfinding algorithms for python3 froked from [python-pathfinding](https://github.com/brean/python-pathfinding) by [@brean](https://github.com/brean). +Pathfinding3D is a comprehensive library designed for 3D pathfinding applications. + Currently there are 7 path-finders bundled in this library, namely: -- A* -- Dijkstra +- A\*: Versatile and most widely used algorithm. +- Dijkstra: A\* without heuristic. - Best-First -- Bi-directional A* +- Bi-directional A\*: Efficient for large graphs with a known goal. - Breadth First Search (BFS) -- Iterative Deeping A\* (IDA*) +- Iterative Deeping A\* (IDA\*): Memory efficient algorithm for large graphs. - Minimum Spanning Tree (MSP) Dijkstra, A\* and Bi-directional A\* take the weight of the fields on the map into account. diff --git a/docs/USAGE.md b/docs/USAGE.md index aa1eebd..3a3fe0f 100644 --- a/docs/USAGE.md +++ b/docs/USAGE.md @@ -1,91 +1,213 @@ # Examples -For usage examples with detailed descriptions take a look at the [examples](https://github.com/harisankar95/pathfinding3D/tree/main/examples/) folder, also take a look at the [test/](https://github.com/harisankar95/pathfinding3D/tree/main/test/) folder for more examples. +For usage examples with detailed descriptions take a look at the [examples](https://github.com/harisankar95/pathfinding3D/tree/main/examples/) folder. ## Basic usage -A simple usage example to find a path using A*. +A simple usage example to find a path using the A* algorithm in a 3D environment. -1. import the required libraries: +1. Import the required libraries: ```python import numpy as np from pathfinding3d.core.diagonal_movement import DiagonalMovement from pathfinding3d.core.grid import Grid - from pathfinding3d.core.node import GridNode from pathfinding3d.finder.a_star import AStarFinder ``` -1. Create a map using a 2D-list. Any value smaller or equal to 0 describes an obstacle. Any number bigger than 0 describes the weight of a field that can be walked on. The bigger the number the higher the cost to walk that field. We ignore the weight for now, all fields have the same cost of 1. Feel free to create a more complex map or use some sensor data as input for it. +2. Create a Map: + - Define a 3D grid using a numpy array. Any value smaller or equal to 0 represents an obstacle. Any positive value represents the weight of a field that can be walked on. The bigger the value higher the cost to walk that field. In this example, all walkable fields have the same cost of 1. Feel free to create a more complex map or use some sensor data as input for it. ```python - # is of style 'x, y, z' + # Define the 3D grid as 'x, y, z' matrix = np.ones((3, 3, 3)) - matrix[1, 1, 1] = 0 + matrix[1, 1, 1] = 0 # Setting an obstacle at the center ``` - Note: you can use negative values to describe different types of obstacles. It does not make a difference for the path finding algorithm but it might be useful for your later map evaluation. + Note: You can use different positive values to represent varying costs of traversing different fields. -1. we create a new grid from this map representation. This will create Node instances for every element of our map. It will also set the size of the map. We assume that your map is a square, so the size width is defined by the length of the outer list, the height by the length of the inner lists, and the depth by the length of the innermost lists. +3. Create the Grid: + - Instantiate a Grid object from the map. This will create Node instances for every element of the map and also sets the size of the map. ```python grid = Grid(matrix=matrix) ``` -1. we get the start and end node from the grid. We assume that the start is at the top left corner and the end at the bottom right corner. You can access the nodes by their coordinates. The first parameter is the x coordinate, the second the y coordinate and the third the z coordinate. The coordinates start at 0. So the top left corner is (0, 0, 0) and the bottom right corner is (2, 2, 2). +4. Define Start and End Nodes: + - Determine the start and end nodes on the grid. Coordinates start at 0. The nodes can be accessed by their coordinates. The first parameter is the x coordinate, the second the y coordinate and the third the z coordinate. So the top left corner is (0, 0, 0) and the bottom right corner is (2, 2, 2). ```python start = grid.node(0, 0, 0) end = grid.node(2, 2, 2) ``` -1. create a new instance of our finder and let it do its work. We allow diagonal movement. The `find_path` function does not only return you the path from the start to the end point it also returns the number of times the algorithm needed to be called until a way was found. +5. Instantiate the Pathfinding Finder: + - Create a new instance of `AStarFinder` and find the path. Here we allow diagonal movement which is not allowed by default. The `find_path` method returns the path and the number of iterations needed. ```python finder = AStarFinder(diagonal_movement=DiagonalMovement.always) path, runs = finder.find_path(start, end, grid) ``` -1. thats it. We found a way. Now we can print the result (or do something else with it). Note that the start and end points are part of the path. +6. Output the Result: + - The path is a list with all the waypoints as nodes. Convert it to a list of coordinate tuples to pretty print it. Note that the start and end points are part of the path. ```python - print('operations:', runs, 'path length:', len(path)) - print('path:', path) + # Path will be a list with all the waypoints as nodes + # Convert it to a list of coordinate tuples + path = [p.identifier for p in path] + + # Output the results + print("operations:", runs, "path length:", len(path)) + print("path:", path) ``` -Here is the whole example if you just want to copy-and-paste the code and play with it: +Here is the whole example which you can copy-and-paste to play with it: -```python + ```python import numpy as np from pathfinding3d.core.diagonal_movement import DiagonalMovement from pathfinding3d.core.grid import Grid - from pathfinding3d.core.node import GridNode from pathfinding3d.finder.a_star import AStarFinder - + # Define the 3D grid matrix = np.ones((3, 3, 3)) matrix[1, 1, 1] = 0 grid = Grid(matrix=matrix) + # Define start and end nodes start = grid.node(0, 0, 0) end = grid.node(2, 2, 2) + # Instantiate the finder and find the path finder = AStarFinder(diagonal_movement=DiagonalMovement.always) - path_, runs = finder.find_path(start, end, grid) - - assert isinstance(path_, list) - assert len(path_) > 0 - assert isinstance(runs, int) - - path = [] - for node in path_: - if isinstance(node, GridNode): - path.append([node.x, node.y, node.z]) - elif isinstance(node, tuple): - path.append([node[0], node[1], node[2]]) - - print('operations:', runs, 'path length:', len(path)) - print('path:', path) + path, runs = finder.find_path(start, end, grid) + + # Path will be a list with all the waypoints as nodes + # Convert it to a list of coordinate tuples + path = [p.identifier for p in path] + + # Output the results + print("operations:", runs, "path length:", len(path)) + print("path:", path) + ``` + +## Steps/Portals/Bridges + +With *pathfinding3d*, you can seamlessly connect multiple grids. This feature is invaluable for simulating multi-story structures connected by staircases, bridges between different buildings, or even magical portals linking disparate locations. + +### Toy Example: Connecting Two Building Storeys with a Bridge + +Let's consider an example where we want to connect the second storey of two adjacent buildings with a bridge. + +1. Define the Grids for Each Building: + + - Create two 3D grid arrays representing each building ('world0' and 'world1'). + + ```python + from pathfinding3d.core.grid import Grid + from pathfinding3d.core.world import World + from pathfinding3d.finder.a_star import AStarFinder + + world0 = [[[1, 1, 1], [1, 0, 1], [1, 1, 1]], + [[1, 1, 1], [1, 0, 1], [1, 1, 1]], + [[1, 1, 1], [1, 0, 1], [1, 1, 1]]] + world1 = [[[1, 1, 1], [1, 0, 1], [1, 1, 1]], + [[1, 1, 1], [1, 0, 1], [1, 1, 1]], + [[1, 1, 1], [1, 0, 1], [1, 1, 1]]] + # create Grid instances for both worlds + grid0 = Grid(matrix=world0, grid_id=0) + grid1 = Grid(matrix=world1, grid_id=1) + ``` + +2. Connect Nodes Between Grids: + - Establish a two-way connection between nodes in different grids to simulate a bridge. + + ```python + # connect the two worlds + grid0.node(2, 2, 2).connect(grid1.node(2, 2, 2)) + grid1.node(2, 2, 2).connect(grid0.node(2, 2, 2)) + ``` + + Note: Establish connections in both directions for bi-directional travel. For unidirectional connections, define only one link. + +3. Using the `World` to Manage Multiple Grids: + - Create a `World` object to manage both grids, allowing `find_neighbors` to access them during pathfinding. + + ```python + # create a world instance + world = World({0: grid0, 1: grid1}) + ``` + +4. Pathfinding Across Connected Grids: + - Utilize `AStarFinder` to find a path from a start node in the first grid to an end node in the second grid. This time we need to pass the `world` object to the `find_path` method. + + ```python + # create a finder instance + finder = AStarFinder() + # find the path + path, _ = finder.find_path(grid0.node(2, 0, 0), grid1.node(2, 0, 0), world) + ``` + + This will output a path traversing from one building to another, demonstrating the unique capability of connecting different 3D spaces. + +5. Visualizing the Path: + - The path is a list of `Node` objects. We can convert the path to a list of tuples (x, y, z, grid_id) and separate the path into two lists of tuples (x, y, z) for each grid. + + ```python + # convert the path to a list of tuples (x, y, z, grid_id) + path = [p.identifier for p in path] + # separate the path into two lists of tuples (x, y, z) + path0 = [p[:3] for p in path if p[3] == 0] + path1 = [p[:3] for p in path if p[3] == 1] + # print the paths + print("path through world 0:", path0) + print("path through world 1:", path1) + ``` + +6. The output should look like this: + + ```python + path through world 0: [(2, 0, 0), (2, 1, 0), (2, 2, 0), (2, 2, 1), (2, 2, 2)] + path through world 1: [(2, 2, 2), (2, 1, 2), (2, 0, 2), (2, 0, 1), (2, 0, 0)] + ``` + + The path through world 0 starts at (2, 0, 0) and ends at (2, 2, 2). The path through world 1 starts at (2, 2, 2) and ends at (2, 0, 0). The two paths are connected by the bridge at (2, 2, 2). + +7. Putting it all together: + + ```python + from pathfinding3d.core.grid import Grid + from pathfinding3d.core.world import World + from pathfinding3d.finder.a_star import AStarFinder + + world0 = [[[1, 1, 1], [1, 0, 1], [1, 1, 1]], [[1, 1, 1], [1, 0, 1], [1, 1, 1]], [[1, 1, 1], [1, 0, 1], [1, 1, 1]]] + world1 = [[[1, 1, 1], [1, 0, 1], [1, 1, 1]], [[1, 1, 1], [1, 0, 1], [1, 1, 1]], [[1, 1, 1], [1, 0, 1], [1, 1, 1]]] + # create Grid instances for both worlds + grid0 = Grid(matrix=world0, grid_id=0) + grid1 = Grid(matrix=world1, grid_id=1) + + # connect the two worlds + grid0.node(2, 2, 2).connect(grid1.node(2, 2, 2)) + grid1.node(2, 2, 2).connect(grid0.node(2, 2, 2)) + + # create a world instance + world = World({0: grid0, 1: grid1}) + + # create a finder instance + finder = AStarFinder() + # find the path + path, _ = finder.find_path(grid0.node(2, 0, 0), grid1.node(2, 0, 0), world) + + # convert the path to a list of tuples (x, y, z, grid_id) + path = [p.identifier for p in path] + # separate the path into two lists of tuples (x, y, z) + path0 = [p[:3] for p in path if p[3] == 0] + path1 = [p[:3] for p in path if p[3] == 1] + # print the paths + print("path through world 0:", path0) + print("path through world 1:", path1) + ``` diff --git a/examples/01_basic_usage.md b/examples/01_basic_usage.md index 193151e..e005fe7 100644 --- a/examples/01_basic_usage.md +++ b/examples/01_basic_usage.md @@ -1,87 +1,92 @@ # Basic usage -A simple usage example to find a path using A*. +A simple usage example to find a path using the A* algorithm in a 3D environment. -1. import the required libraries: +1. Import the required libraries: ```python import numpy as np from pathfinding3d.core.diagonal_movement import DiagonalMovement from pathfinding3d.core.grid import Grid - from pathfinding3d.core.node import GridNode from pathfinding3d.finder.a_star import AStarFinder ``` -1. Create a map using a 2D-list. Any value smaller or equal to 0 describes an obstacle. Any number bigger than 0 describes the weight of a field that can be walked on. The bigger the number the higher the cost to walk that field. We ignore the weight for now, all fields have the same cost of 1. Feel free to create a more complex map or use some sensor data as input for it. +2. Create a Map: + - Define a 3D grid using a numpy array. Any value smaller or equal to 0 represents an obstacle. Any positive value represents the weight of a field that can be walked on. The bigger the value higher the cost to walk that field. In this example, all walkable fields have the same cost of 1. Feel free to create a more complex map or use some sensor data as input for it. ```python - # is of style 'x, y, z' + # Define the 3D grid as 'x, y, z' matrix = np.ones((3, 3, 3)) - matrix[1, 1, 1] = 0 + matrix[1, 1, 1] = 0 # Setting an obstacle at the center ``` - Note: you can use negative values to describe different types of obstacles. It does not make a difference for the path finding algorithm but it might be useful for your later map evaluation. + Note: You can use different positive values to represent varying costs of traversing different fields. -1. we create a new grid from this map representation. This will create Node instances for every element of our map. It will also set the size of the map. We assume that your map is a square, so the size width is defined by the length of the outer list, the height by the length of the inner lists, and the depth by the length of the innermost lists. +3. Create the Grid: + - Instantiate a Grid object from the map. This will create Node instances for every element of the map and also sets the size of the map. ```python grid = Grid(matrix=matrix) ``` -1. we get the start and end node from the grid. We assume that the start is at the top left corner and the end at the bottom right corner. You can access the nodes by their coordinates. The first parameter is the x coordinate, the second the y coordinate and the third the z coordinate. The coordinates start at 0. So the top left corner is (0, 0, 0) and the bottom right corner is (2, 2, 2). +4. Define Start and End Nodes: + - Determine the start and end nodes on the grid. Coordinates start at 0. The nodes can be accessed by their coordinates. The first parameter is the x coordinate, the second the y coordinate and the third the z coordinate. So the top left corner is (0, 0, 0) and the bottom right corner is (2, 2, 2). ```python start = grid.node(0, 0, 0) end = grid.node(2, 2, 2) ``` -1. create a new instance of our finder and let it do its work. We allow diagonal movement. The `find_path` function does not only return you the path from the start to the end point it also returns the number of times the algorithm needed to be called until a way was found. +5. Instantiate the Pathfinding Finder: + - Create a new instance of `AStarFinder` and find the path. Here we allow diagonal movement which is not allowed by default. The `find_path` method returns the path and the number of iterations needed. ```python finder = AStarFinder(diagonal_movement=DiagonalMovement.always) path, runs = finder.find_path(start, end, grid) ``` -1. thats it. We found a way. Now we can print the result (or do something else with it). Note that the start and end points are part of the path. +6. Output the Result: + - The path is a list with all the waypoints as nodes. Convert it to a list of coordinate tuples to pretty print it. Note that the start and end points are part of the path. ```python - print('operations:', runs, 'path length:', len(path)) - print('path:', path) + # Path will be a list with all the waypoints as nodes + # Convert it to a list of coordinate tuples + path = [p.identifier for p in path] + + # Output the results + print("operations:", runs, "path length:", len(path)) + print("path:", path) ``` -Here is the whole example if you just want to copy-and-paste the code and play with it: +Here is the whole example which you can copy-and-paste to play with it: -```python + ```python import numpy as np from pathfinding3d.core.diagonal_movement import DiagonalMovement from pathfinding3d.core.grid import Grid - from pathfinding3d.core.node import GridNode from pathfinding3d.finder.a_star import AStarFinder - + # Define the 3D grid matrix = np.ones((3, 3, 3)) matrix[1, 1, 1] = 0 grid = Grid(matrix=matrix) + # Define start and end nodes start = grid.node(0, 0, 0) end = grid.node(2, 2, 2) + # Instantiate the finder and find the path finder = AStarFinder(diagonal_movement=DiagonalMovement.always) - path_, runs = finder.find_path(start, end, grid) - - assert isinstance(path_, list) - assert len(path_) > 0 - assert isinstance(runs, int) - - path = [] - for node in path_: - if isinstance(node, GridNode): - path.append([node.x, node.y, node.z]) - elif isinstance(node, tuple): - path.append([node[0], node[1], node[2]]) - - print('operations:', runs, 'path length:', len(path)) - print('path:', path) + path, runs = finder.find_path(start, end, grid) + + # Path will be a list with all the waypoints as nodes + # Convert it to a list of coordinate tuples + path = [p.identifier for p in path] + + # Output the results + print("operations:", runs, "path length:", len(path)) + print("path:", path) + ``` diff --git a/examples/02_connecting_grids.md b/examples/02_connecting_grids.md index f78e042..6c80ca4 100644 --- a/examples/02_connecting_grids.md +++ b/examples/02_connecting_grids.md @@ -1,39 +1,116 @@ -# steps/portals/bridges +# Steps/Portals/Bridges -*python-pathfinding-3d* allows you to connect grids. This could be useful to create buildings with multiple storeys that are connected by bridges or different areas you want to connect with portals. +With *pathfinding3d*, you can seamlessly connect multiple grids. This feature is invaluable for simulating multi-story structures connected by staircases, bridges between different buildings, or even magical portals linking disparate locations. -Lets say we want to connect 2nd storey of two buildings with a bridge: +## Toy Example: Connecting Two Building Storeys with a Bridge -```python -world0 = [[[1, 1, 1], [1, 0, 1], [1, 1, 1]], [[1, 1, 1], [1, 0, 1], [1, 1, 1]], [[1, 1, 1], [1, 0, 1], [1, 1, 1]]] -world1 = [[[1, 1, 1], [1, 0, 1], [1, 1, 1]], [[1, 1, 1], [1, 0, 1], [1, 1, 1]], [[1, 1, 1], [1, 0, 1], [1, 1, 1]]] -# create Grid instances for both worlds -grid0 = Grid(matrix=level0, grid_id=0) -grid1 = Grid(matrix=level1, grid_id=1) -``` +Let's consider an example where we want to connect the second storey of two adjacent buildings with a bridge. -We can connect a node from one grid to another by defining a connection between the two grids like this: +1. Define the Grids for Each Building: -```python -grid0.node(2, 2, 2).connect(grid1.node(2, 2, 2)) -grid1.node(2, 2, 2).connect(grid0.node(2, 2, 2)) -``` + - Create two 3D grid arrays representing each building ('world0' and 'world1'). -Note that we need to do this in both directions if we want to allow the connection to go both ways (otherwise the portal can only go in one direction, which might be a desired feature). + ```python + from pathfinding3d.core.grid import Grid + from pathfinding3d.core.world import World + from pathfinding3d.finder.a_star import AStarFinder -Because the `find_neighbors`-function in the finder needs to look up both grids we need to provide both grids to the finder. We can create a "world" that looks up both grids: + world0 = [[[1, 1, 1], [1, 0, 1], [1, 1, 1]], + [[1, 1, 1], [1, 0, 1], [1, 1, 1]], + [[1, 1, 1], [1, 0, 1], [1, 1, 1]]] + world1 = [[[1, 1, 1], [1, 0, 1], [1, 1, 1]], + [[1, 1, 1], [1, 0, 1], [1, 1, 1]], + [[1, 1, 1], [1, 0, 1], [1, 1, 1]]] + # create Grid instances for both worlds + grid0 = Grid(matrix=world0, grid_id=0) + grid1 = Grid(matrix=world1, grid_id=1) + ``` -```python - # create world with both grids -world = World({ - 0: grid0, - 1: grid1 -}) +2. Connect Nodes Between Grids: + - Establish a two-way connection between nodes in different grids to simulate a bridge. -finder = AStarFinder() -path, _ = finder.find_path(grid0.node(2, 0, 0), grid1.node(2, 0, 0), world) -``` + ```python + # connect the two worlds + grid0.node(2, 2, 2).connect(grid1.node(2, 2, 2)) + grid1.node(2, 2, 2).connect(grid0.node(2, 2, 2)) + ``` -and that gives you the path from the start node in the first grid to the end node in the second grid. + Note: Establish connections in both directions for bi-directional travel. For unidirectional connections, define only one link. -For the whole code take a look at the `test_connect_grids.py` file in the `test`-folder +3. Using the `World` to Manage Multiple Grids: + - Create a `World` object to manage both grids, allowing `find_neighbors` to access them during pathfinding. + + ```python + # create a world instance + world = World({0: grid0, 1: grid1}) + ``` + +4. Pathfinding Across Connected Grids: + - Utilize `AStarFinder` to find a path from a start node in the first grid to an end node in the second grid. This time we need to pass the `world` object to the `find_path` method. + + ```python + # create a finder instance + finder = AStarFinder() + # find the path + path, _ = finder.find_path(grid0.node(2, 0, 0), grid1.node(2, 0, 0), world) + ``` + + This will output a path traversing from one building to another, demonstrating the unique capability of connecting different 3D spaces. + +5. Visualizing the Path: + - The path is a list of `Node` objects. We can convert the path to a list of tuples (x, y, z, grid_id) and separate the path into two lists of tuples (x, y, z) for each grid. + + ```python + # convert the path to a list of tuples (x, y, z, grid_id) + path = [p.identifier for p in path] + # separate the path into two lists of tuples (x, y, z) + path0 = [p[:3] for p in path if p[3] == 0] + path1 = [p[:3] for p in path if p[3] == 1] + # print the paths + print("path through world 0:", path0) + print("path through world 1:", path1) + ``` + +6. The output should look like this: + + ```python + path through world 0: [(2, 0, 0), (2, 1, 0), (2, 2, 0), (2, 2, 1), (2, 2, 2)] + path through world 1: [(2, 2, 2), (2, 1, 2), (2, 0, 2), (2, 0, 1), (2, 0, 0)] + ``` + + The path through world 0 starts at (2, 0, 0) and ends at (2, 2, 2). The path through world 1 starts at (2, 2, 2) and ends at (2, 0, 0). The two paths are connected by the bridge at (2, 2, 2). + +7. Putting it all together: + + ```python + from pathfinding3d.core.grid import Grid + from pathfinding3d.core.world import World + from pathfinding3d.finder.a_star import AStarFinder + + world0 = [[[1, 1, 1], [1, 0, 1], [1, 1, 1]], [[1, 1, 1], [1, 0, 1], [1, 1, 1]], [[1, 1, 1], [1, 0, 1], [1, 1, 1]]] + world1 = [[[1, 1, 1], [1, 0, 1], [1, 1, 1]], [[1, 1, 1], [1, 0, 1], [1, 1, 1]], [[1, 1, 1], [1, 0, 1], [1, 1, 1]]] + # create Grid instances for both worlds + grid0 = Grid(matrix=world0, grid_id=0) + grid1 = Grid(matrix=world1, grid_id=1) + + # connect the two worlds + grid0.node(2, 2, 2).connect(grid1.node(2, 2, 2)) + grid1.node(2, 2, 2).connect(grid0.node(2, 2, 2)) + + # create a world instance + world = World({0: grid0, 1: grid1}) + + # create a finder instance + finder = AStarFinder() + # find the path + path, _ = finder.find_path(grid0.node(2, 0, 0), grid1.node(2, 0, 0), world) + + # convert the path to a list of tuples (x, y, z, grid_id) + path = [p.identifier for p in path] + # separate the path into two lists of tuples (x, y, z) + path0 = [p[:3] for p in path if p[3] == 0] + path1 = [p[:3] for p in path if p[3] == 1] + # print the paths + print("path through world 0:", path0) + print("path through world 1:", path1) + ``` From ec08367de8b83f5102ad4f3e2b80940d9bdbafa5 Mon Sep 17 00:00:00 2001 From: Hari Date: Sun, 21 Jan 2024 00:59:48 +0100 Subject: [PATCH 08/10] Update 03_view_map.py --- examples/03_view_map.py | 43 ++++++++++++++++++++++++++--------------- 1 file changed, 27 insertions(+), 16 deletions(-) diff --git a/examples/03_view_map.py b/examples/03_view_map.py index d054edd..93de41d 100644 --- a/examples/03_view_map.py +++ b/examples/03_view_map.py @@ -1,12 +1,18 @@ +""" +Visualize the 3D map and the path found by the A* algorithm +Requires open3d for visualization. Install it using `pip install open3d` +""" + # import the necessary packages import os import numpy as np from pathfinding3d.core.diagonal_movement import DiagonalMovement -from pathfinding3d.core.grid import Grid, GridNode +from pathfinding3d.core.grid import Grid from pathfinding3d.finder.a_star import AStarFinder +# attempt to import Open3D for visualization USE_OPEN3D = True try: # for visualization @@ -16,11 +22,13 @@ USE_OPEN3D = False print("Open3D is not installed. Please install it using 'pip install open3d'") -# load map +# load map as a 3D numpy array +# each element in the matrix represents a point in the space: +# 0 indicates an obstacle, 1 indicates free space sample_map_path = os.path.join(os.path.dirname(__file__), "sample_map.npy") matrix = np.load(sample_map_path) -# define start and end points +# define start and end points as [x, y, z] coordinates start_pt = [21, 21, 21] end_pt = [5, 38, 33] @@ -29,24 +37,24 @@ start = grid.node(*start_pt) end = grid.node(*end_pt) -# initialize A* finder +# initialize A* finder with specified diagonal movement setting finder = AStarFinder(diagonal_movement=DiagonalMovement.only_when_no_obstacle) -path_, runs = finder.find_path(start, end, grid) + +# use the finder to get the path +path, runs = finder.find_path(start, end, grid) + +# print results path_cost = end.g -print(f"path cost: {path_cost:.4f}, path length: {len(path_)}, runs: {runs}") - -path = [] -for node in path_: - if isinstance(node, GridNode): - path.append([node.x, node.y, node.z]) - elif isinstance(node, tuple): - path.append([node[0], node[1], node[2]]) +print(f"path cost: {path_cost:.4f}, path length: {len(path)}, runs: {runs}") + +# convert the path to a list of coordinate tuples +path = [p.identifier for p in path] print(f"path: {path}") # visualize path in open3d if USE_OPEN3D: - # Find the obstacles and represent in blue + # Identifying obstacles and representing them in blu obstacle_indices = np.where(matrix == 0) xyz_pt = np.stack(obstacle_indices, axis=-1).astype(float) colors = np.zeros((xyz_pt.shape[0], 3)) @@ -57,15 +65,18 @@ end_color = np.array([[0, 1.0, 0]]) # Green path_colors = np.full((len(path) - 2, 3), [0.7, 0.7, 0.7]) # Grey for the path - # Combine points and colors + # Combine points and colors for visualization xyz_pt = np.concatenate((xyz_pt, [start_pt], [end_pt], path[1:-1])) colors = np.concatenate((colors, start_color, end_color, path_colors)) - # Create and visualize the point cloud + # Create the point cloud pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(xyz_pt) pcd.colors = o3d.utility.Vector3dVector(colors) + # Create the voxel grid from the point cloud voxel_grid = o3d.geometry.VoxelGrid.create_from_point_cloud(pcd, voxel_size=1.0) axes = o3d.geometry.TriangleMesh.create_coordinate_frame(size=15.0, origin=np.array([-3.0, -3.0, -3.0])) + + # Visualize the voxel grid o3d.visualization.draw_geometries([axes, voxel_grid], window_name="Voxel Env", width=1024, height=768) From b624762b3cd344eb34f96b04ddddaf08e539ebed Mon Sep 17 00:00:00 2001 From: Hari Date: Sun, 21 Jan 2024 10:25:36 +0100 Subject: [PATCH 09/10] Update setup.cfg --- setup.cfg | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/setup.cfg b/setup.cfg index 738a327..4ee2cdf 100644 --- a/setup.cfg +++ b/setup.cfg @@ -21,22 +21,23 @@ source = pathfinding3d show_missing = True exclude_also = def __repr__ - def __str__ - def __lt__ - def __eq__ + def __str__ + def __lt__ + def __eq__ [pylint] -disable = missing-docstring, - invalid-name, - too-many-instance-attributes, - too-many-locals, - too-many-nested-blocks, - too-many-public-methods, - too-few-public-methods, - too-many-arguments, - too-many-branches, - # many functions will naturally have unused arguments. - unused-argument, +disable = + missing-docstring, + invalid-name, + too-many-instance-attributes, + too-many-locals, + too-many-nested-blocks, + too-many-public-methods, + too-few-public-methods, + too-many-arguments, + too-many-branches, + # many functions will naturally have unused arguments. + unused-argument, [pylint.FORMAT] max-line-length = 120 From 97806e7bef769fdbfa4972bbb0c92f98bf621436 Mon Sep 17 00:00:00 2001 From: Hari Date: Sun, 21 Jan 2024 10:28:41 +0100 Subject: [PATCH 10/10] Update setup.py --- setup.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 8ecee78..bf61569 100644 --- a/setup.py +++ b/setup.py @@ -44,5 +44,6 @@ "pytest", "coverage", ], - min_python_version="3.8", + python_requires=">=3.8", + platforms=["any"], )