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 Tutorial: Routing Pipes Through a CAD Assembly for the full pipeline tutorial.

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#

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)

# 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”:

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

Step 3 — Create the Grid#

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

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#

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

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

Step 6 — Read the result#

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

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

Complete Example#

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#

DiagonalMovement controls which directions the pathfinder is allowed to move in:

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:

  • Grid with a grid_id parameter

  • World to manage multiple grids

  • Node connections to link nodes across grids

Step 1 — Create two grids#

Assign each grid a unique grid_id:

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”:

# 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 World object so the finder can navigate across them:

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

Step 4 — Find a cross-grid path#

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

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

# 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:

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#

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:

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 Tutorial: Smoother Paths with Theta*.

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 Tutorial: Smoother Paths with Theta*)

  • 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:

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#