Source code for pathfinding3d.core.grid

import math
import warnings
from functools import lru_cache
from typing import List, Optional, Tuple, Union

import numpy as np

from .diagonal_movement import DiagonalMovement
from .node import GridNode

try:
    import plotly.graph_objects as go

    USE_PLOTLY = True
except ImportError:
    USE_PLOTLY = False


MatrixType = Optional[Union[List[List[List[int]]], np.ndarray]]


[docs]def build_nodes( width: int, height: int, depth: int, matrix: MatrixType = None, inverse: bool = False, grid_id: Optional[int] = None, ) -> List[List[List[GridNode]]]: """ Create nodes according to grid size. If a matrix is given it will be used to determine what nodes are walkable. Parameters ---------- width : int The width of the grid. height : int The height of the grid. depth : int The depth of the grid. matrix : MatrixType A 3D array of values (numbers or objects specifying weight) that determine how nodes are connected and if they are walkable. If no matrix is given, all nodes will be walkable. inverse : bool, optional If true, all values in the matrix that are not 0 will be considered walkable. Otherwise all values that are 0 will be considered walkable. grid_id : int, optional The id of the grid. Returns ------- List[List[List[GridNode]]] A list of list of lists containing the nodes in the grid. """ nodes: List = [] use_matrix = matrix is not None for x in range(width): nodes.append([]) for y in range(height): nodes[x].append([]) for z in range(depth): # 0, '0', False will be obstacles # all other values mark walkable cells. # you can use values bigger then 1 to assign a weight. # If inverse is False it changes # (1 and up becomes obstacle and 0 or everything negative marks a # free cells) weight = int(matrix[x][y][z]) if use_matrix else 1 walkable = weight <= 0 if inverse else weight >= 1 nodes[x][y].append(GridNode(x=x, y=y, z=z, walkable=walkable, weight=weight, grid_id=grid_id)) return nodes
[docs]class Grid: """ A grid represents the map (as 3d-list of nodes). """
[docs] def __init__( self, width: int = 0, height: int = 0, depth: int = 0, matrix: MatrixType = None, grid_id: Optional[int] = None, inverse: bool = False, ): """ Create a new grid. Parameters ---------- width : int, optional The width of the grid. height : int, optional The height of the grid. depth : int, optional The depth of the grid. matrix : MatrixType A 3D array of values (numbers or objects specifying weight) that determine how nodes are connected and if they are walkable. If no matrix is given, all nodes will be walkable. inverse : bool, optional If true, all values in the matrix that are not 0 will be considered walkable. Otherwise all values that are 0 will be considered walkable. """ self.width, self.height, self.depth = self._validate_dimensions(width, height, depth, matrix) self.nodes = ( build_nodes(self.width, self.height, self.depth, matrix, inverse, grid_id) if self.is_valid_grid() else [[[]]] )
[docs] def _validate_dimensions(self, width: int, height: int, depth: int, matrix: MatrixType) -> tuple: if matrix is not None: if not ( isinstance(matrix, (list, np.ndarray)) and len(matrix) > 0 and len(matrix[0]) > 0 and len(matrix[0][0]) > 0 ): raise ValueError("Provided matrix is not a 3D structure or is empty.") return len(matrix), len(matrix[0]), len(matrix[0][0]) return width, height, depth
[docs] def is_valid_grid(self) -> bool: """ Check if grid is valid Returns ------- bool True, if grid is valid """ return self.width > 0 and self.height > 0 and self.depth > 0
[docs] def node(self, x: int, y: int, z: int) -> Optional[GridNode]: """ Get node at position Parameters ---------- x : int x position y : int y position z : int z position Returns ------- GridNode node at position """ return self.nodes[x][y][z] if self.inside(x, y, z) else None
[docs] def inside(self, x: int, y: int, z: int) -> bool: """ Check, if field position is inside map Parameters ---------- x : int x position y : int y position z : int z position Returns ------- bool True, if position is inside map """ return 0 <= x < self.width and 0 <= y < self.height and 0 <= z < self.depth
[docs] def walkable(self, x: int, y: int, z: int) -> bool: """ Check, if the tile is inside grid and if it is set as walkable Parameters ---------- x : int x position y : int y position z : int z position Returns ------- bool True, if position is inside map and walkable """ return self.inside(x, y, z) and self.nodes[x][y][z].walkable
[docs] @lru_cache(maxsize=128) def _calc_cost(self, dx: int, dy: int, dz: int) -> float: """ Get the distance between current node and the neighbor (cost) Parameters ---------- dx : int x distance dy : int y distance dz : int z distance Returns ------- float distance between current node and the neighbor (cost) """ return math.sqrt(dx * dx + dy * dy + dz * dz)
[docs] def calc_cost(self, node_a: GridNode, node_b: GridNode, weighted: bool = False) -> float: """ Get the distance between current node and the neighbor (cost) Parameters ---------- node_a : GridNode current node node_b : GridNode neighbor node weighted : bool, optional True, if weighted algorithm is used, by default False Returns ------- float distance between current node and the neighbor (cost) """ # Check if we have a straight, diagonal in plane or diagonal in space dx = node_b.x - node_a.x dy = node_b.y - node_a.y dz = node_b.z - node_a.z ng = self._calc_cost(dx, dy, dz) # weight for weighted algorithms if weighted: ng *= node_b.weight return ng
[docs] def neighbors( self, node: GridNode, diagonal_movement: int = DiagonalMovement.never, ) -> List[GridNode]: """ Get all neighbors of one node Parameters ---------- node : GridNode node to get neighbors from diagonal_movement : int, optional if diagonal movement is allowed (see enum in diagonal_movement), by default DiagonalMovement.never Returns ------- List[GridNode] list of all neighbors """ x, y, z = node.x, node.y, node.z neighbors = [] # current plane cs0 = cd0 = cs1 = cd1 = cs2 = cd2 = cs3 = cd3 = False # upper plane us0 = ud0 = us1 = ud1 = us2 = ud2 = us3 = ud3 = ut = False # ut = upper top # 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 # +y if self.walkable(x, y + 1, z): neighbors.append(self.nodes[x][y + 1][z]) cs2 = 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): neighbors.append(self.nodes[x][y][z + 1]) ut = True # -z if self.walkable(x, y, z - 1): neighbors.append(self.nodes[x][y][z - 1]) lb = True # check for connections to other grids if node.connections: neighbors.extend(node.connections) if diagonal_movement == DiagonalMovement.never: return neighbors if diagonal_movement == DiagonalMovement.only_when_no_obstacle: cd0 = cs0 and cs1 cd1 = cs1 and cs2 cd2 = cs2 and cs3 cd3 = cs3 and cs0 us0 = cs0 and ut us1 = cs1 and ut us2 = cs2 and ut us3 = cs3 and ut ls0 = cs0 and lb ls1 = cs1 and lb ls2 = cs2 and lb ls3 = cs3 and lb elif diagonal_movement == DiagonalMovement.if_at_most_one_obstacle: cd0 = cs0 or cs1 cd1 = cs1 or cs2 cd2 = cs2 or cs3 cd3 = cs3 or cs0 us0 = cs0 or ut us1 = cs1 or ut us2 = cs2 or ut us3 = cs3 or ut ls0 = cs0 or lb ls1 = cs1 or lb ls2 = cs2 or lb ls3 = cs3 or lb elif diagonal_movement == DiagonalMovement.always: cd0 = cd1 = cd2 = cd3 = True us0 = us1 = us2 = us3 = True ls0 = ls1 = ls2 = ls3 = True # +x -y 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 cd2 and self.walkable(x - 1, y + 1, z): neighbors.append(self.nodes[x - 1][y + 1][z]) else: cd2 = False # -x -y if cd3 and self.walkable(x - 1, y - 1, z): neighbors.append(self.nodes[x - 1][y - 1][z]) else: 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 us1 and self.walkable(x + 1, y, z + 1): neighbors.append(self.nodes[x + 1][y][z + 1]) else: us1 = False # +y +z if us2 and self.walkable(x, y + 1, z + 1): neighbors.append(self.nodes[x][y + 1][z + 1]) else: us2 = False # -x +z if us3 and self.walkable(x - 1, y, z + 1): neighbors.append(self.nodes[x - 1][y][z + 1]) else: us3 = False # -y -z if ls0 and self.walkable(x, y - 1, z - 1): neighbors.append(self.nodes[x][y - 1][z - 1]) else: ls0 = False # +x -z if ls1 and self.walkable(x + 1, y, z - 1): neighbors.append(self.nodes[x + 1][y][z - 1]) else: ls1 = False # +y -z if ls2 and self.walkable(x, y + 1, z - 1): neighbors.append(self.nodes[x][y + 1][z - 1]) else: ls2 = False # -x -z if ls3 and self.walkable(x - 1, y, z - 1): neighbors.append(self.nodes[x - 1][y][z - 1]) else: ls3 = False # remaining daigonal neighbors if diagonal_movement == DiagonalMovement.only_when_no_obstacle: 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 = 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([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([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 ud1 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 ud3 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): 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 ld2 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): neighbors.append(self.nodes[x - 1][y - 1][z - 1]) return neighbors
[docs] def cleanup(self): """ Cleanup grid """ for x_nodes in self.nodes: for y_nodes in x_nodes: for z_node in y_nodes: z_node.cleanup()
[docs] def visualize( self, path: Optional[List[Union[GridNode, Tuple]]] = None, start: Optional[Union[GridNode, Tuple]] = None, end: Optional[Union[GridNode, Tuple]] = None, visualize_weight: bool = True, save_html: bool = False, save_to: str = "./pathfinding3d_visualization.html", always_show: bool = False, ): """ Creates a 3D visualization of the grid, including optional path, start/end points, and node weights using plotly. This method is designed to help visualize the spatial layout, obstacles, and pathfinding results in three dimensions. Parameters ---------- path : Optional[List[Union[GridNode, Tuple]]], optional The path to visualize, specified as a list of GridNode instances or coordinate tuples (x, y, z). If omitted, the grid is visualized without a path. Defaults to None. start : Optional[Union[GridNode, Tuple]], optional The start node for the path, as either a GridNode or a tuple of coordinates. If not provided and a path is given, the first node of the path is used. Defaults to None. end : Optional[Union[GridNode, Tuple]], optional The end node for the path, as either a GridNode or a tuple of coordinates. If not provided and a path is given, the last node of the path is used. Defaults to None. visualize_weight : bool, optional Whether to visualize the weights of the nodes, enhancing the representation of node costs. Defaults to True. save_html : bool, optional If True, the visualization is saved to an HTML file specified by `save_to` instead of being displayed. Defaults to False. save_to : str, optional File path where the HTML visualization is saved if `save_html` is True. Defaults to "./pathfinding3d_visualization.html". always_show : bool, optional If True, displays the visualization in the browser even when `save_html` is True. Useful for immediate feedback while also saving the result. Defaults to False. Raises ------ Warning If plotly is not installed, a warning is logged and the visualization is skipped. Notes ----- - Requires plotly for visualization. Install with `pip install plotly` if not already installed. """ if not USE_PLOTLY: warnings.warn("Plotly is not installed. Please install it to use this feature.") return # Extract obstacle and weight information directly from the grid X, Y, Z, obstacle_values, weight_values = [], [], [], [], [] for x in range(self.width): for y in range(self.height): for z in range(self.depth): node = self.node(x, y, z) X.append(x) Y.append(y) Z.append(z) obstacle_values.append(0 if node.walkable else 1) weight_values.append(node.weight if node.walkable else 0) # Create obstacle volume visualization obstacle_vol = go.Volume( x=np.array(X), y=np.array(Y), z=np.array(Z), value=np.array(obstacle_values), isomin=0.1, isomax=1.0, opacity=0.1, surface_count=25, # Increase for better visibility colorscale="Greys", showscale=False, name="Obstacles", ) # List of items to visualize visualizations = [obstacle_vol] # Create weight volume visualization if visualize_weight: weight_vol = go.Volume( x=np.array(X), y=np.array(Y), z=np.array(Z), value=np.array(weight_values), isomin=1.01, # Assuming default weight is 1, adjust as needed isomax=max(weight_values) * 1.01, opacity=0.5, # Adjust for better visibility surface_count=25, colorscale="Viridis", # A different colorscale for distinction showscale=True, colorbar=dict(title="Weight", ticks="outside"), ) visualizations.append(weight_vol) # Add path visualization if path is provided if path: # Convert path to coordinate tuples path = [p.identifier if isinstance(p, GridNode) else p for p in path] # Create path visualization path_x, path_y, path_z = zip(*path) path_trace = go.Scatter3d( x=path_x, y=path_y, z=path_z, mode="markers+lines", marker=dict(size=6, color="red", opacity=0.9), line=dict(color="red", width=3), name="Path", hovertext=[f"Step {i}: ({x}, {y}, {z})" for i, (x, y, z) in enumerate(path)], hoverinfo="text", ) visualizations.append(path_trace) # Set start and end nodes if not provided start = start or path[0] end = end or path[-1] # Add start and end node visualizations if available if start: start = start.identifier if isinstance(start, GridNode) else start start_trace = go.Scatter3d( x=[start[0]], y=[start[1]], z=[start[2]], mode="markers", marker=dict(size=8, color="green", symbol="diamond"), name="Start", hovertext=f"Start: {start}", hoverinfo="text", ) visualizations.append(start_trace) if end: end = end.identifier if isinstance(end, GridNode) else end end_trace = go.Scatter3d( x=[end[0]], y=[end[1]], z=[end[2]], mode="markers", marker=dict(size=8, color="blue", symbol="diamond"), name="End", hovertext=f"End: {end}", hoverinfo="text", ) visualizations.append(end_trace) # Camera settings # Set camera perpendicular to the z-axis camera = dict(eye=dict(x=0.0, y=0.0, z=self.depth / 4)) # Specify layout layout = go.Layout( title="3D Pathfinding Visualization", scene=dict( xaxis=dict(title="X-axis", showbackground=True), yaxis=dict(title="Y-axis", showbackground=True), zaxis=dict(title="Z-axis", showbackground=True), aspectmode="auto", ), legend=dict(yanchor="top", y=0.99, xanchor="left", x=0.01), autosize=True, scene_camera=camera, ) # Create figure fig = go.Figure(data=visualizations, layout=layout) # Save visualization to HTML file if specified if save_html: fig.write_html(save_to, auto_open=False) print(f"Visualization saved to: {save_to}") if always_show or not save_html: fig.show()