Roadmaps II: Sensor-Based Roadmaps and Silhouettes

Where the planar GVD stops being a roadmap and what replaces it — the generalized Voronoi graph by intersecting equidistance sheets transversally, its disconnection and the hierarchical patch, a Lyapunov tracing law that lets a robot build the graph from range data alone, Canny's silhouette roadmap and the opportunistic path planner, and the price of exact planning.

The problem: a map nobody drew

Drop Rusty into the Apartment with no floor plan, only its range ring. Twenty seconds into the replay — under three minutes of simulated driving — a graph of the whole floor exists: every room, every doorway, the corridor, drawn by the robot driving along it.

Nothing in that animation knows where a wall is. Each step the robot reads its 360 rays, finds the local minima of the range — the distances $d_i$ to the nearby obstacles and, from their bearings, the gradients $\nabla d_i$ — and commands one velocity. The blue curve is Chapter 8's generalized Voronoi diagram; the robot does not compute it, it lives on it, because staying on it is a condition the sensor can check: two of the ranges are equal. The run ends when every curve leaving every meet point has been driven: $27$ meet points, $33$ boundary points where a curve runs into a corner, $62$ edges and $103.4$ m of roadmap after $205.0$ m of driving.

Two questions organize the rest of the chapter. Why does this work — what makes a velocity law converge onto a curve it cannot see? And what happens in three dimensions, where "two readings tie" is a surface and the robot would be wandering on sheets? The first answer is a Lyapunov function. The second is a story about transversality, disconnection, and slices — and it ends in a complexity bound that sends us to sampling.

Building intuition

Driving the graph

Watch the explorer in phases. Access: from wherever it starts, Rusty backs away from the nearest return — gradient ascent of $D$, exactly Chapter 8's Lemma 5.2.1 — until a second return is as short as the first. Trace: now it holds the difference of the two shortest returns at zero while moving perpendicular to the chord between their two closest points. The $\Gamma$ sparkline under the floor plan is the square of that difference: it spikes when the robot is pushed off the curve (the parabolic edges near door jambs curve faster than the law can follow at a gentle gain) and decays back. Home: when a third return closes in, a meet point is near; the robot switches law and converges on the point where all three tie. Branch: each meet point in the plane has three departing edges. The hollow node's number counts those not yet driven. Backtrack: when a node has none left, the robot drives along edges it already knows to the nearest node that still has one — depth-first search over a graph whose nodes are being discovered by the search itself. Terminate: when no node has an undriven edge, the graph is complete.

The faint blue curves underneath are Chapter 8's exact GVD of the same walls, traced from geometry. The sensor's graph lands on them: every meet point the robot homes onto is three-way equidistant by exact wall geometry to $10^{-6}$, and all twenty-six exact meet points are found within five centimetres. Two more — in the corridor at $(6.00, 4.30)$ and in the bedroom at $(7.85, 6.79)$ — are genuine meet points that Chapter 8's lattice-seeded tracer missed, which the same exact test confirms. The headline control is $\beta$, the stiffness with which the robot is pulled back onto the curve. The misconception this widget kills is the one in the chapter title: you do not need a map to build a roadmap. The roadmap is a sensor quantity.

From sheets to curves

In the plane one equation, $d_i = d_j$, leaves one free direction: a curve. In $\mathbb{R}^3$ it leaves two: a sheet. Imagine the Apartment extruded upward into a box-shaped room with a floor and a ceiling. The set of points equally far from the floor and the ceiling is the horizontal plane halfway up; equally far from the floor and the west wall is a tilted plane; and so on. These sheets form Chapter 8's GVD in three dimensions, and it is a perfectly good two-dimensional retract — but a planner searching a two-dimensional set has not gained much.

So intersect more. Points equidistant to the floor, the ceiling and the west wall lie on two sheets at once, and two planes in $\mathbb{R}^3$ meet in a line. That is a generalized Voronoi graph edge: think of a ball rolling along the room, touching the floor, the ceiling and one wall at once; its centre traces the edge. Where a fourth obstacle ties, four sheets meet at a point — a meet point. Two conditions must hold for the picture to be right: the sheets must actually cross rather than graze, and the resulting curves must connect. The first is transversality; the second, it turns out, fails.

