Tutorial: 3D Pathfinding with routing.pathfinding
==================================================

The ``routing.pathfinding`` module provides a standalone 3D grid pathfinding library.
You can use it independently of the full CAD routing pipeline — for any problem that
needs to find shortest paths through a 3D discrete space.

**When to use this module directly:**

- You already have a 3D occupancy grid (numpy array) describing your environment
- You do not need CAD integration, design rules, or pipe-type constraints
- You want to explore alternative pathfinding algorithms (Theta*, Dijkstra, IDA*)
- You need cross-grid pathfinding (multiple disconnected spaces linked by portals)

**When to use the full routing pipeline instead:**

- You start from a STEP/STP CAD file
- You have port connection points and pipe types to route
- You need engineering design rules (bend radius, slope, pooling)

See :doc:`basic_routing` for the full pipeline tutorial.

.. contents:: On this page
   :local:
   :depth: 1


Part 1 — Basic A* in a 3D Grid
-------------------------------

This example finds the shortest path through a 3×3×3 grid with an obstacle at the centre.


Step 1 — Import
~~~~~~~~~~~~~~~

.. code-block:: python

   import numpy as np

   from routing.pathfinding.core.diagonal_movement import DiagonalMovement
   from routing.pathfinding.core.grid import Grid
   from routing.pathfinding.finder.a_star import AStarFinder


Step 2 — Create a map matrix
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Define the environment as a 3D numpy array. The value at each cell controls traversability:

- ``0`` or negative: **obstacle** — the pathfinder cannot enter this cell
- Positive value: **walkable** — the cost to enter this cell (higher = more expensive)

.. code-block:: python

   # 3×3×3 grid, all cells walkable with cost = 1
   matrix = np.ones((3, 3, 3))

   # Place an obstacle at the center
   matrix[1, 1, 1] = 0

Using non-uniform weights lets you model "prefer this corridor":

.. code-block:: python

   matrix = np.ones((10, 10, 10))
   matrix[5, :, :] = 3.0   # passing through the x=5 plane costs 3× more


Step 3 — Create the Grid
~~~~~~~~~~~~~~~~~~~~~~~~~

.. code-block:: python

   grid = Grid(matrix=matrix)

The ``Grid`` object creates a node for every cell in the matrix and stores adjacency
information. Obstacle cells are marked as non-walkable.


Step 4 — Define start and end
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Access nodes by their (x, y, z) integer indices (0-based):

.. code-block:: python

   start = grid.node(0, 0, 0)   # corner (0, 0, 0)
   end   = grid.node(2, 2, 2)   # opposite corner (2, 2, 2)


Step 5 — Find the path
~~~~~~~~~~~~~~~~~~~~~~~

.. code-block:: python

   finder = AStarFinder(diagonal_movement=DiagonalMovement.always)
   path, runs = finder.find_path(start, end, grid)

``path`` is a list of :class:`Node` objects; ``runs`` is the number of A* iterations.


Step 6 — Read the result
~~~~~~~~~~~~~~~~~~~~~~~~~

.. code-block:: python

   # Convert to list of (x, y, z) coordinate tuples
   path = [p.identifier for p in path]

   print("iterations:", runs)
   print("path length:", len(path))
   print("path:", path)

Expected output (diagonal movement, path avoids center obstacle):

.. code-block:: text

   iterations: 4
   path length: 3
   path: [(0, 0, 0), (1, 1, 0), (2, 2, 2)]


Complete Example
~~~~~~~~~~~~~~~~

.. code-block:: python

   import numpy as np
   from routing.pathfinding.core.diagonal_movement import DiagonalMovement
   from routing.pathfinding.core.grid import Grid
   from routing.pathfinding.finder.a_star import AStarFinder

   matrix = np.ones((3, 3, 3))
   matrix[1, 1, 1] = 0           # obstacle at center

   grid  = Grid(matrix=matrix)
   start = grid.node(0, 0, 0)
   end   = grid.node(2, 2, 2)

   finder = AStarFinder(diagonal_movement=DiagonalMovement.always)
   path, runs = finder.find_path(start, end, grid)

   path = [p.identifier for p in path]
   print("operations:", runs, "path length:", len(path))
   print("path:", path)


