Combinatorial Motion Planning

Chapter 1: Visibility Graphs

Imagine you are standing in a 2D room filled with polygonal obstacles. You want to walk from point A to point B along the shortest possible path. The key insight: the shortest path never curves in open space. It is a sequence of straight line segments. And each segment must either start, end, or bend at a vertex of an obstacle.

Why vertices? A shortest path that passes through the interior of an obstacle edge could always be shortcut. A path that bends in free space could always be straightened. The only places a shortest path is forced to change direction are the corners of obstacles — the vertices. This is LaValle’s Theorem 6.1, and it is the foundation of the visibility graph.

What does “visibility” mean precisely?

Two points p and q are visible to each other if the open line segment pq intersects no obstacle. We allow endpoints to touch obstacle boundaries (since start and goal may lie on edges), but the interior of the segment must be in C_free.

This condition has an important subtlety: what about line segments that run along an obstacle edge? LaValle handles this carefully — a segment that grazes the boundary of an obstacle (passes through the obstacle’s interior on at least one side) is not considered free. The segment must lie entirely in the closure of C_free.

Building the visibility graph

The visibility graph G = (V, E) has:

• Vertices V = {q_init, q_goal} ∪ {all obstacle vertices}
• Edges E = {(u,v) : u and v are mutually visible and u ≠ v}
• Edge weights = Euclidean distance between endpoints

After construction, run Dijkstra’s algorithm on G. The shortest path in G is the shortest path in 2D Euclidean space through C_free.

Construction cost: naïvely, for n total vertices (obstacle corners + start + goal), there are O(n²) candidate edges. Each edge requires checking visibility against all O(n) obstacle edges, giving O(n³) total. Rotational sweep algorithms (Lee and Preparata, 1979) reduce this to O(n² log n) by processing all vertices visible from a fixed source in a single angular sweep.

python
import numpy as np
from itertools import combinations

def segments_intersect(p1, p2, p3, p4):
    """Do open segments p1-p2 and p3-p4 intersect?

    Uses cross-product orientation test.
    Returns True if segments cross in their interiors.
    """
    def cross(o, a, b):
        return (a[0]-o[0])*(b[1]-o[1]) - (a[1]-o[1])*(b[0]-o[0])
    d1, d2 = cross(p3,p4,p1), cross(p3,p4,p2)
    d3, d4 = cross(p1,p2,p3), cross(p1,p2,p4)
    if ((d1>0 and d2<0) or (d1<0 and d2>0)) and \
       ((d3>0 and d4<0) or (d3<0 and d4>0)):
        return True
    return False

def is_visible(u, v, obstacle_edges):
    """True if segment u-v does not cross any obstacle edge."""
    for (p3, p4) in obstacle_edges:
        if segments_intersect(u, v, p3, p4):
            return False
    return True

def build_visibility_graph(vertices, obstacle_edges):
    """
    vertices: list of (x,y) — obstacle corners + start + goal
    obstacle_edges: list of ((x1,y1),(x2,y2)) pairs

    Returns adjacency dict with Euclidean distance weights.

    Worked example: 3 vertices forming a triangle obstacle
      vertices = [(0,0),(1,0),(0.5,1),(−1,0.5),(2,0.5)]
      (last two are start and goal)
    """
    graph = {v: {} for v in vertices}
    for u, v in combinations(vertices, 2):
        if is_visible(u, v, obstacle_edges):
            dist = np.linalg.norm(np.array(u) - np.array(v))
            graph[u][v] = dist
            graph[v][u] = dist
    return graph  # then run Dijkstra on graph

Chapter 2: Shortest Paths via Visibility

Building the visibility graph gives you a weighted undirected graph G. Now run Dijkstra’s algorithm on G. The path it returns is the globally shortest path through 2D Euclidean space. Not approximately shortest. Exactly shortest.

Let’s trace Dijkstra on a concrete example. Suppose there are two rectangular obstacles creating a narrow corridor. The visibility graph has 10 vertices (8 obstacle corners, start, goal). Dijkstra maintains a priority queue of (distance, vertex) pairs and a distance table initialized to ∞ except d(start) = 0.