A slice through a doughnut

Canny's idea comes at the same problem from the other side. Sweep a plane through the configuration space and look at what the plane cuts out: a few closed curves. On each curve mark the extreme points in some fixed direction. As the plane moves those marks move, and their traces are curves — silhouettes — that a planner can follow. On a torus lying flat, a plane sweeping across it first touches the outer rim, cuts one oval, and then, as it reaches the hole, the oval pinches and splits into two. That moment is a critical slice: the silhouette breaks there, and the algorithm repairs it by solving the same problem one dimension down, inside the critical slice. The Silhouette Sweep in the mathematics section plays this out.

The mathematics

The generalized Voronoi graph

Note what the definition does not say: $SS_{ijk}$ is not $SS_{ij} \cap SS_{ik}$. Transitivity of equality gives $d_j = d_k$ for free, but not $\nabla d_j \ne \nabla d_k$, and all three gradients must differ for the preimage theorem to apply. Choset's Table 5.2 compares the two structures in $\mathbb{R}^m$:

Two lines in the plane each have codimension one; meeting transversally, their intersection has codimension two — a point. Two planes in $\mathbb{R}^3$ meet transversally in a line, a plane and a line in a point; two lines in $\mathbb{R}^3$ can never meet transversally, because a generic nudge separates them. Two planes in $\mathbb{R}^4$, codimension two each, meet in a point. That arithmetic is the whole content of Table 5.2.

Why the GVG falls apart, and the hierarchical GVG

In the room the GVG is connected. Put a box in the middle of the room — Choset's Fig. 5.19 — and it is not. The outer network is unchanged; but around the box there is a second component, a halo: the loop of points equally far from the floor, the ceiling and the box. No edge of the outer network touches it.

Tracing without a predictor: the Lyapunov law

Chapter 3 traced curves with a discrete predictor–corrector: step along the tangent, then Newton back onto the curve. Choset §5.3.3 replaces both by one velocity,

with $G = d_1 - d_2$ in the plane and $G = [d_1 - d_2,\ d_1 - d_3]^\top$ in $\mathbb{R}^3$ (eq. 5.5). On the GVG $G = 0$ and the law reduces to $\alpha v$: move along the tangent. Off it, the second term pulls back — Choset calls $\alpha$ and $\beta$ "spring constants": $\alpha$ is the speed along the edge, $\beta$ the stiffness of the return. Both terms need only $G$ and $DG$, and $DG$'s rows are differences of distance gradients — unit vectors pointing back along the shortest rays. That is the epigraph: the planner can compute $\nabla G$ solely from range sensor information.

The funnel is the chapter's micro-world: two point obstacles at $(0, 0)$ and $(4, 0)$, whose GVG is the bisector $x = 2$, and Rusty at $(2.5, 1)$. The shading is $\Gamma = \tfrac12 G^2$ — a trough along the bisector — and the robot slides down its wall while advancing along its floor. The sparkline plots $\Gamma(t)$ on the curve $\Gamma_0 e^{2\beta t}$, which the derivation below says it must follow; slide $\beta$ above zero and the robot climbs out instead. The misconception this widget kills: the controller needs the GVG's equation. It needs $G$ and $DG$ at the point where the robot is, and nothing else.

The chapter's micro-example, by hand

Take the funnel's opening position: sites $c_1 = (0, 0)$, $c_2 = (4, 0)$, Rusty at $q = (2.5, 1)$, gains $\alpha = 1$, $\beta = -1$.

The velocity points left, toward $x = 2$, while advancing along the edge. Look at the correction's $x$-component: exactly $-0.5$, minus Rusty's offset from the bisector. For two point sites the Newton-like step $(DG)^\dagger G$, projected on the line through the sites, is exactly the signed distance to the bisector — not approximately, everywhere — and Exercise 1 asks why. The check pins all of these numbers to $10^{-4}$ and the identity to $10^{-12}$ at twenty seeded points around twenty seeded pairs of sites. Integrating the law with explicit Euler steps of $0.01$ s, $\Gamma$ never increases, and at $t = 2$ it is within $3\,\%$ of $e^{-4}\Gamma_0$, the continuous-time prediction; after $200$ steps of $0.05$ s the robot is on $x = 2$ to $10^{-3}$, having driven up the edge. With $\beta = +1$, $\Gamma$ more than triples in $0.6$ s.