DiagonalMovement options
~~~~~~~~~~~~~~~~~~~~~~~~

:class:`~routing.pathfinding.core.diagonal_movement.DiagonalMovement` controls which
directions the pathfinder is allowed to move in:

.. list-table::
   :header-rows: 1
   :widths: 40 60

   * - Value
     - Behaviour
   * - ``DiagonalMovement.never``
     - Only move along the 6 axis-aligned directions (±X, ±Y, ±Z). Produces grid-aligned
       staircase paths. Fastest computation.
   * - ``DiagonalMovement.always``
     - Move in all 26 directions (6 faces + 12 edges + 8 corners). Shortest Euclidean
       paths. May cut through corners of obstacles.
   * - ``DiagonalMovement.only_when_no_obstacle``
     - Move diagonally only if neither adjacent face cell is an obstacle. Safe paths
       without corner cutting.
   * - ``DiagonalMovement.if_at_most_one_obstacle``
     - Move diagonally if at most one of the adjacent face cells is an obstacle. Allows
       some corner-cutting through single-cell obstacles. This one is not recommended.


Part 2 — Connecting Multiple Grids
------------------------------------

``routing.pathfinding`` supports pathfinding across **multiple separate grids** linked
by explicit node connections. This is useful for:

- Multi-storey buildings connected by stairs or lifts
- Separate rooms connected by doorways or tunnels
- Any routing problem with disconnected sub-spaces

The key objects are:

- :class:`~routing.pathfinding.core.grid.Grid` with a ``grid_id`` parameter
- :class:`~routing.pathfinding.core.world.World` to manage multiple grids
- Node connections to link nodes across grids


Step 1 — Create two grids
~~~~~~~~~~~~~~~~~~~~~~~~~~

Assign each grid a unique ``grid_id``:

.. code-block:: python

   from routing.pathfinding.core.grid import Grid
   from routing.pathfinding.core.world import World
   from routing.pathfinding.finder.a_star import AStarFinder

   # Define two 3D spaces (3×3×3 each, center column is obstacle)
   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]]]

   grid0 = Grid(matrix=world0, grid_id=0)
   grid1 = Grid(matrix=world1, grid_id=1)


Step 2 — Connect nodes between grids
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Create explicit connections between one node in grid0 and one node in grid1.
These act as "portals" or "bridges":

.. code-block:: python

   # Bidirectional bridge: node (2,2,2) in grid0 ↔ node (2,2,2) in grid1
   grid0.node(2, 2, 2).connect(grid1.node(2, 2, 2))
   grid1.node(2, 2, 2).connect(grid0.node(2, 2, 2))

For a one-way connection (e.g. a one-way staircase), call ``.connect()`` in only one
direction.


Step 3 — Create a World
~~~~~~~~~~~~~~~~~~~~~~~~

Wrap both grids in a :class:`~routing.pathfinding.core.world.World` object so the finder
can navigate across them:

.. code-block:: python

   world = World({0: grid0, 1: grid1})


Step 4 — Find a cross-grid path
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Pass ``world`` (instead of a single grid) to ``find_path()``:

.. code-block:: python

   finder = AStarFinder()
   path, _ = finder.find_path(
       grid0.node(2, 0, 0),   # start in grid 0
       grid1.node(2, 0, 0),   # end in grid 1
       world,
   )


Step 5 — Separate the path by grid
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Each node in the path has a 4-element identifier ``(x, y, z, grid_id)``:

.. code-block:: python

   # Convert to (x, y, z, grid_id) tuples
   path = [p.identifier for p in path]

   # Split by grid
   path_in_grid0 = [p[:3] for p in path if p[3] == 0]
   path_in_grid1 = [p[:3] for p in path if p[3] == 1]

   print("path through grid 0:", path_in_grid0)
   print("path through grid 1:", path_in_grid1)

Expected output:

.. code-block:: text

   path through grid 0: [(2, 0, 0), (2, 1, 0), (2, 2, 0), (2, 2, 1), (2, 2, 2)]
   path through grid 1: [(2, 2, 2), (2, 1, 2), (2, 0, 2), (2, 0, 1), (2, 0, 0)]

