Motion Planning: From Geometry to Probability

Configuration space, A* done properly, potential fields and the local minima that kill them, and the sampling-based planners that are only probably complete — with the guarantees stated precisely enough to be worth having.

A goal is not a plan

Put Rusty at one end of a room, put a goal at the other, and write the first controller anyone writes: drive toward the goal, and push away from whatever you are about to hit. It is two lines of code, it needs no map beyond a range sensor, and it is exactly right — until it is catastrophically wrong.

Watch the bead until it stops. Then read the readout: the status is not failed, it is stuck, and the gradient norm is essentially zero. Nothing has gone wrong with the code. The controller has arrived at a point where the pull of the goal and the push of the wall cancel exactly, and by its own definition of success — descend the potential — it has succeeded. It is standing in the middle of a cup, reporting that there is nowhere better to go, and it is telling the truth.

This is the failure mode that motivates every other algorithm in the chapter. A reactive controller knows the world only through its local gradient. Escaping the cup requires going up the potential — moving away from the goal for a while — and a rule that only ever descends can never decide to do that. To decide to do it, something has to have looked at the whole room first.

Now flip on wave-front mode. The shading changes, the identical greedy descent runs again, and the bead walks out of the cup and around the obstacle. Nothing about the controller changed; the field it descends did. That single toggle is the arc of this chapter compressed into one control, and by the end of the Foundation section you will know exactly why the second field cannot have a trap and the first one must.

Four planners, one query

Before any formalism, watch the four algorithms this chapter derives race on a single query: from room A in the south-west corner of the Apartment to the bedroom in the north-east, out through one doorway, down the corridor, and in through another. Every planner sees the same map, the same start, the same goal, and the same sample budget.

Four things are worth noticing, and each becomes a theorem later.

A* is exhaustive and it shows. The blue wash is every lattice cell A* has opened or closed — about eight hundred of them at the default resolution. It returns the best path the lattice contains, and it pays for that with memory proportional to the area it searched.

RRT answers first and answers badly. The tree lunges into open space, finds a path, and stops improving. Its answer is jagged, it usually goes through a doorway a person would not have chosen, and depending on the seed it costs 14–32% more than A*'s. That is not a bug; it is the contract. RRT promises feasibility, fast, and nothing else.

RRT* starts identical to RRT and then bends. Same samples, same seed, same first solution at the same iteration — and then the cost curve walks steadily downward as the rewiring ball shrinks. Optimality here is not a property of the answer at any moment. It is a limit.

PRM builds something none of the others build: a reusable structure. On this one query the roadmap is mostly wasted work. Ask a thousand queries in the same apartment and it is the only planner here that is not starting from scratch each time.

One number in the scoreboard should look wrong. RRT* ends up cheaper than A*'s "optimum" — around 14.0 m against 15.06 m. Both are correct. A* is optimal on the lattice, and the lattice is not the plane: an 8-connected grid can only travel in eight directions, so it pays an octile detour that a sampler placing nodes anywhere never pays. Keep that discrepancy in mind. It is the honest face of resolution completeness, and we make it precise below.

The mathematics

One notational warning, made once and precisely. Throughout this book $x_t$ is the state the filter believes in — a random variable with a distribution over it. In this chapter $q$ is a configuration the planner imagines — a deterministic point it is considering, which the robot may never occupy. For Rusty the two are numerically the same triple $(x, y, \theta)$, and it is tempting to conflate them. Do not. Chapter 22 plans over distributions rather than points, and the whole difficulty of that chapter is the difference between these two symbols.

The space the robot never visits

The robot has a shape. That single fact is what makes planning hard, because "does this path collide?" is a question about a swept area, not a point. The configuration-space construction makes the shape disappear.

Write $R(q) \subset \mathcal{W}$ for the set of workspace points the robot occupies at configuration $q$. Then the obstacle in the workspace induces an obstacle in the configuration space:

A path is a continuous map $\tau : [0,1] \to \mathcal{Q}_{\mathrm{free}}$ with $\tau(0) = q_{\mathrm{start}}$ and $\tau(1) = q_{\mathrm{goal}}$. A trajectory is a path parameterized by time, which is a strictly stronger object: it has velocities, and therefore dynamics. This chapter plans paths. Chapter 23 turns them into trajectories.

The payoff is immediate for the case Rusty is closest to.