Meet-point homing

Beyond a point robot: the rod

The rod-GVG edges play the role of meet points, but each is a whole curve of orientations: a small rod can spin a full turn while staying three-way equidistant, so $RF_{ijk}$ is homeomorphic to $S^1$ (Choset's footnote 5). The R-edges inherit adjacency from the plane. The chapter keeps the rod to a definition and a check — Exercise 6 builds it — and the check confirms the first claim: a $0.3$ m rod centred near the Workbench's widest meet point (clearance $1.539$) stays three-way equidistant through all $36$ orientations sampled, Newton in $(x, y)$ at fixed $\theta$ converging to $10^{-10}$, and $130$ R-edge placements are sampled from Chapter 8's point-GVD.

Silhouettes: Canny's roadmap

The sweep makes the definitions concrete on three bodies. The sphere has one silhouette, its equator — traced as two curves, the maxima and the minima of $q_2$ — born at $\lambda = -r$ and dying at $\lambda = r$. The torus lying flat ($R = 2$, $r = 0.7$) is Choset's Figs. 5.31–5.32: the slice is one closed curve for $|\lambda| > R - r$ and two for $|\lambda| < R - r$, so the critical values are $\pm 2.7$ (birth and death) and $\pm 1.3$ (pinch). Its silhouette is four arcs — the $q_2 > 0$ and $q_2 < 0$ halves of the outer and inner equators — and they are not connected to one another: four components. The linking curves on the two pinch slices join them into one. The ellipsoid with a bent hole is Fig. 5.25 simplified: an ellipsoid with semi-axes $3$, $2$ and $1.5$ and a hole of radius $0.7$ drilled along $q_3$ at $q_1 = 0.6$, its axis bending as $y = 0.5 z^2$. The bend matters. With a straight hole the wall is a cylinder, $q_2$ is constant along its vertical lines, and the extrema are degenerate — not Morse. Bent, each slice's wall curve is a parabola with one nondegenerate extremum. Choset describes the silhouette as the ellipsoid's equator, the perimeter of the hole and the two curves along its side; the sweep finds it in six pieces, and the linking curves join them into one. That is the misconception the widget kills: the silhouette is not the outline of the body as you see it. It is the critical set of a projection, and it lives where the surface's tangent plane contains the $q_3$ direction.

Choset's ellipsoid has four critical points, $Cp_1$ to $Cp_4$, because his hole goes down and comes back up, punching through the slice twice on the way in and twice on the way out. Ours goes straight through, so it has two; the honest bookkeeping above is what the sweep finds, not what the figure shows.

The opportunistic path planner and the generalized gradient

Canny's silhouettes extremize a coordinate. Canny and Lin's opportunistic path planner (OPP) extremizes something a robot cares about: the distance to the nearest obstacle, restricted to the slice.

What exactness costs

Canny's algorithm was the first general roadmap algorithm, and with it came the complexity bound that made roadmap theory a theory: for a configuration space of dimension $n$ whose obstacles are bounded by $p$ polynomials of degree at most $w$, any path-planning query can be answered in

time [Choset refs. 90–92]. That is singly exponential in $n$, where the earlier general method of Schwartz and Sharir — cylindrical algebraic decomposition — was doubly exponential. It is still enormous. Taking the constant in the $O(\cdot)$ as one, $p = 10$ and $w = 2$, the base-ten logarithm of the bound is $7.2$ for $n = 2$, $27.7$ for $n = 3$, $81.4$ for $n = 4$ and $396.5$ for $n = 6$ — a six-joint arm among quadric obstacles: more operations than there are atoms in the observable universe, by over three hundred orders of magnitude. And no algorithm can do fundamentally better in general: Reif showed that the generalized mover's problem — many independently moving parts — is PSPACE-hard (Appendix D). Exact, complete planning in high dimensions is not going to get cheap.

Canny's roadmap is complete, and it has never been a practical planner; the opportunistic planner, which is, gives up nothing of the theory but runs on a sweep that still scales with the volume of the space. The honest reading is that this chapter's exact structures are tools for low dimensions and for understanding: the GVG for a mobile robot that must explore, silhouettes as the reason sweep-based coverage works (Chapter 10), critical points as the place where everything interesting happens. For a six-joint arm, Part III trades completeness for probabilistic completeness and the problem becomes tractable.

The algorithm

Implementation in Rust

This chapter adds the gvg submodule to Chapter 8's roadmap crate: scan.rs turns a range ring into the $k$ nearest obstacles, tracer.rs is eq. (5.5), explore.rs the five-step exploration, hgvg.rs the period following, canny.rs the silhouette demonstrator and opp.rs the freeways. The tracer is written once for any number of readings; const generics carry the sizes, and the pseudoinverse returns None exactly where the Lyapunov argument stops holding.

use nalgebra::{SMatrix, SVector, Vector2};

/// The k nearest hits in a range ring: distance and unit direction away from each (C §4.3.1).
/// `obstacle_id` is `None` from a scan — the sensor cannot name what it sees, and the law never asks.
pub struct Nearest<const K: usize> {
    pub d: [f64; K],
    pub grad: [Vector2<f64>; K],
    pub obstacle_id: [Option<usize>; K],
}

pub struct Twist { pub v: Vector2<f64> }
pub struct GvgTracer { pub alpha: f64, pub beta: f64 }

/// DG† = DGᵀ (DG DGᵀ)⁻¹ for a full-row-rank differential. `None` where the Gram matrix is singular:
/// two sheet normals parallel (a non-transversal crossing) or, for the edge law, a meet point —
/// exactly where Lyapunov's neighborhood ends and homing has to take over.
pub fn pseudo_inverse<const R: usize, const C: usize>(dg: &SMatrix<f64, R, C>) -> Option<SMatrix<f64, C, R>> {
    let gram = dg * dg.transpose();
    gram.try_inverse().map(|inv| dg.transpose() * inv)
}

/// G and DG from the first R + 1 readings: rows d₁ − d_j and ∇d₁ − ∇d_j.
fn constraint<const K: usize, const R: usize>(n: &Nearest<K>) -> (SVector<f64, R>, SMatrix<f64, R, 2>) {
    let g = SVector::<f64, R>::from_fn(|j, _| n.d[0] - n.d[j + 1]);
    let dg = SMatrix::<f64, R, 2>::from_fn(|j, c| n.grad[0][c] - n.grad[j + 1][c]);
    (g, dg)
}

impl GvgTracer {
    /// Edge tracing, C eq. (5.5): q̇ = α v + β (DG)† G with v ∈ Null(DG), ‖v‖ = 1, signed to keep
    /// going the way we were going (Choset's footnote 4). Also returns Γ = ½ GᵀG; Γ̇ = β GᵀG.
    pub fn trace_step(&self, n: &Nearest<2>, heading: Vector2<f64>) -> Option<(Twist, f64)> {
        let (g, dg) = constraint::<2, 1>(n);
        let dg_pinv = pseudo_inverse(&dg)?;
        let row = dg.row(0);
        let mut v = Vector2::new(-row[1], row[0]).normalize();
        if v.dot(&heading) < 0.0 { v = -v; }
        let correction = self.beta * (dg_pinv * g);
        Some((Twist { v: self.alpha * v + correction }, 0.5 * g.norm_squared()))
    }

    /// Meet-point homing: DG is square, Null(DG) = {0}, so the law is a Newton-like step
    /// β DG⁻¹ G toward the centre of the circle through the three closest points.
    pub fn home_step(&self, n: &Nearest<3>) -> Option<(Twist, f64)> {
        let (g, dg) = constraint::<3, 2>(n);
        let dg_pinv = pseudo_inverse(&dg)?;
        Some((Twist { v: self.beta * (dg_pinv * g) }, 0.5 * g.norm_squared()))
    }

    /// Access (Ch. 8, Lemma 5.2.1): climb ∇d₁ until the second reading ties. Unit speed, so the
    /// ascent takes d₂ − d₁ seconds at most from any start in a bounded room.
    pub fn access_step(&self, n: &Nearest<2>) -> Twist {
        Twist { v: self.alpha * n.grad[0] }
    }
}

The tracer returns Option rather than a bare (Twist, f64) so that the one place the derivation fails — a singular $DG\, DG^\top$ — is a value the caller must handle, not a NaN that drives the robot into a wall. The explorer is a state machine around the tracer, and its graph is petgraph's.

use petgraph::graph::{NodeIndex, UnGraph};

pub struct MeetNode { pub q: Vector2<f64>, pub clearance: f64, pub departures: Vec<Vector2<f64>>, pub explored: Vec<bool> }
pub struct Edge { pub polyline: Vec<Vector2<f64>>, pub length: f64 }
/// The incrementally built topological map; `frontier` lists (node, slot) pairs not yet driven.
pub struct GvgGraph { pub graph: UnGraph<MeetNode, Edge>, pub frontier: Vec<(NodeIndex, usize)> }
pub enum Event { OnEdge, MeetPoint(NodeIndex), BoundaryPoint, Done }
enum Phase { Access, Depart, Trace, Home, Backtrack(Vec<Vector2<f64>>), Done }

pub struct Explorer<'r> {
    pub robot: &'r mut sim::Rusty,
    pub tracer: GvgTracer,
    pub graph: GvgGraph,
    phase: Phase, heading: Vector2<f64>, edge: Vec<Vector2<f64>>, from: NodeIndex,
    pub gamma_log: Vec<f64>,
}