Dijkstra on the visibility graph: step by step

Complexity analysis

Let n = number of obstacle vertices (plus start and goal). The visibility graph has O(n) vertices and O(n²) edges in the worst case (every pair of vertices is visible). Dijkstra with a binary heap runs in O((V + E) log V) = O(n² log n). Building the graph first costs O(n³) naïvely (O(n²) pairs × O(n) intersection tests each) or O(n² log n) with the Lee–Preparata rotational sweep.

So the overall algorithm runs in O(n² log n). For a robot navigating a building with 200 wall corners, n = 200, n² log n ≈ 200,000 × 7.6 ≈ 1.5 million operations. That’s microseconds on modern hardware.

The rotational sweep for visibility

The O(n² log n) algorithm for building the visibility graph uses a rotational sweep from each vertex. Fix a source vertex s. Sort all other vertices by their polar angle around s. Maintain a set of obstacle edges that intersect the current sweep ray, ordered by distance from s. As the ray sweeps past each vertex v:

• If v is an obstacle vertex, check if sv intersects the closest edge in the ray intersection set. If not, sv is a visibility edge.
• Insert or remove edges adjacent to v from the intersection set.

This processes all vertices visible from s in O(n log n) time. Running from all n sources gives O(n² log n) total.

python
import heapq

def dijkstra(graph, start, goal):
    """
    graph: dict of {vertex: {neighbor: distance}}
    Returns (shortest_distance, path_list).

    Worked example on visibility graph with 5 vertices:
      graph = {
        'S': {'A': 2.0, 'B': 3.5},
        'A': {'S': 2.0, 'B': 1.2, 'G': 4.1},
        'B': {'S': 3.5, 'A': 1.2, 'G': 2.8},
        'G': {'A': 4.1, 'B': 2.8}
      }
      dijkstra(graph, 'S', 'G')
      -> (5.5, ['S', 'B', 'G'])   # S->B = 3.5, B->G = 2.8
      # Not S->A->G = 6.1, not S->A->B->G = 7.7
    """
    dist  = {v: float('inf') for v in graph}
    prev  = {v: None for v in graph}
    dist[start] = 0.0
    pq = [(0.0, start)]

    while pq:
        d, u = heapq.heappop(pq)
        if d > dist[u]: continue   # stale entry
        if u == goal: break
        for v, w in graph[u].items():
            nd = d + w
            if nd < dist[v]:
                dist[v] = nd
                prev[v] = u
                heapq.heappush(pq, (nd, v))

    # Backtrack path
    path, node = [], goal
    while node is not None:
        path.append(node)
        node = prev[node]
    return dist[goal], path[::-1]

Chapter 3: Vertical Cell Decomposition

The visibility graph finds the shortest path. But sometimes you just need any path — and you need an approach that generalizes beyond Euclidean distance. Enter cell decomposition: carve C_free into simple pieces, build a graph of adjacencies, search the graph.

The simplest and most beautiful cell decomposition is the vertical decomposition (LaValle §6.2.2, Figure 6.7). Imagine sweeping a vertical line from left to right across the 2D workspace. Every time the sweep line hits an obstacle vertex, it triggers a vertical cut that splits C_free into a new piece.

What the sweep line produces

When the vertical line hits a vertex v of an obstacle, LaValle’s algorithm (Algorithm 6.1) shoots two rays from v: one straight up and one straight down, until each ray hits an obstacle boundary or the workspace boundary. This creates a vertical slice through C_free at the x-coordinate of v.

Between consecutive event x-coordinates (obstacle vertices sorted by x), each connected piece of C_free is a trapezoid (or triangle, or rectangle — all special cases of trapezoid). The left and right sides are vertical segments from the slice events; the top and bottom are portions of obstacle edges or workspace boundary.

The event queue

The sweep line algorithm maintains two data structures:

When the sweep line reaches a vertex v at position (x_v, y_v), the algorithm classifies v by its connectivity: is it a left endpoint, right endpoint, or interior point of the obstacles incident to it? Each case triggers different trapezoid creation and termination events.

Three types of events

Chapter 4: Trapezoidal Decomposition

After the sweep line completes, C_free has been partitioned into trapezoids. Now the key question: how do you turn this geometric partition into a path?

The answer is the adjacency graph (LaValle Definition 6.2): two trapezoids are adjacent if they share a vertical edge segment. Build a graph where each trapezoid is a node and each shared vertical wall is an edge. Then finding a path from q_init to q_goal in C_free is equivalent to finding a path in this finite graph — any graph search (BFS, DFS, Dijkstra) works.

From cell sequence to actual path

A BFS path gives you a sequence of adjacent trapezoids: T₁, T₂, ..., T_k. This tells you which regions to pass through, but not the actual waypoints. To construct a path:

This is not the shortest path (that requires the visibility graph). But it is a valid path guaranteed to exist whenever a path exists — completeness is free.

Handling multiple connected components

If C_free has multiple disconnected regions (some obstacles completely enclose subregions), the adjacency graph is disconnected. BFS from q_init’s trapezoid will never reach q_goal’s trapezoid, correctly reporting “no path exists.” This is the completeness guarantee in action: the algorithm never returns a false negative.

python
from collections import deque

def build_adjacency_graph(trapezoids):
    """
    trapezoids: list of Trapezoid objects.
    Each Trapezoid has:
      .left_x, .right_x      : x-extent
      .top_edge, .bot_edge   : y(x) functions for top/bottom
      .left_wall, .right_wall: (y_lo, y_hi) of vertical walls

    Returns: dict of {trap_id: [adjacent_trap_ids]}

    Two trapezoids are adjacent iff their shared vertical wall
    has non-zero length overlap.
    """
    adj = {i: [] for i in range(len(trapezoids))}
    for i, t1 in enumerate(trapezoids):
        for j, t2 in enumerate(trapezoids):
            if i >= j: continue
            # Adjacent iff one's right_x == other's left_x
            # AND their vertical walls overlap in y
            if abs(t1.right_x - t2.left_x) < 1e-9:
                overlap = min(t1.right_wall[1], t2.left_wall[1]) - \
                          max(t1.right_wall[0], t2.left_wall[0])
                if overlap > 1e-9:
                    adj[i].append(j); adj[j].append(i)
    return adj

def bfs_trapezoids(adj, start_trap, goal_trap):
    """BFS through adjacency graph. Returns list of trap ids or None."""
    queue = deque([[start_trap]])
    visited = {start_trap}
    while queue:
        path = queue.popleft()
        node = path[-1]
        if node == goal_trap: return path
        for nbr in adj[node]:
            if nbr not in visited:
                visited.add(nbr)
                queue.append(path + [nbr])
    return None  # no path exists

Chapter 5: General Cell Decompositions

Trapezoidal decomposition is one specific way to carve up C_free. But what if C_free isn’t described by polygonal obstacles? What if the boundary curves are smooth? What if you’re working in 3D or higher dimensions? The vertical sweep idea generalizes into a broader theory of cell decompositions that LaValle develops in §6.3.

A cell decomposition is a partition of C_free into a finite collection of open cells {C₁, C₂, ..., C_m} where each cell is homeomorphic to an open ball — no holes, no handles, topologically simple. Two cells are adjacent if their closures share a piece of boundary. The adjacency graph G = (V, E) has one node per cell and one edge per adjacency.

Exact vs. approximate decompositions

LaValle makes a critical distinction between two kinds of cell decompositions. In an exact decomposition, the cells perfectly cover C_free — no cell overlaps C_obs, and every point of C_free belongs to some cell. A path in the adjacency graph guarantees a collision-free path in C-space. Trapezoidal decomposition is exact.

In an approximate decomposition, cells are simpler (usually axis-aligned cubes) but they may overlap C_obs. Some cells are labeled “mixed” — partly free, partly occupied. These cells are either discarded (safe but incomplete) or subdivided recursively (adaptive resolution). This is the basis of octree and quadtree planners in robotics.

