Tutorial: 3D Pathfinding with routing.pathfinding#

The routing.pathfinding module is the pathfinding engine of routing. Its finders search a SmartGrid built from a plain weight array, so you can use them without the 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 or pipe types

  • You want to explore alternative pathfinding algorithms (Theta*, Dijkstra, best-first)

  • 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.core.grid import SmartGrid
from routing.pathfinding.core.diagonal_movement import DiagonalMovement
from routing.pathfinding.finder.a_star import AStarFinder

Step 2 — Create a Weight Array#

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 weight of the cell: entering it costs the length of the step times this weight (higher = more expensive)

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

# Place an obstacle at the centre
weights[1, 1, 1] = 0

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

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

Step 3 — Create the Grid#

grid = SmartGrid(weights, voxel_size=1.0, origin=(0.0, 0.0, 0.0))

The SmartGrid creates a node for every walkable cell and records, per cell, which of its neighbours are walkable. Obstacle cells have no node. voxel_size and origin — the centre of cell (0, 0, 0) — place the grid in space.

Step 4 — Define Start and End#

Access nodes by their (x, y, z) integer indices (0-based), and give the start the heading the path leaves it with:

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

The search requires that heading: in the routing pipeline it is the direction of the port.

Step 5 — Find the Path#

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

path is a list of SmartGridNode objects, from start to end; runs is the number of A* iterations.

Step 6 — Read the Result#

# Convert to a list of (x, y, z) index tuples
cells = [(node.x, node.y, node.z) for node in path]

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

# IMPORTANT: cleanup before searching the grid again
grid.cleanup()

Expected output (diagonal movement, path avoids the centre obstacle):

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

node.position gives the world coordinates of a cell centre instead of its indices.

Complete Example#

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

weights = np.ones((3, 3, 3))
weights[1, 1, 1] = 0           # obstacle at the centre

grid  = SmartGrid(weights, voxel_size=1.0, origin=(0.0, 0.0, 0.0))
start = grid.node(0, 0, 0)
end   = grid.node(2, 2, 2)
start.direction = (1, 0, 0)

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

cells = [(node.x, node.y, node.z) for node in path]
print("iterations:", runs, "path length:", len(cells))
print("path:", cells)
grid.cleanup()

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 every cell the move passes by is walkable (for a corner move, the three face cells as well). Safe paths without corner cutting.

DiagonalMovement.if_at_most_one_obstacle

Move diagonally if at most one of the cells the move passes by 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:

  • SmartGrid with a grid_id parameter

  • SmartWorld to manage multiple grids

  • Node connections to link nodes across grids

Step 1 — Create Two Grids#

Assign each grid a unique grid_id, and place it in space: here two 3×3×3 rooms, the second one right above the first.

import numpy as np

from routing.core.grid import SmartGrid
from routing.core.world import SmartWorld
from routing.pathfinding.finder.a_star import AStarFinder

# A 3×3×3 room with a pillar through its centre
room = np.ones((3, 3, 3))
room[1, 1, :] = 0

ground_floor = SmartGrid(room, voxel_size=1.0, origin=(0.0, 0.0, 0.0), grid_id=0)
first_floor  = SmartGrid(room, voxel_size=1.0, origin=(0.0, 0.0, 3.0), grid_id=1)

Step 2 — Connect Nodes Between Grids#

Create explicit connections between one node of each grid. These act as “portals” or “bridges”:

# Bidirectional stair: node (2,2,2) of the ground floor ↔ node (2,2,0) of the first floor
stair_foot, stair_head = ground_floor.node(2, 2, 2), first_floor.node(2, 2, 0)
stair_foot.connect(stair_head)
stair_head.connect(stair_foot)

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

Step 3 — Create a SmartWorld#

Wrap both grids in a SmartWorld so the finder can navigate across them. The world takes the frame covering its grids, at the coarsest voxel size:

building = SmartWorld(
    grids={0: ground_floor, 1: first_floor},
    world_matrix_shape=(3, 3, 6),
    world_origin=(0.0, 0.0, 0.0),
    voxel_size=1.0,
)

Step 4 — Find a Cross-Grid Path#

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

start = ground_floor.node(2, 0, 0)   # start in grid 0
start.direction = (0, 1, 0)

finder = AStarFinder()
path, _ = finder.find_path(
    start,
    first_floor.node(2, 0, 2),       # end in grid 1
    building,
)

Step 5 — Separate the Path by Grid#

Each node knows its grid; its identifier is (x, y, z, grid_id):

path_in_grid0 = [(node.x, node.y, node.z) for node in path if node.grid_id == 0]
path_in_grid1 = [(node.x, node.y, node.z) for node in path if node.grid_id == 1]

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

building.cleanup()   # cleans every grid of the world

Expected output:

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

The path climbs the stair from (2,2,2) on the ground floor to (2,2,0) on the first floor, and arrives at (2,0,2) there.

Complete Multi-Grid Example#

import numpy as np

from routing.core.grid import SmartGrid
from routing.core.world import SmartWorld
from routing.pathfinding.finder.a_star import AStarFinder

room = np.ones((3, 3, 3))
room[1, 1, :] = 0

ground_floor = SmartGrid(room, voxel_size=1.0, origin=(0.0, 0.0, 0.0), grid_id=0)
first_floor  = SmartGrid(room, voxel_size=1.0, origin=(0.0, 0.0, 3.0), grid_id=1)

# Bidirectional stair between the two floors
stair_foot, stair_head = ground_floor.node(2, 2, 2), first_floor.node(2, 2, 0)
stair_foot.connect(stair_head)
stair_head.connect(stair_foot)

building = SmartWorld(
    grids={0: ground_floor, 1: first_floor},
    world_matrix_shape=(3, 3, 6),
    world_origin=(0.0, 0.0, 0.0),
    voxel_size=1.0,
)

start = ground_floor.node(2, 0, 0)
start.direction = (0, 1, 0)
path, _ = AStarFinder().find_path(start, first_floor.node(2, 0, 2), building)

path0 = [(node.x, node.y, node.z) for node in path if node.grid_id == 0]
path1 = [(node.x, node.y, node.z) for node in path if node.grid_id == 1]
print("path through grid 0:", path0)
print("path through grid 1:", path1)
building.cleanup()

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.best_first import BestFirstFinder

Best-first (greedy) — very fast but not guaranteed optimal

from routing.pathfinding.finder.breadth_first import BreadthFirstFinder

BFS — shortest path in length, treating all cells as equal weight

All of them run the same compiled search; they differ by their policy (heuristic, shortcuts, weights). IDA*, bidirectional A* and the minimum spanning tree of pathfinding3D are not part of the engine.

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

Loading Large Maps#

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

import numpy as np
from routing.core.grid import SmartGrid
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 weight)
weights = np.load("my_environment.npy")

grid  = SmartGrid(weights, voxel_size=1.0, origin=(0.0, 0.0, 0.0))
start = grid.node(0, 0, 0)
end   = grid.node(weights.shape[0] - 1, weights.shape[1] - 1, weights.shape[2] - 1)
start.direction = (1, 0, 0)

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

Next Steps#