impl Explorer<'_> {
    /// One tick: scan; access / trace / home by phase; DFS to the next unexplored slot.
    pub fn tick(&mut self, dt: f64) -> Event {
        let near: Nearest<3> = scan::nearest_from_scan(&self.robot.scan());
        match &self.phase {
            Phase::Access => {
                if near.d[1] - near.d[0] > ACCESS_TOL { self.drive(self.tracer.access_step(&near.first2()), dt); return Event::OnEdge; }
                self.from = self.add_node(self.robot.position(), access_departures(&near));
                self.phase = Phase::Depart;
                Event::OnEdge
            }
            Phase::Trace => {
                // "A sudden change in one of the two closest obstacles": a third reading closes in.
                if near.d[2] - near.d[0] < MEET_TOL { self.phase = Phase::Home; return Event::OnEdge; }
                if near.d[0] < BOUNDARY_TOL { return self.close_at_boundary(); }
                let Some((tw, gamma)) = self.tracer.trace_step(&near.first2(), self.heading) else {
                    self.phase = Phase::Home; // DG DGᵀ singular: we are at (or past) a meet point
                    return Event::OnEdge;
                };
                self.gamma_log.push(gamma);
                self.drive(tw, dt);
                self.edge.push(self.robot.position());
                Event::OnEdge
            }
            Phase::Home => match newton_home(&mut *self.robot, &self.tracer, HOME_TOL) {
                Some(q) => {
                    let node = self.node_near(q, NODE_TOL).unwrap_or_else(|| self.add_meet_node(q, &near));
                    self.close_edge(node);
                    self.next_from(node);
                    Event::MeetPoint(node)
                }
                None => { self.phase = Phase::Trace; Event::OnEdge } // spurious tie: carry on
            },
            Phase::Depart => self.depart(dt),
            Phase::Backtrack(_) => self.follow_backtrack(dt),
            Phase::Done => Event::Done,
        }
    }

    /// DFS bookkeeping: an unexplored slot here, or BFS over known edges to the nearest node with one.
    fn next_from(&mut self, n: NodeIndex) {
        self.phase = if self.graph.graph[n].explored.iter().any(|e| !e) { Phase::Depart }
            else { match self.path_to_frontier(n) { Some(p) => Phase::Backtrack(p), None => Phase::Done } };
    }
}