The path enters grid1 at the bridge node (2,2,2) and arrives at (2,0,0) in grid1.


Complete Multi-Grid Example
~~~~~~~~~~~~~~~~~~~~~~~~~~~~

.. code-block:: python

   from routing.pathfinding.core.grid import Grid
   from routing.pathfinding.core.world import World
   from routing.pathfinding.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]]]

   grid0 = Grid(matrix=world0, grid_id=0)
   grid1 = Grid(matrix=world1, grid_id=1)

   # Bidirectional bridge between (2,2,2) in each grid
   grid0.node(2, 2, 2).connect(grid1.node(2, 2, 2))
   grid1.node(2, 2, 2).connect(grid0.node(2, 2, 2))

   world = World({0: grid0, 1: grid1})

   finder = AStarFinder()
   path, _ = finder.find_path(grid0.node(2, 0, 0), grid1.node(2, 0, 0), world)

   path = [p.identifier for p in path]
   path0 = [p[:3] for p in path if p[3] == 0]
   path1 = [p[:3] for p in path if p[3] == 1]
   print("path through grid 0:", path0)
   print("path through grid 1:", path1)


Available Finders
-----------------

``routing.pathfinding`` includes several pathfinding algorithms:

.. list-table::
   :header-rows: 1
   :widths: 35 65

   * - Import
     - Description
   * - ``from routing.pathfinding.finder.a_star import AStarFinder``
     - A* — fast, general-purpose, works well with any heuristic
   * - ``from routing.pathfinding.finder.theta_star import ThetaStarFinder``
     - Theta* — produces smoother, more direct paths by allowing any-angle movement.
       See :doc:`theta_star`.
   * - ``from routing.pathfinding.finder.dijkstra import DijkstraFinder``
     - Dijkstra — always finds the optimal path; slower than A* for large grids
   * - ``from routing.pathfinding.finder.bi_a_star import BiAStarFinder``
     - Bidirectional A* — searches from both ends simultaneously; faster on long paths
   * - ``from routing.pathfinding.finder.best_first import BestFirstFinder``
     - Best-first (greedy) — very fast but not guaranteed optimal
   * - ``from routing.pathfinding.finder.ida_star import IDAStarFinder``
     - IDA* — memory-efficient iterative deepening A*; slow on large grids
   * - ``from routing.pathfinding.finder.breadth_first import BreadthFirstFinder``
     - BFS — uniform-cost search (treats all cells as equal weight)
   * - ``from routing.pathfinding.finder.msp import MinimumSpanningTree``
     - Minimum spanning tree finder

**Which to choose:**

- For most use cases: **AStarFinder** with ``DiagonalMovement.always``
- For smooth, short paths: **ThetaStarFinder** (see :doc:`theta_star`)
- When optimality is required regardless of speed: **DijkstraFinder**
- For large maps where memory is constrained: **IDAStarFinder**


Loading Large Maps
------------------

For large environments, load a pre-computed numpy array from a file:

.. code-block:: python

   import numpy as np
   from routing.pathfinding.core.grid import Grid
   from routing.pathfinding.finder.a_star import AStarFinder
   from routing.pathfinding.core.diagonal_movement import DiagonalMovement

   # Load a saved map (0 = obstacle, positive = walkable with that cost)
   matrix = np.load("my_environment.npy")

   grid  = Grid(matrix=matrix)
   start = grid.node(0, 0, 0)
   end   = grid.node(matrix.shape[0]-1, matrix.shape[1]-1, matrix.shape[2]-1)

   finder = AStarFinder(diagonal_movement=DiagonalMovement.only_when_no_obstacle)
   path, runs = finder.find_path(start, end, grid)
   path = [p.identifier for p in path]
   print(f"Found path of length {len(path)} in {runs} iterations")

See ``scripts/pathfinding/examples/03_view_map.py`` for an example with Open3D
visualization.


Next Steps
----------

- :doc:`theta_star` — produce smoother paths with Theta*
- :doc:`../user_guide/pathfinding_module` — full reference for the pathfinding module
- :doc:`basic_routing` — use the full CAD routing pipeline