Everything downstream inherits this reduction. When a widget in this chapter says "free", it is asking whether $D(q) > r$; when it draws the C-obstacle, it is drawing the inflated map. Toggle show the C-obstacle in the Planner Arena and watch the doorways narrow: the doorway that matters to the planner is not the one in the wall, it is the one in the inflated map.

What it means to have solved the problem

Planners come with wildly different promises, and the differences are not marketing. State them crisply, because the rest of the chapter is a tour of who satisfies which.

Note what each one does not say. Resolution completeness says nothing about a passage narrower than a cell — that passage does not exist as far as the planner is concerned, and refining the grid is the only cure. Probabilistic completeness says nothing about when: the failure probability decays, but as we will compute below, its constants can be so bad that the guarantee is worthless at any budget you would actually run. And asymptotic optimality is a statement about a limit, not about the answer on your screen.

Search: Dijkstra, A*, and what the heuristic actually buys

Discretize $\mathcal{Q}_{\mathrm{free}}$ into an 8-connected lattice of cells and you have a graph $G = (V, E)$ with non-negative edge costs — one cell width for an orthogonal move, $\sqrt{2}$ cell widths for a diagonal. Now planning is shortest path, a solved problem since 1959.

The only interesting question is how much of the graph you have to touch.

Setting $\varepsilon = 0$ deletes the heuristic and line 3 pops in order of $g$ alone: that is Dijkstra. Setting $\varepsilon = 1$ is A*. Setting $\varepsilon > 1$ is weighted A*, which is faster and no longer optimal — by a factor you can bound exactly.

The heuristic must satisfy one property to keep the answer honest.

On an 8-connected lattice with unit-square cells the natural choice is the octile distance, which is the exact cost-to-go in an empty grid:

It is admissible because obstacles can only make the true path longer, and consistent because it is itself a metric that lower-bounds every edge cost.

Weighted A* is bounded, not wild. With $h$ admissible and priority $f = g + \varepsilon h$ for $\varepsilon \ge 1$, repeat Steps 2–5. When the goal is popped, $g(g) \le f_\varepsilon(v_i) = g^*(v_i) + \varepsilon h(v_i) \le \varepsilon (g^*(v_i) + h^*(v_i)) = \varepsilon C^*$. So the returned path is never worse than $\varepsilon$ times optimal.

That bound is loose in practice, which is exactly why weighted A* is everywhere in real stacks. Here is the Apartment query, measured on the same lattice the arena uses (0.2 m cells, 0.25 m robot radius, start $(1,1)$, goal $(11,8)$):

Read three lessons off it. The heuristic buys expansions and nothing else: A* and Dijkstra return byte-identical paths, and A* touches 42% as many cells. Weighting further is startlingly cheap — at $\varepsilon = 1.5$ the search expands a third as many cells as A* and still happens to return the optimal path. And the theoretical bound is nowhere near tight: at $\varepsilon = 5$ the guarantee permits a five-fold blowup and the measured penalty is 3.3%.

The non-monotonicity between $\varepsilon = 1.5$ and $\varepsilon = 2$ is real and worth a moment. Weighted A* is not a smooth dial. Raising $\varepsilon$ changes which ties break which way, and a greedier search can wander into a dead-end room and pay to come back out. The only honest way to pick $\varepsilon$ is to measure it on your map.

Potential fields, and why they must die

The controller from the hook, written down properly. Khatib's 1986 formulation puts a quadratic well at the goal — conic far away, so a distant goal does not produce an absurd pull:

and a barrier at every obstacle that switches off entirely beyond a radius of influence $Q^*$:

Note $D(q)$: the repulsive term is a function of the distance field, so its gradient is just the direction away from the nearest obstacle, scaled. Chapter 19's ESDF is doing all the geometric work again.

Line 2 is the whole tragedy. The loop terminates when the gradient vanishes — and the gradient vanishes at the goal and at every other critical point of $U$. The algorithm cannot tell the two apart, because locally they are the same event.

The cure you have already built

There is a navigation function for an arbitrary floorplan, it is trivial to compute, and you have met it twice already. Run Dijkstra backwards from the goal over the whole lattice, with no early exit, and label every free cell with its cost-to-go.

Every free cell except the goal now has a neighbor with strictly smaller $V$ — that is not a heuristic claim, it is the construction: $V(c)$ was assigned by relaxing an edge from the predecessor on its shortest path, and that predecessor has $V$ smaller by the edge weight. So greedy descent on $V$ cannot get stuck, and the wave-front is a discrete navigation function. The self-check wavefront: every free cell has a strictly cheaper neighbour verifies this over all 1953 reachable cells of the Apartment.