Canny's demonstrator works on implicit surfaces. Each slice is a planar curve-tracing problem, so Chapter 3's tracer does the work; the extrema are the sign changes of one partial derivative.

use nalgebra::{SVector, Vector2};

pub struct Silhouette {
    pub curves: Vec<Vec<SVector<f64, 3>>>,
    pub critical_points: Vec<SVector<f64, 3>>,
    pub linking: Vec<Vec<SVector<f64, 3>>>,
    pub depth: usize,
}
struct Slice { lambda: f64, pieces: Vec<Vec<Vector2<f64>>>, extrema: Vec<(SVector<f64, 3>, bool)> }

/// Sweep π₁ slices; π₂-extrema where D(f, π₁₂) = [∇f; e₁; e₂] loses rank, i.e. ∂f/∂q₃ = 0
/// (Lemma 5.5.2); recurse once on the critical slices, where the (curves, extrema) count changes.
pub fn canny(f: &dyn Fn(SVector<f64, 3>) -> f64, grad_f: &dyn Fn(SVector<f64, 3>) -> SVector<f64, 3>,
             bounds: [(f64, f64); 3], n_slices: usize) -> Silhouette {
    let lambdas: Vec<f64> = (0..=n_slices).map(|k| bounds[0].0 + (bounds[0].1 - bounds[0].0) * k as f64 / n_slices as f64).collect();
    let slices: Vec<Slice> = lambdas.iter().map(|&l| slice(f, grad_f, l, bounds)).collect();
    let mut sil = Silhouette { curves: chain_extrema(&slices), critical_points: vec![], linking: vec![], depth: 1 };
    for w in slices.windows(2) {
        if signature(&w[0]) == signature(&w[1]) { continue; }
        // Bisect on λ for the critical value; the slice there is pinched, so the linking curves
        // are taken just either side — the recursion's roadmap of a one-dimensional set is the set.
        let l_star = bisect_signature(f, grad_f, bounds, w[0].lambda, w[1].lambda, 8);
        let eps = 0.5 * (w[1].lambda - w[0].lambda);
        for l in [l_star - eps, l_star + eps] {
            sil.linking.extend(slice(f, grad_f, l, bounds).pieces.into_iter().map(|c| lift(l, &c)));
        }
        sil.critical_points.push(changed_extremum(&w[0], &w[1], l_star));
        sil.depth = 2;
    }
    sil
}