The connectivity graph encodes all topology

Once you have any valid cell decomposition, the adjacency graph captures everything that matters for path planning. The continuous geometry is abstracted away. You can add obstacles by marking cells as occupied; you can query connectivity via BFS in O(m) where m is the number of cells. If the decomposition has m = O(n^d) cells in d-dimensional space, BFS costs O(n^d) — exponential in dimension but guaranteed.

python
from collections import deque

class Cell:
    """A cell in the decomposition.

    status: 'free' | 'occupied' | 'mixed'
    neighbors: list of adjacent Cell objects
    """
    def __init__(self, bounds, status='free'):
        self.bounds = bounds      # (x_lo,x_hi,y_lo,y_hi,...)
        self.status = status
        self.neighbors = []
        self.center = [0.5*(b[0]+b[1]) for b in bounds]

def bfs_cells(start_cell, goal_cell):
    """BFS through cell adjacency graph.

    Only traverses 'free' cells.
    Returns list of cells from start to goal, or None.

    Worked example: 3-cell hallway:
      A(free) -- B(free) -- C(free)
      bfs_cells(A, C) -> [A, B, C]

    With obstacle:
      A(free) -- B(occupied) -- C(free)
      bfs_cells(A, C) -> None
    """
    queue = deque([[start_cell]])
    visited = {start_cell}
    while queue:
        path = queue.popleft()
        cell = path[-1]
        if cell is goal_cell: return path
        for nbr in cell.neighbors:
            if nbr not in visited and nbr.status == 'free':
                visited.add(nbr)
                queue.append(path + [nbr])
    return None

Chapter 6: Showcase — Cylindrical Algebraic Decomposition

So far we have decomposed 2D polygonal environments using geometric sweep lines. Now consider a harder problem: C-space obstacles defined by polynomial inequalities, not polygons. The robot configuration space might be described by equations like x² + y² ≤ 1 (a disk obstacle) or (x − 0.3)² + y² ≤ 0.04. How do you decompose C_free exactly when the boundaries are curves, not line segments?

The answer is Cylindrical Algebraic Decomposition (CAD), developed by Collins (1975) and central to LaValle’s §6.4. The idea: project the obstacles onto a lower-dimensional space, decompose that space, then “lift” each piece back up. In 2D, project onto the x-axis; in 3D, project onto the xy-plane; and so on recursively.

What “cylindrical” means

A decomposition is cylindrical if every pair of cells either is disjoint or one is a “cylinder” over the other — meaning the projection of the higher-dimensional cell onto the lower-dimensional base space maps entirely onto a single lower-dimensional cell. This nested structure means you can plan in the projected space and then lift back up safely.

For 2D C-space with polynomial obstacles, the CAD algorithm:

Showcase: Interactive CAD on Circular Obstacles

The simulation below applies the CAD idea to circular obstacles (defined by x² + y² = r²). Place obstacles by clicking the canvas; see the decomposition recomputed and a path found (or failure reported) in real time.

Chapter 7: Complexity of Motion Planning

Now for the bad news. We have been building increasingly powerful exact planners — visibility graphs for 2D polygons, trapezoidal decomposition, cylindrical algebraic decomposition. Each works beautifully in its domain. But how hard is motion planning in general?

In 1979, Reif proved that the general motion planning problem is PSPACE-hard (LaValle §6.5, citing Reif 1979). This is a fundamental lower bound: no algorithm can solve all motion planning instances efficiently unless PSPACE = P, which is widely believed to be false. The proof uses a reduction from the QSAT problem (quantified Boolean satisfiability), which is PSPACE-complete.

Complexity by problem type

The hardness depends critically on what kind of C-space and obstacles you are working with. LaValle §6.5 surveys the landscape:

The Reif reduction: why robots are hard

Reif’s 1979 proof constructs a physical system that simulates a QSAT formula. Each Boolean variable is a gear whose rotation corresponds to True or False. Each quantifier corresponds to a robot arm that must be moved in a specific order. Finding a path for all the arms simultaneously is equivalent to deciding QSAT — known to be PSPACE-complete. Therefore motion planning is PSPACE-hard.

The implication for practice: efficient complete algorithms exist for low-dimensional C-spaces with special structure (polygon, algebraic). For high-dimensional C-spaces (robot arms with 6+ DOF), sampling-based planners are the only practical option — they sacrifice completeness and optimality to remain computationally feasible.

Hardness even for simple-looking problems

Adding rotation to a 2D disk robot moving among 2D obstacles takes the problem from polynomial to the harder end of the spectrum. Adding one more rigid body (two coupled robots in 2D) makes it PSPACE-hard. The jump from “polynomial” to “exponential” can happen from a single change in the problem specification. This is why motion planning complexity is studied so carefully: small changes in the problem statement can radically change what’s computationally feasible.

Chapter 8: Exact vs. Sampling — When to Use Each

You now know both families: sampling-based planners (Chapter 5) and combinatorial planners (this chapter). They are not rivals — they are tools for different jobs. Choosing the wrong one is expensive. This chapter makes the trade-offs concrete.

The fundamental tension

Combinatorial planners give you guarantees: completeness, optimality (for visibility graphs), and exact answers. But they scale poorly. The visibility graph for n vertices costs O(n² log n); trapezoidal decomposition costs O(n log n); CAD costs doubly exponential in dimension. For a 6-DOF robot arm with smooth algebraic obstacles, the exact decomposition would have 10³⁰+ cells. Infeasible.

Sampling-based planners scale gracefully. RRT builds a tree of n nodes in O(n log n) with a k-d tree. PRM builds a roadmap in O(n log n). Neither has any dependence on the dimensionality of C-space in their per-iteration cost (only in the number of iterations needed for coverage). But they cannot guarantee optimality or, in finite time, completeness.

The narrow passage problem favors exact methods

Sampling-based planners struggle with narrow passages — regions of C_free that are necessary to pass through but have very small measure (volume). A random sample has probability proportional to volume of landing in the passage; narrow passages have near-zero volume. Exponentially many samples may be needed.

Exact planners don’t suffer this. The trapezoidal decomposition captures every passage exactly, no matter how narrow. If a passage is 10⊃−⊃¹⊃&sup0; wide, it still gets a cell in the decomposition. One BFS step traverses it. This is the major practical advantage of exact methods in low dimensions.

Structured comparison

Chapter 9: Connections & What Comes Next

Combinatorial planning is a complete, principled approach to motion planning that delivers exact answers. But it is also a lens: every sampling-based planner can be seen as an attempt to approximate some combinatorial structure without paying its full computational cost. PRM builds an approximate roadmap; RRT builds an approximate tree. The theoretical guarantees of combinatorial methods illuminate why sampling methods work and where they fail.

Where combinatorial ideas show up elsewhere

Limitations of combinatorial planning

• Dimension barrier: exact decompositions are doubly exponential in d. Beyond d ≈ 6, no exact method is computationally feasible.
• Requires algebraic description: C_obs must be described by polynomials. Sensor-based maps (point clouds, occupancy grids) don’t fit.
• Static world: if obstacles move, the decomposition must be rebuilt — O(n log n) or more per update.
• Assumes exact geometry: real robot geometry has manufacturing tolerances; exact planners give exact answers to slightly wrong problems.

Comparison with related chapters

Related micro-lessons

The ideas in this chapter connect to several other lessons in Parminces and the microLearning series:

• Ch 2: Discrete Planning — BFS, Dijkstra, and A* that power the adjacency graph search here
• Ch 4: Configuration Space — the C-space abstraction that makes combinatorial planning possible
• Ch 5: Sampling-Based Planning — the probabilistic counterpart; understand both to know when to use each

“If you can solve a problem with an exact algorithm, you should. But first make sure you actually have the problem the exact algorithm solves.”
— paraphrasing the practical wisdom of Steven LaValle, Planning Algorithms, 2006