Go back to the Potential Well and toggle wave-front mode with that in mind. The bead does not get smarter. The field it descends stopped being an invention and started being a computed answer.

One more identity, and it closes a loop from the previous chapter. Run the identical Dijkstra with every obstacle cell as a source instead of the goal, and you get the distance from each free cell to the nearest obstacle. Choset calls that the brushfire algorithm and pictures it as a wave washing out from the obstacles. It is the Euclidean distance transform of Chapter 19, computed by a different route — approximately, because the 8-connected lattice can only measure length in octile steps, and the octile metric overestimates Euclidean distance by up to $\cos 22.5^\circ + (\sqrt2 - 1)\sin 22.5^\circ = 1.0824$, i.e. 8.24%, attained at exactly $22.5^\circ$. The self-check pins both error sources: on a 0.1 m lattice, brushfire never under-estimates the true distance and never exceeds $1.0824\,D + 2\sqrt{2}\,\Delta x$.

Brushfire is the same algorithm as the wave-front is the same algorithm as Dijkstra. Only the source set changes. That is the kind of collapse worth remembering.

Sampling: build the graph out of luck

Grids die of dimension. A 3-D pose lattice at 5 cm and 5° is already 20 million cells; a 6-DOF arm is hopeless. The move that rescues planning is the same one Chapter 8 made for beliefs: stop trying to represent $\mathcal{Q}_{\mathrm{free}}$ and sample it.

The learning phase and the query phase are separate on purpose. That separation is PRM's reason to exist — and, on the single query in the arena, its handicap.

Now the guarantee. It is worth stating with its constants visible, because the constants are the part that bites.

Now do the arithmetic the theorem invites, on the widget's own geometry. At a corridor width of 0.65 m the free area is $\mu = 51.2\ \mathrm{m}^2$, Rusty's radius shrinks the clear width to 0.35 m so the best path has clearance $\delta = 0.175$ m, and the route is about 8 m long. That gives $a = 183$ and $b = 1.18 \times 10^{-4}$. At the widget's budget of $n = 180$ milestones, the bound evaluates to $179$ — a probability bound larger than one, which is to say: the theorem tells you nothing at all. To force the bound below 0.1 you would need about 64,000 samples. At a corridor width of 0.35 m, you would need about 3.9 million.

Meanwhile, measure it. At $n = 180$ the roadmap connects the two rooms in roughly one run in six — the widget's 30-trial sweep reports 13% — and at $n = 800$ it succeeds 90% of the time. So the experiment reaches 90% success at 800 samples and the theorem promises 90% at 64,000: the guarantee is right, and it is eighty times too slow to be useful. Shrink the corridor to 0.35 m and the experiment reports 0% at any budget you will wait for, which is the same fact seen from the other side.

The escape is not more samples, it is better-placed ones. Toggle bridge-test sampling and watch the success curve stop collapsing. The bridge test of Hsu et al. (2003) draws a point, draws a second point a Gaussian step away, and keeps the midpoint only when both endpoints are in collision and the midpoint is free. That configuration — free point straddled by two blocked ones — is a geometric signature of a narrow passage rather than a bet on its volume, which is why the green curve in the widget holds at 83% where the blue one has fallen to 13%. It is also why pure bridge sampling is useless on its own: it finds passages and nothing else, so the library mixes it 60/40 with uniform samples, exactly as Hsu et al. prescribe.

RRT: growing into the space instead of covering it

PRM builds a graph for all queries. RRT builds a tree for one, and it grows it by repeatedly throwing a dart and reaching toward wherever it lands.

Line 1 is why it works. Nearest-neighbor selection makes the probability of extending from a given vertex proportional to the volume of that vertex's Voronoi cell, so the tree is biased toward unexplored territory by construction — the "rapidly exploring" in the name is a Voronoi bias, not a metaphor. Add a small probability (5% in the arena) of setting $q_{\mathrm{rand}} = q_{\mathrm{goal}}$ and the tree also remembers what it is for.

Line 4 is why it fails. A vertex's parent is chosen once, at insertion, from whatever was nearest at the time — and it is never reconsidered. The tree commits to whichever edges were cheap to reach early, and those commitments become the skeleton of every path it will ever return.

RRT*: what optimality costs

The repair is two operations, inserted between lines 3 and 4, both confined to a ball of radius $r_n$ around the new node.

Line 5 is the step everyone forgets. Re-parenting a node changes the cost-to-come of everything below it, and a tree whose costs are stale still looks rewired while silently optimizing the wrong objective. Our implementation pushes the update down the subtree, and the self-check rrt*: cost-to-come is consistent through every edge after rewiring asserts $\mathrm{cost}(v) = \mathrm{cost}(\mathrm{parent}(v)) + \norm{v - \mathrm{parent}(v)}$ at every node of the tree, to machine precision, after a full run. It is the invariant most worth testing in this chapter, because it is the one whose violation is invisible.

Rusty is not a point

Every planner so far returned a polyline. Rusty cannot drive a polyline. A differential-drive robot can spin in place, so it can technically follow one — badly, stopping at every vertex — but a car-like robot cannot follow one at all, and even Rusty's velocity controller (Chapter 9) turns each corner into a slow, error-accumulating pivot.

The constraint is that a wheel cannot slide sideways. In the plane that is one linear equation on the velocity:

This is a Pfaffian constraint: linear in the velocities, and non-integrable — there is no function $F(x,y,\theta)$ whose level sets it defines. That non-integrability is what makes it nonholonomic, and it has a precise and initially surprising consequence: the constraint removes no configurations at all. A car can reach any pose in the plane; parallel parking is the constructive proof. What the constraint restricts is the set of paths, not the set of destinations.

So the local planner has to change. Instead of a straight line between two configurations, we need the shortest drivable curve — and for a car with a minimum turning radius, that curve has a closed form.

Dubins proved in 1957 that the optimum is always one of exactly six words built from three primitives — $L$ (left at full lock), $R$ (right at full lock), $S$ (straight). The reason is Pontryagin's maximum principle: the steering that minimizes time is bang-bang, so the wheel is always hard over or dead ahead, and enumerating the possible switch structures leaves only $CSC$ and $CCC$ patterns.

A worked example you can check by hand

Take the hardest query a car ever faces: turn around on the spot. Start at the origin facing east, finish at the origin facing west, with $\rho = 1$:

The "obvious" answer is turn, drive, turn. Evaluate $LSL$: the left-turn circle at the start is centered at $(0, 1)$ and the one at the goal at $(0, -1)$, so the straight run is the $2\rho = 2$ between their centers, and each turn is three-quarters of a circle. Segments $(3\pi/2,\; 2,\; 3\pi/2)$, total

$RSR$ ties it by reflection. $LSR$ and $RSL$ do not exist at all: an internal tangent needs the two circles at least $2\rho$ apart, and here the start's left circle and the goal's right circle are the same circle. Their discriminants come out negative, which is exactly how the closed form reports "no such word".

Now the three-arc answer. Three unit circles are involved: the right-turn circle at the start, at $C_1 = (0, -1)$; the right-turn circle at the goal, at $C_3 = (0, +1)$; and a middle circle $C_2$ that must be tangent to both, so $\norm{C_2 - C_1} = \norm{C_2 - C_3} = 2\rho = 2$. With $C_1$ and $C_3$ two apart, the solution is $C_2 = (\pm\sqrt{3},\, 0)$ — the three centers form an equilateral triangle of side $2\rho$. Take $C_2 = (\sqrt{3}, 0)$.

The tangent points are the midpoints of the triangle's sides: $(\tfrac{\sqrt3}{2}, -\tfrac12)$ and $(\tfrac{\sqrt3}{2}, +\tfrac12)$. Measured at $C_1$, the start $(0,0)$ sits at $90^\circ$ and the first tangent point at $30^\circ$, so the first right arc turns $60^\circ = \pi/3$. By symmetry the last arc is $\pi/3$. Measured at $C_2$, the two tangent points sit at $210^\circ$ and $150^\circ$, and the middle arc is a left turn, so it goes the long way round: $360^\circ - 60^\circ = 300^\circ = 5\pi/3$. Total:

The three-arc word wins by 36%. $LRL$ ties it by reflection. Check the heading bookkeeping: the net turn is $-60^\circ + 300^\circ - 60^\circ = 180^\circ$, as required. This is the fact the Dubins Dial exists to make visceral — bring the two poses together and the answer stops being turn–straight–turn.

#[test]
fn worked_example_ch20_dubins_u_turn() {
    use std::f64::consts::PI;
    let q0 = Isometry2::new(Vector2::zeros(), 0.0);
    let q1 = Isometry2::new(Vector2::zeros(), PI);

    let best = dubins_shortest_path(&q0, &q1, 1.0).expect("a U-turn is always feasible");

    // The CCC word wins, and its arcs are π/3, 5π/3, π/3 — the equilateral
    // triangle of turning-circle centers, worked by hand in the text.
    assert!(matches!(best.word, Word::RLR | Word::LRL));
    assert_relative_eq!(best.segments[0], PI / 3.0, epsilon = 1e-12);
    assert_relative_eq!(best.segments[1], 5.0 * PI / 3.0, epsilon = 1e-12);
    assert_relative_eq!(best.length, 7.0 * PI / 3.0, epsilon = 1e-12);

    // ...and the CSC alternative really is 56% longer, not merely "longer".
    let lsl = dubins_word(Word::LSL, &q0, &q1, 1.0).unwrap();
    assert_relative_eq!(lsl.length, 3.0 * PI + 2.0, epsilon = 1e-12);
    assert!(dubins_word(Word::LSR, &q0, &q1, 1.0).is_none()); // circles coincide
}

Allowing reverse gives the Reeds–Shepp family: 48 word classes, with up to two cusps where the car changes direction. It is never worse than Dubins and often much better at close range, which is what the widget's reverse toggle shows. Our library implements only the pure-reverse subset — plan forwards with both headings flipped, then drive the curve backwards — and says so, because a genuine Reeds–Shepp planner also allows switching direction mid-path, which the subset cannot express.

Hybrid A*, at recipe level

Dubins gives us a drivable edge. To get a drivable plan in a real map, the DARPA Urban Challenge produced a construction that is now the default in every parking-lot planner. It is a recipe rather than a theorem, and this chapter presents it as one.

Search an $(x, y, \theta)$ lattice with A*, but keep a continuous state in each cell instead of snapping to the cell center: expand by integrating a small set of motion primitives (full left, full right, straight — forwards and backwards) from the actual pose stored there. Keep only the cheapest continuous state per cell, which is what makes the search finite. Near the goal, attempt an analytic expansion: a single Dubins or Reeds–Shepp shot to the goal pose, accepted if it is collision-free. And drive it with a dual heuristic,

The first term is a precomputed Reeds–Shepp lookup table that knows about the turning radius but not the walls; the second is exactly the wave-front from earlier in this chapter, which knows about the walls but not the turning radius. Neither dominates, both are admissible, and their maximum is admissible too — so the dual heuristic is free accuracy. Dolgov, Thrun, Montemerlo and Diebel report roughly an order-of-magnitude reduction in expansions from that maximum alone.

The result is not optimal — the continuous-state-per-cell pruning discards states that could have been part of the optimum — but it is drivable, fast, and it is what is under the hood when a car parks itself.

Implementation in Rust

Three types carry the chapter: the configuration space, the local planner, and the tree. Everything else is a function of them.

use nalgebra::{Isometry2, Point2, Vector2};
use parry2d_f64::query::intersection_test;
use parry2d_f64::shape::{ConvexPolygon, Polyline};

use ch19_maps::Esdf; // Chapter 19's Euclidean distance transform
use sim::World;      // Chapter 4's Apartment

/// Configuration space of a planar robot over a known map.
///
/// The whole chapter rests on one reduction (derivation F.1): a disc of radius
/// `r` is free at `q` exactly when the distance to the nearest obstacle exceeds
/// `r`. So `is_free` never touches geometry — it reads the distance field that
/// Chapter 19 already built, at O(1).
pub struct CSpace2 {
    /// D(q). Also the clearance the potential field differentiates, and the
    /// cost term the integration lab weights paths by.
    esdf: Esdf,
    /// Circumscribed radius: the conservative inflation.
    radius: f64,
    /// Inscribed radius: the *optimistic* one. Between the two we must ask parry.
    inradius: f64,
    /// `None` means the robot really is a disc, and the ESDF test is exact.
    footprint: Option<ConvexPolygon>,
    walls: Polyline,
}

impl CSpace2 {
    #[inline]
    pub fn clearance(&self, p: &Point2<f64>) -> f64 {
        self.esdf.at(p.x, p.y)
    }

    /// Three-tier test. The cheap answers are conclusive far from walls in both
    /// directions; only the annulus between the inscribed and circumscribed
    /// radii costs a real collision query, and in a floorplan that is a few
    /// percent of configurations.
    pub fn is_free(&self, q: &Isometry2<f64>) -> bool {
        let p = Point2::from(q.translation.vector);
        let d = self.clearance(&p);
        match &self.footprint {
            None => d > self.radius,
            Some(poly) => {
                if d > self.radius {
                    return true; // circumscribed disc fits: certainly free
                }
                if d <= self.inradius {
                    return false; // inscribed disc does not: certainly blocked
                }
                !intersection_test(q, poly, &Isometry2::identity(), &self.walls)
                    .expect("convex polygon vs polyline is supported")
            }
        }
    }

    /// Swept check along a segment, by **sphere marching**.
    ///
    /// Standing at `p` with clearance `c`, every point within `c - r` of `p` is
    /// free, so we may advance by exactly that much and re-test. The distance
    /// field is 1-Lipschitz, which is what makes the skip sound; `min_step`
    /// keeps the loop terminating when the path grazes a wall. A 6 m edge
    /// across the corridor costs about a dozen lookups instead of 300 samples.
    pub fn edge_free(&self, a: &Point2<f64>, b: &Point2<f64>, min_step: f64) -> bool {
        let delta = b - a;
        let len = delta.norm();
        if len < 1e-9 {
            return self.clearance(a) > self.radius;
        }
        let u = delta / len;
        let mut s = 0.0;
        while s < len {
            let c = self.clearance(&(a + u * s)) - self.radius;
            if c <= 0.0 {
                return false;
            }
            s += c.max(min_step);
        }
        self.clearance(b) > self.radius
    }
}

/// The local planner, as a trait. Straight lines for a holonomic point; Dubins
/// or Reeds–Shepp for Rusty. Every sampling planner below is generic over it,
/// which is the entire difference between "plans a path" and "plans a path the
/// robot can drive".
pub trait Steer {
    /// A curve from `a` toward `b`, truncated at `max_len`, with its cost.
    fn steer(
        &self,
        a: &Isometry2<f64>,
        b: &Isometry2<f64>,
        max_len: f64,
    ) -> Option<(Vec<Isometry2<f64>>, f64)>;
}

pub struct StraightLine;
pub struct Dubins {
    pub rho: f64,
}
pub struct ReedsShepp {
    pub rho: f64,
}

The lattice search is deliberately not a petgraph graph. Materializing 2700 nodes and 20,000 edges to represent a grid whose neighbors are computable by adding $\pm 1$ to an index is a pessimization, and knowing when a graph library helps is part of the skill.

use std::cmp::Ordering;
use std::collections::BinaryHeap;

/// An open-list entry. `Ord` is deliberately reversed — `BinaryHeap` is a *max*
/// heap — and ties break on the cell index so a run is reproducible: two cells
/// with equal f must be expanded in a defined order, or the animated frontier
/// flickers between frames.
#[derive(PartialEq)]
struct Open {
    f: f64,
    cell: u32,
}

impl Eq for Open {}
impl Ord for Open {
    fn cmp(&self, other: &Self) -> Ordering {
        other
            .f
            .partial_cmp(&self.f)
            .unwrap_or(Ordering::Equal)
            .then_with(|| other.cell.cmp(&self.cell))
    }
}
impl PartialOrd for Open {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

/// Octile distance in metres: exact cost-to-go on an *empty* 8-connected grid,
/// hence admissible on any grid, hence A* stays optimal (derivation F.2).
#[inline]
fn octile(di: i32, dj: i32, cell: f64) -> f64 {
    let (a, b) = ((di.abs() as f64), (dj.abs() as f64));
    (a.max(b) + (std::f64::consts::SQRT_2 - 1.0) * a.min(b)) * cell
}

/// `a_star(G, s, g, h)` over an implicit 8-connected lattice.
///
/// `epsilon` is the heuristic weight: 0 is Dijkstra, 1 is A*, and ε > 1 is
/// weighted A*, whose path is guaranteed within a factor ε of optimal.
pub fn a_star_grid(
    grid: &Lattice,
    start: u32,
    goal: u32,
    epsilon: f64,
) -> Option<(Vec<u32>, f64)> {
    let n = (grid.nx * grid.ny) as usize;
    let mut g = vec![f64::INFINITY; n];
    let mut parent = vec![u32::MAX; n];
    let mut closed = vec![false; n];
    let mut heap = BinaryHeap::new();

    let (gi, gj) = grid.coords(goal);
    let h = |c: u32| {
        let (i, j) = grid.coords(c);
        octile(i - gi, j - gj, grid.cell)
    };

    g[start as usize] = 0.0;
    heap.push(Open { f: epsilon * h(start), cell: start });

    while let Some(Open { cell, .. }) = heap.pop() {
        if closed[cell as usize] {
            continue; // a stale entry: we already expanded this cell more cheaply
        }
        closed[cell as usize] = true;
        if cell == goal {
            return Some((trace(&parent, start, goal), g[goal as usize]));
        }

        let (i, j) = grid.coords(cell);
        for &(di, dj, w) in Lattice::NEIGHBORS {
            let (ni, nj) = (i + di, j + dj);
            let Some(nb) = grid.index(ni, nj).filter(|&k| grid.free(k)) else { continue };
            // No corner cutting: a diagonal move needs both orthogonal cells
            // free, or the disc clips a corner the lattice claims is fine.
            if di != 0 && dj != 0 && !(grid.free_at(ni, j) && grid.free_at(i, nj)) {
                continue;
            }
            let tentative = g[cell as usize] + w * grid.cell;
            if tentative < g[nb as usize] {
                g[nb as usize] = tentative;
                parent[nb as usize] = cell;
                heap.push(Open { f: tentative + epsilon * h(nb), cell: nb });
            }
        }
    }
    None
}

The roadmap, on the other hand, is a graph — an explicit one that outlives every query — and that is exactly the case petgraph is for.

use nalgebra::Point2;
use petgraph::algo::astar;
use petgraph::graph::{NodeIndex, UnGraph};
use rand::rngs::SmallRng;

use crate::cspace::CSpace2;

/// A probabilistic roadmap: milestones as nodes, verified local plans as edges.
///
/// `petgraph` owns the topology and the traversal; this type owns the geometry
/// and the collision checking. The split matters because the roadmap is built
/// once and queried forever — which is PRM's whole reason to exist, and the
/// reason it loses on the single query in w20.1.
pub struct Prm {
    graph: UnGraph<Point2<f64>, f64>,
    k: usize,
    max_edge: f64,
    /// Straight-line checks attempted. The honest measure of PRM's real cost:
    /// it is dominated by collision checking, not by graph work.
    pub local_plans: usize,
}

impl Prm {
    /// Draw one milestone and wire it to its k nearest neighbours.
    pub fn step(&mut self, cs: &CSpace2, rng: &mut SmallRng) -> Option<NodeIndex> {
        let q = cs.sample_free(rng)?;
        let id = self.graph.add_node(q);

        // A linear neighbour scan is O(n), so building the roadmap is O(n²).
        // At the scale this chapter runs it that is not what dominates the
        // profile — the ~14,000 swept collision checks are. Reach for a k-d
        // tree when a profiler says to, not when the asymptotics do.
        let mut cand: Vec<_> = self
            .graph
            .node_indices()
            .filter(|&j| j != id)
            .map(|j| (j, (self.graph[j] - q).norm()))
            .filter(|&(_, d)| d <= self.max_edge)
            .collect();
        cand.sort_by(|a, b| a.1.total_cmp(&b.1));

        for &(j, d) in cand.iter().take(self.k) {
            self.local_plans += 1;
            if cs.edge_free(&q, &self.graph[j], 0.02) {
                self.graph.add_edge(id, j, d);
            }
        }
        Some(id)
    }

    /// Attach start and goal, then let petgraph's A* answer the query. The
    /// attachment is temporary: the roadmap itself is never modified, so the
    /// next query pays only for its own two attachments.
    pub fn query(&self, cs: &CSpace2, s: Point2<f64>, g: Point2<f64>) -> Option<(Vec<Point2<f64>>, f64)> {
        let mut view = self.graph.clone();
        let (si, gi) = (self.attach(&mut view, cs, s), self.attach(&mut view, cs, g));
        let goal_pt = view[gi];
        let (cost, path) = astar(
            &view,
            si,
            |n| n == gi,
            |e| *e.weight(),
            // Euclidean distance to the goal: admissible, because no roadmap
            // edge is shorter than the straight line it was checked along.
            |n| (view[n] - goal_pt).norm(),
        )?;
        Some((path.into_iter().map(|n| view[n]).collect(), cost))
    }
}

RRT and RRT* are one type, because they differ by exactly the two operations of the algorithm box. Writing them separately would hide the chapter's point.

use nalgebra::{Isometry2, Point2};
use rand::rngs::SmallRng;
use rand::{Rng, SeedableRng};

use crate::cspace::{CSpace2, Steer};

pub struct Node {
    pub q: Isometry2<f64>,
    pub parent: Option<u32>,
    /// Cost-to-come along tree edges. The quantity rewiring exists to lower.
    pub cost: f64,
}

pub struct RrtStar<S: Steer> {
    steer: S,
    nodes: Vec<Node>,
    children: Vec<Vec<u32>>,
    nn: KdTree2,
    /// γ in r_n = γ (log n / n)^{1/d}. Must exceed the Karaman–Frazzoli bound
    /// or asymptotic optimality is forfeit — so we check, loudly, in debug.
    gamma: f64,
    /// `false` gives plain RRT: no choose-parent, no rewire, no guarantee.
    rewire: bool,
    goal: Isometry2<f64>,
    best: Option<(u32, f64)>,
}

impl<S: Steer> RrtStar<S> {
    pub fn new(cs: &CSpace2, steer: S, start: Isometry2<f64>, goal: Isometry2<f64>, gamma: f64) -> Self {
        // 2 (1 + 1/d)^{1/d} (μ(Q_free)/ζ_d)^{1/d}, with d = 2 and ζ₂ = π.
        debug_assert!(
            gamma > 2.0 * 1.5_f64.sqrt() * (cs.free_measure() / std::f64::consts::PI).sqrt(),
            "γ below the Karaman–Frazzoli bound: this tree is not asymptotically optimal",
        );
        /* ... */
    }

    /// r_n = min(γ (log n / n)^{1/d}, r_max), with d = 2. The cap keeps the
    /// first few hundred iterations from rewiring the whole tree; with γ = 12.3
    /// and r_max = 2 m it is inactive by n ≈ 200, so it cannot touch the limit
    /// the theorem is about.
    fn connection_radius(&self) -> f64 {
        let n = self.nodes.len().max(2) as f64;
        (self.gamma * (n.ln() / n).sqrt()).min(self.max_radius)
    }

    pub fn step(&mut self, cs: &CSpace2, rng: &mut SmallRng) -> ExtendStatus {
        let q_rand = if rng.random_bool(self.goal_bias) {
            self.goal
        } else {
            cs.sample_uniform(rng)
        };

        let near_idx = self.nn.nearest(&q_rand);
        let Some((path, len)) = self.steer.steer(&self.nodes[near_idx].q, &q_rand, self.step_size)
        else {
            return ExtendStatus::Trapped;
        };
        let q_new = *path.last().unwrap();
        if !cs.path_free(&path) {
            return ExtendStatus::Trapped;
        }

        let (mut parent, mut cost) = (near_idx, self.nodes[near_idx].cost + len);

        if self.rewire {
            let r = self.connection_radius();
            let near = self.nn.within(&q_new, r);

            // choose parent: cheapest collision-free connection in the ball,
            // which need not be the nearest one.
            for &i in &near {
                if let Some((p, l)) = self.steer.steer(&self.nodes[i].q, &q_new, f64::INFINITY) {
                    let c = self.nodes[i].cost + l;
                    if c < cost && cs.path_free(&p) {
                        parent = i;
                        cost = c;
                    }
                }
            }
            let id = self.push(q_new, Some(parent), cost);

            // rewire: does anyone in the ball reach home more cheaply through
            // the newcomer?
            for &i in &near {
                if i == parent {
                    continue;
                }
                if let Some((p, l)) = self.steer.steer(&q_new, &self.nodes[i].q, f64::INFINITY) {
                    if cost + l < self.nodes[i].cost && cs.path_free(&p) {
                        self.reparent(i, id, cost + l);
                    }
                }
            }
            self.check_goal(id);
            return ExtendStatus::Advanced;
        }

        let id = self.push(q_new, Some(parent), cost);
        self.check_goal(id);
        ExtendStatus::Advanced
    }

    /// Hang `i` under `new_parent`, then push the saving down its subtree.
    ///
    /// Skipping the propagation is the classic RRT* bug: the tree *looks*
    /// rewired, but the costs it minimises are stale, so it quietly stops being
    /// asymptotically optimal while every visual check still passes.
    fn reparent(&mut self, i: u32, new_parent: u32, cost: f64) {
        if let Some(old) = self.nodes[i as usize].parent {
            self.children[old as usize].retain(|&c| c != i);
        }
        self.nodes[i as usize].parent = Some(new_parent);
        self.nodes[i as usize].cost = cost;
        self.children[new_parent as usize].push(i);

        let mut stack = vec![i];
        while let Some(p) = stack.pop() {
            for k in 0..self.children[p as usize].len() {
                let c = self.children[p as usize][k];
                let edge = self.edge_cost(p, c);
                self.nodes[c as usize].cost = self.nodes[p as usize].cost + edge;
                stack.push(c);
            }
        }
    }
}