/// The curves of f(λ, ·) = 0 by Chapter 3's predictor–corrector, and the sign changes of ∂f/∂q₃
/// along each: there the tangent (−f₃, f₂) has no q₂-component, so q₂ is extremal.
fn slice(f: &dyn Fn(SVector<f64, 3>) -> f64, grad_f: &dyn Fn(SVector<f64, 3>) -> SVector<f64, 3>,
         lambda: f64, bounds: [(f64, f64); 3]) -> Slice {
    let g = |p: Vector2<f64>| f(SVector::<f64, 3>::new(lambda, p.x, p.y));
    let dg = |p: Vector2<f64>| { let n = grad_f(SVector::<f64, 3>::new(lambda, p.x, p.y)); Vector2::new(n[1], n[2]) };
    let pieces = trace_all_closed(&g, &dg, bounds[1], bounds[2], bugs::TraceParams { step: 0.03, ..Default::default() });
    let f3 = |p: &Vector2<f64>| grad_f(SVector::<f64, 3>::new(lambda, p.x, p.y))[2];
    let extrema = pieces.iter().flat_map(|c| sign_changes(c, f3).map(|(p, is_max)| (lift_point(lambda, p), is_max))).collect();
    Slice { lambda, pieces, extrema }
}

The opportunistic planner's one new idea fits in a function: a slice maximum of a nonsmooth minimum is a convex-hull test on first derivatives.

/// 0 ∈ ∂D̃(q*) = Co{∂d̃_i/∂y : i ∈ Z(q*)} on a vertical slice: the active slopes bracket zero.
/// For one active obstacle this is the smooth condition ∂d̃/∂y = 0; at a tie it is a kink maximum.
pub fn generalized_gradient_has_zero(slopes: &[f64], tol: f64) -> bool {
    let lo = slopes.iter().cloned().fold(f64::INFINITY, f64::min);
    let hi = slopes.iter().cloned().fold(f64::NEG_INFINITY, f64::max);
    lo <= tol && hi >= -tol
}

pub struct FreewayPoint { pub q: Point2<f64>, pub d: f64, pub active: Vec<usize>, pub slopes: Vec<f64>, pub channel: usize }

/// Strict local maxima of D̃(·; λ) on each channel of the slice x = λ, golden-section refined.
pub fn slice_maxima(world: &OppWorld, lambda: f64, samples: usize) -> Vec<FreewayPoint> {
    channels(world, lambda).iter().enumerate().flat_map(|(ci, &(lo, hi))| {
        let d = |y: f64| world.nearest(Point2::new(lambda, y)).d;
        strict_maxima(d, lo, hi, samples).into_iter().map(move |y| world.freeway_point(lambda, y, ci))
    }).collect()
}

The worked example, and its printed output

use nalgebra::Vector2;
use roadmap::gvg::{GvgTracer, Nearest};

/// Two point sites; Nearest in *site* order (site 1 at the origin), as the text labels them.
fn nearest(q: Vector2<f64>, sites: &[Vector2<f64>; 2]) -> Nearest<2> {
    let d = sites.map(|c| (q - c).norm());
    let grad = sites.map(|c| (q - c).normalize());
    Nearest { d, grad, obstacle_id: [Some(0), Some(1)] }
}

fn main() {
    let sites = [Vector2::new(0.0, 0.0), Vector2::new(4.0, 0.0)];
    let tracer = GvgTracer { alpha: 1.0, beta: -1.0 };
    let q = Vector2::new(2.5, 1.0);
    let n = nearest(q, &sites);
    let g = n.d[0] - n.d[1];
    let dg = n.grad[0] - n.grad[1];
    let dg_pinv = dg / dg.norm_squared();
    let v = Vector2::new(-dg.y, dg.x).normalize();
    let (tw, gamma) = tracer.trace_step(&n, Vector2::new(0.0, 1.0)).unwrap();
    println!("G = {g:.4}  DG = ({:.4}, {:.4})  DG+ = ({:.4}, {:.4})  v = ({:.4}, {:.4})", dg.x, dg.y, dg_pinv.x, dg_pinv.y, v.x, v.y);
    println!("correction = ({:.4}, {:.4})  qdot = ({:.4}, {:.4})  Gamma = {gamma:.4}  dGamma = {:.4}",
             tracer.beta * dg_pinv.x * g, tracer.beta * dg_pinv.y * g, tw.v.x, tw.v.y, tracer.beta * g * g);

    // The Lyapunov box: integrate, watch Γ. The edge x = 2 is vertical, so "keep going up" is the
    // heading for the whole run.
    let heading = Vector2::new(0.0, 1.0);
    let (mut q, mut prev) = (q, f64::INFINITY);
    for _ in 0..200 {
        let (tw, gamma) = tracer.trace_step(&nearest(q, &sites), heading).unwrap();
        assert!(gamma <= prev + 1e-12, "Γ must not increase");
        prev = gamma;
        q += tw.v * 0.05;
    }
    println!("200 steps of dt = 0.05: Gamma monotone, |x - 2| < 1e-3: {}", (q.x - 2.0).abs() < 1e-3);
}

G = 0.8898  DG = (1.7605, -0.1833)  DG+ = (0.5619, -0.0585)  v = (0.1036, 0.9946)
correction = (-0.5000, 0.0521)  qdot = (-0.3964, 1.0467)  Gamma = 0.3959  dGamma = -0.7918
200 steps of dt = 0.05: Gamma monotone, |x - 2| < 1e-3: true

lyapunov_step_matches_text pins every printed number to $10^{-4}$, the correction's $x$-component to $-0.5$ to $10^{-12}$, and the monotone descent of $\Gamma$. Two hundred steps of $0.01$ s would not land the robot: at $t = 2$ s the offset has shrunk only by $e^{-2}$, to about $0.07$. So the test integrates to $t = 10$ s, where $|x - 2| < 10^{-3}$ holds, and checks the $t = 2$ s value of $\Gamma$ against $e^{-4}\Gamma_0$ instead. The other two examples print the chapter's larger numbers: