clearpath 0.1.0

Deterministic 2D obstacle-avoidance path planning and smooth cubic Bezier fitting
Documentation
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
//! The sweep that builds the vertical decomposition.

use alloc::vec::Vec;

use super::cell::{Cell, CellBuilder, CellId, Portal, Working};
use super::Decomposition;
use crate::error::PathPlanError;
use crate::geom::point::Point2D;
use crate::geom::polygon::{BoundingBox, Polygon, rectilinear_rectangles};
use crate::geom::predicates::{Orientation, orient2d};
use crate::geom::FreeSpace;

/// A boundary edge: one edge of an offset obstacle, or one of the workspace walls.
#[derive(Clone, Copy, Debug)]
pub(crate) struct Edge {
    a: Point2D,
    b: Point2D,
    /// Which obstacle this edge belongs to, or `None` for a workspace wall.
    obstacle: Option<u32>,
    /// Index of the edge within its obstacle, used to keep the sort stable and deterministic.
    index: u32,
}

impl Edge {
    /// The x range the edge covers. A vertical edge has an empty range and is active in no slab.
    #[inline]
    fn x_span(&self) -> (f64, f64) {
        let lo = self.a.x.min(self.b.x);
        let hi = self.a.x.max(self.b.x);
        (lo, hi)
    }

    /// The y of the edge at `x`, by exact linear interpolation.
    #[inline]
    fn y_at(&self, x: f64) -> f64 {
        let dx = self.b.x - self.a.x;
        if dx == 0.0 {
            return self.a.y;
        }
        self.a.y + (self.b.y - self.a.y) * ((x - self.a.x) / dx)
    }

    /// Whether the edge covers the whole open slab `(lo, hi)`.
    #[inline]
    fn covers(&self, lo: f64, hi: f64) -> bool {
        let (a, b) = self.x_span();
        a <= lo && hi <= b
    }
}

/// Builds the vertical decomposition of the free space, with `margin` applied.
///
/// `margin` is realised *before* the sweep, by insetting the forbidden geometry: the workspace is
/// eroded and every rectilinear obstacle is partitioned into rectangles and each rectangle is
/// inset. That is exact — see [`rectilinear_rectangles`] for why the inset of a union of
/// rectangles is the erosion of the union — and it leaves the sweep working on plain
/// axis-aligned boxes, which is both the cheapest and the most robust thing to decompose.
///
/// Eroding the *cells* instead, or offsetting the obstacle rings, is what this replaces. Both are
/// wrong: a cell's own faces do not account for obstacle features just beyond a portal, and
/// offsetting a ring's boundary needs trimming between offset features, without which a `margin`
/// large relative to a feature produces geometry that cuts through the obstacle it came from.
/// A `margin` against a non-rectilinear obstacle is therefore rejected rather than approximated;
/// see [`PathPlanError::MarginUnsupportedGeometry`].
pub fn decompose(space: &FreeSpace, margin: f64) -> Result<Decomposition, PathPlanError> {
    let mut out = Decomposition::empty();
    decompose_into(&mut out, space, margin, &[])?;
    Ok(out)
}

/// [`decompose`], with extra abscissae forced into the event set.
///
/// Giving the sweep the start and goal abscissae puts each of them on an event line, so the start
/// and goal always land in *different* slabs. That matters downstream: the funnel is only proven for
/// a corridor whose portals are ordered along the path, and a corridor can only be monotone in x if
/// its two ends are not in the same slab. Without this, any query whose endpoints share a slab —
/// which is most queries against a diagram — is forced onto the non-monotone fallback path.
pub fn decompose_with_guides(
    space: &FreeSpace,
    margin: f64,
    guides: &[f64],
) -> Result<Decomposition, PathPlanError> {
    let mut out = Decomposition::empty();
    decompose_into(&mut out, space, margin, guides)?;
    Ok(out)
}

/// Refills `out` with the vertical decomposition of the inflated free space.
///
/// Every buffer in `out` — and the transient sweep state it owns — keeps its capacity, so a warm
/// query allocates nothing here. That is the point of taking the decomposition by mutable
/// reference: it is the only part of a query that scales with the number of event abscissae, and
/// it was about half of a warm query's allocations.
///
/// On any error `out` is left cleared rather than half-built, so a caller that ignores the result
/// cannot read a stale partition.
pub fn decompose_into(
    out: &mut Decomposition,
    space: &FreeSpace,
    margin: f64,
    guides: &[f64],
) -> Result<(), PathPlanError> {
    out.clear();
    if !margin.is_finite() || margin < 0.0 {
        return Err(PathPlanError::InvalidConfig("margin must be finite and >= 0"));
    }
    if space.workspace.is_degenerate() {
        return Err(PathPlanError::DegenerateWorkspace);
    }
    let (workspace, forbidden) = apply_margin(space, margin)?;
    if workspace.is_degenerate() {
        // The margin is larger than the workspace on at least one axis, so there is no free space
        // left. An empty decomposition routes as `NoPathFound`, which is the honest answer.
        return Ok(());
    }

    out.working.reset();
    collect_edges(&mut out.working, &workspace, &forbidden);

    // The event abscissae: every boundary vertex, the workspace's own sides, and any guides.
    out.xs.reserve(out.working.edges.len() * 2 + guides.len() + 2);
    for e in &out.working.edges {
        out.xs.push(e.a.x);
        out.xs.push(e.b.x);
    }
    out.xs.push(workspace.min.x);
    out.xs.push(workspace.max.x);
    for g in guides {
        // Only strictly inside the eroded workspace: an event line on or outside the boundary would
        // create a slab lying entirely outside the free space.
        if g.is_finite() && *g > workspace.min.x && *g < workspace.max.x {
            out.xs.push(*g);
        }
    }
    sort_f64_ascending(&mut out.xs);
    out.xs.dedup();
    if out.xs.len() < 2 {
        return Ok(());
    }

    // The active edge list and the probe points for each slab, as flat runs in one buffer each
    // rather than a `Vec` per slab. A `Vec` of `Vec`s is one allocation per slab per list, and a
    // decomposition has one slab per distinct event abscissa.
    for k in 0..out.xs.len() - 1 {
        let (lo, hi) = (out.xs[k], out.xs[k + 1]);
        let mid = 0.5 * (lo + hi);
        let start = out.working.active_flat.len();
        out.working
            .active_flat
            .extend(out.working.edges.iter().copied().filter(|e| e.covers(lo, hi)));
        sort_by_y_at(&mut out.working.active_flat[start..], mid);
        out.working.active_offsets.push(out.working.active_flat.len() as u32);
        gap_probes_into(&out.working.active_flat[start..], mid, &mut out.working.probes);
        out.working.probe_offsets.push(out.working.probes.len() as u32);
    }

    // Run `k` of a flat buffer, where `offsets` holds one end offset per run and run 0 starts at 0.
    let run_of = |offsets: &[u32], k: usize| {
        let a = if k == 0 { 0 } else { offsets[k - 1] as usize };
        let b = if k < offsets.len() { offsets[k] as usize } else { usize::MAX };
        (a, b)
    };

    // Cells, grouped by slab in one flat buffer with an index of where each slab's run starts.
    // Same reason as the working lists: a `Vec` per slab was one allocation per slab.
    let mut builder = CellBuilder::default();
    for k in 0..out.xs.len() - 1 {
        let (pa, pb) = run_of(&out.working.probe_offsets, k);
        let (aa, ab) = run_of(&out.working.active_offsets, k);
        let slab_probes = &out.working.probes[pa..pb];
        let slab_edges = &out.working.active_flat[aa..ab];
        for (gap, probe) in slab_probes {
            if is_free(&forbidden, *probe) && workspace.contains(*probe) {
                let cell = cell_from_gap(slab_edges, *gap, out.xs[k], out.xs[k + 1]);
                out.slab_cells.push(builder.push_cell(cell));
            }
        }
        out.slab_cell_offsets.push(out.slab_cells.len() as u32);
    }

    // Portals: at every event line, join the cells of the two adjacent slabs over their vertical
    // overlap. Both lists are sorted and disjoint in y, so this is a linear merge.
    for (k, x) in out.xs.iter().enumerate().take(out.xs.len() - 1).skip(1) {
        let (la, lb) = run_of(&out.slab_cell_offsets, k - 1);
        let (ra, rb) = run_of(&out.slab_cell_offsets, k);
        connect_slabs(&mut builder, &out.slab_cells[la..lb], &out.slab_cells[ra..rb], *x);
    }

    // A portal whose overlap collapsed to zero length carries no information and would make the
    // funnel see a zero-width gate. Dropping it also drops the adjacency, which is what we want:
    // two cells that touch only at a point are not connected.
    out.working.remap.clear();
    out.working.remap.resize(builder.portals.len(), u32::MAX);
    out.portals.clear();
    for (old, portal) in builder.portals.iter().enumerate() {
        if portal.hi > portal.lo {
            out.working.remap[old] = out.portals.len() as u32;
            out.portals.push(*portal);
        }
    }

    // Compressed adjacency, built by two counting passes. A cell's side can overlap several cells on
    // the other side, so both sides are lists rather than single portals; and a `Vec<Vec<_>>` here
    // was one allocation per cell.
    let n = out.slab_cells.len();
    out.right_index.refill_from_portals(n, &out.portals, true, &mut out.working.cursor);
    out.left_index.refill_from_portals(n, &out.portals, false, &mut out.working.cursor);
    out.cells.append(&mut builder.cells);
    Ok(())
}

/// Every edge of every offset obstacle, plus the four workspace walls.
///
/// The walls are what keep the sweep inside the workspace: the bottom and top walls are always
/// active, so the gaps beyond them are never free, and the side walls close the x range.
fn collect_edges(working: &mut Working, ws: &BoundingBox, obstacles: &[Polygon]) {
    working.edges.clear();
    for (i, p) in obstacles.iter().enumerate() {
        let v = p.vertices();
        for k in 0..v.len() {
            working.edges.push(Edge {
                a: v[k],
                b: v[(k + 1) % v.len()],
                obstacle: Some(i as u32),
                index: k as u32,
            });
        }
    }
    let corners = ws.corners();
    for k in 0..4 {
        working.edges.push(Edge {
            a: corners[k],
            b: corners[(k + 1) % 4],
            obstacle: None,
            index: 4 + k as u32,
        });
    }
}

/// Ascending sort of `f64` as a total order on the bit patterns.
///
/// `f64::partial_cmp().unwrap()` panics on NaN, and comparing the numbers themselves is not a
/// total order on the bit patterns (`-0.0 == 0.0` but their bits differ), so neither can satisfy
/// the D2 determinism requirement of section 4.4.
///
/// The key is the standard IEEE-754 flip — negatives have every bit inverted, non-negatives get
/// the sign bit set — which orders *unsigned*, monotonically in the numeric value, with `-0.0`
/// before `+0.0` and NaN at the extremes. Comparing it as a signed integer would put every
/// non-negative value before every negative one, because the transform moves positives into the
/// upper half of the unsigned range.
pub(crate) fn sort_f64_ascending(values: &mut [f64]) {
    values.sort_by_key(|x| f64_sort_key(*x));
}

/// The total-order key for a `f64`. See [`sort_f64_ascending`].
#[inline]
pub(crate) fn f64_sort_key(x: f64) -> u64 {
    let bits = x.to_bits();
    if bits >> 63 == 0 { bits | (1u64 << 63) } else { !bits }
}

/// Sorts active edges bottom to top at abscissa `x`, with a deterministic tie-break.
///
/// The primary key is the exact orientation of the two edges at `x` — not a float comparison of
/// two separately-computed intercepts, which would disagree with the true order when the
/// intercepts are close.
fn sort_by_y_at(edges: &mut [Edge], x: f64) {
    edges.sort_by(|e1, e2| {
        // Normalise `e1` to run in the +x direction first. The sign of `orient2d` depends on the
        // direction of the directed edge, so comparing two edges whose stored endpoints happen to
        // run in opposite directions would report both as "less than" each other — a comparator
        // that is not antisymmetric, which `sort_by` is entitled to turn into an arbitrary order.
        let (p, q) = if e1.a.x <= e1.b.x { (e1.a, e1.b) } else { (e1.b, e1.a) };
        // `e2`'s intercept at `x` lies strictly above `e1` exactly when it is left of `p -> q`.
        let o = orient2d(p, q, Point2D::new(x, e2.y_at(x)));
        match o {
            Orientation::Clockwise => core::cmp::Ordering::Greater,
            Orientation::CounterClockwise => core::cmp::Ordering::Less,
            Orientation::Collinear => {
                // Collinear at this x. Break by the obstacles' identity so overlapping obstacles
                // keep a fixed relative order, then by edge index.
                (e1.obstacle, e1.index).cmp(&(e2.obstacle, e2.index))
            }
        }
    });
}

/// A probe point strictly inside each gap between consecutive active edges, plus one below the
/// lowest and one above the highest, each tagged with the gap index it belongs to.
///
/// The lowest active edge is always the workspace's bottom wall, so the below-gap probe is
/// outside the workspace and the above-gap probe is outside its top wall; both are classified by
/// the same free-space test and rejected. Carrying the gap index avoids recovering it by
/// re-scanning the active list, which cannot then disagree with the list it was built from.
fn gap_probes_into(active: &[Edge], x: f64, out: &mut Vec<(usize, Point2D)>) {
    let n = active.len();
    for i in 0..=n {
        let below = i.checked_sub(1).map_or(f64::NEG_INFINITY, |j| active[j].y_at(x));
        let above = active.get(i).map_or(f64::INFINITY, |e| e.y_at(x));
        let y = match (below.is_finite(), above.is_finite()) {
            (true, true) => 0.5 * (below + above),
            // Below the lowest edge: one unit under it. Above the highest: one unit over it.
            (false, true) => above - 1.0,
            (true, false) => below + 1.0,
            (false, false) => 0.0,
        };
        out.push((i, Point2D::new(x, y)));
    }
}

/// The workspace eroded by `margin` and the forbidden geometry inset by `margin`.
///
/// Exact for rectilinear obstacles. See [`decompose`] for why, and for the rejection of a
/// non-rectilinear obstacle under a nonzero `margin`.
fn apply_margin(space: &FreeSpace, margin: f64) -> Result<(BoundingBox, Vec<Polygon>), PathPlanError> {
    Ok((space.workspace.eroded(margin), forbidden_rectangles(&space.obstacles, margin)?))
}

/// The forbidden geometry of the inflated domain, as the rectangles that cover it.
///
/// Exposed because the rectilinear router (section 7.2) needs the same geometry and must not
/// re-derive the margin: two implementations of the margin is one more than there should be.
pub fn forbidden_rectangles(
    obstacles: &[Polygon],
    margin: f64,
) -> Result<Vec<Polygon>, PathPlanError> {
    if margin == 0.0 {
        return Ok(obstacles.to_vec());
    }
    let mut out: Vec<Polygon> = Vec::with_capacity(obstacles.len());
    for obstacle in obstacles {
        let parts = rectilinear_rectangles(obstacle.vertices())
            .ok_or(PathPlanError::MarginUnsupportedGeometry { margin })?;
        for part in parts {
            let grown = part.outset(margin);
            out.push(
                Polygon::new(grown.corners().to_vec())
                    .map_err(|_| PathPlanError::MarginUnsupportedGeometry { margin })?,
            );
        }
    }
    Ok(out)
}

/// Whether `p` is outside every forbidden polygon.
fn is_free(forbidden: &[Polygon], p: Point2D) -> bool {
    !forbidden.iter().any(|o| o.contains(p))
}

/// The cell for gap `i` of `active`: the region between `active[i-1]` and `active[i]` over
/// `[lo_x, hi_x]`. The gap above the last edge has a virtual lower boundary at `y = 0` and the
/// gap below the first has a virtual upper one; both are classified as outside the workspace and
/// rejected before reaching here, so the virtual values never surface.
fn cell_from_gap(active: &[Edge], i: usize, lo_x: f64, hi_x: f64) -> Cell {
    let lo_edge = i.checked_sub(1).and_then(|j| active.get(j)).copied();
    let hi_edge = active.get(i).copied();
    Cell {
        bl: Point2D::new(lo_x, lo_edge.map_or(0.0, |e| e.y_at(lo_x))),
        br: Point2D::new(hi_x, lo_edge.map_or(0.0, |e| e.y_at(hi_x))),
        tr: Point2D::new(hi_x, hi_edge.map_or(0.0, |e| e.y_at(hi_x))),
        tl: Point2D::new(lo_x, hi_edge.map_or(0.0, |e| e.y_at(lo_x))),
    }
}

/// Joins the cells of two adjacent slabs across the event line at `x`, creating a portal for every
/// pair with a positive-length vertical overlap.
///
/// One cell's side can overlap several on the other side, so this is a merge that can emit several
/// portals for the same cell; the adjacency is built from the portal list afterwards, so nothing is
/// recorded per cell here.
fn connect_slabs(builder: &mut CellBuilder, left: &[CellId], right: &[CellId], x: f64) {
    let (mut i, mut j) = (0usize, 0usize);
    while i < left.len() && j < right.len() {
        let l = builder.cells[left[i] as usize].y_range_at(x);
        let r = builder.cells[right[j] as usize].y_range_at(x);
        let lo = l.0.max(r.0);
        let hi = l.1.min(r.1);
        if hi > lo {
            builder.push_portal(Portal { x, lo, hi, left: left[i], right: right[j] });
        }
        // Advance whichever cell ends first; equal ends advance both.
        if l.1 < r.1 {
            i += 1;
        } else if r.1 < l.1 {
            j += 1;
        } else {
            i += 1;
            j += 1;
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::geom::FreeSpace;
    use alloc::vec;
    use alloc::vec::Vec;

    fn square(x0: f64, y0: f64, x1: f64, y1: f64) -> Polygon {
        Polygon::new(vec![
            Point2D::new(x0, y0),
            Point2D::new(x1, y0),
            Point2D::new(x1, y1),
            Point2D::new(x0, y1),
        ])
        .unwrap()
    }

    fn space(obstacles: Vec<Polygon>) -> FreeSpace {
        FreeSpace::new(
            BoundingBox::new(Point2D::ZERO, Point2D::new(100.0, 100.0)),
            obstacles,
        )
        .unwrap()
    }

    #[test]
    fn a_margin_erodes_the_workspace_walls() {
        let d = decompose(&space(vec![]), 5.0).unwrap();
        assert_eq!(d.locate(Point2D::new(1.0, 50.0)), None, "too close to the west wall");
        assert_eq!(d.locate(Point2D::new(99.0, 50.0)), None, "too close to the east wall");
        assert!(d.locate(Point2D::new(50.0, 50.0)).is_some());
    }

    #[test]
    fn a_margin_erodes_away_from_obstacles() {
        let d = decompose(&space(vec![square(40.0, 40.0, 60.0, 60.0)]), 5.0).unwrap();
        assert!(d.locate(Point2D::new(50.0, 38.0)).is_none(), "within margin of the box");
        assert!(d.locate(Point2D::new(50.0, 33.0)).is_some(), "clear of the box by the margin");
    }

    #[test]
    fn a_gap_narrower_than_twice_the_margin_closes() {
        // A full-height vertical barrier with a 10-unit gap in it.
        let barrier = || {
            space(vec![square(40.0, 0.0, 44.0, 45.0), square(40.0, 55.0, 44.0, 100.0)])
        };
        let open = decompose(&barrier(), 2.0).unwrap();
        assert!(open.locate(Point2D::new(42.0, 50.0)).is_some(), "a 6-unit passage should survive");
        let shut = decompose(&barrier(), 6.0).unwrap();
        assert!(shut.locate(Point2D::new(42.0, 50.0)).is_none(), "a 10-unit gap cannot pass");
        // The right-hand region still exists as free space; it is just unreachable.
        assert!(shut.locate(Point2D::new(80.0, 50.0)).is_some());
    }

    #[test]
    fn a_margin_larger_than_the_workspace_yields_no_cells() {
        let d = decompose(&space(vec![]), 60.0).unwrap();
        assert_eq!(d.cell_count(), 0);
        assert!(d.locate(Point2D::new(50.0, 50.0)).is_none());
    }

    #[test]
    fn a_margin_on_a_sloped_obstacle_is_rejected_not_approximated() {
        let triangle = Polygon::new(vec![
            Point2D::new(10.0, 10.0),
            Point2D::new(40.0, 10.0),
            Point2D::new(10.0, 40.0),
        ])
        .unwrap();
        // Exact at zero margin, refused above it.
        assert!(decompose(&space(vec![triangle.clone()]), 0.0).is_ok());
        assert_eq!(
            decompose(&space(vec![triangle]), 1.0).unwrap_err(),
            PathPlanError::MarginUnsupportedGeometry { margin: 1.0 }
        );
    }

    #[test]
    fn a_margin_on_an_l_shaped_obstacle_is_exact() {
        // An L is rectilinear, so a margin against it is supported, not refused.
        let l = Polygon::new(vec![
            Point2D::new(10.0, 10.0),
            Point2D::new(60.0, 10.0),
            Point2D::new(60.0, 30.0),
            Point2D::new(30.0, 30.0),
            Point2D::new(30.0, 60.0),
            Point2D::new(10.0, 60.0),
        ])
        .unwrap();
        let d = decompose(&space(vec![l]), 2.0).unwrap();
        // The arm's right face is x = 30 and the base's top face is y = 30, so a 2-unit margin
        // pushes the notch's free region out to x > 32 and y > 32.
        assert!(d.locate(Point2D::new(35.0, 35.0)).is_some(), "in the notch, clear of both faces");
        assert!(d.locate(Point2D::new(31.0, 35.0)).is_none(), "1 unit from the arm");
        assert!(d.locate(Point2D::new(35.0, 31.0)).is_none(), "1 unit from the base");
        assert!(d.locate(Point2D::new(33.0, 35.0)).is_some(), "3 units from the arm");
        // The base's interior is still solid, and its top face keeps its standoff.
        assert!(d.locate(Point2D::new(50.0, 20.0)).is_none(), "inside the base");
        assert!(d.locate(Point2D::new(50.0, 31.0)).is_none(), "1 unit above the base");
    }

    #[test]
    fn an_empty_domain_is_a_single_strip() {
        let d = decompose(&space(vec![]), 0.0).unwrap();
        assert!(d.cell_count() >= 1);
        assert!(d.locate(Point2D::new(50.0, 50.0)).is_some());
    }

    #[test]
    fn a_box_splits_free_space() {
        let d = decompose(&space(vec![square(40.0, 40.0, 60.0, 60.0)]), 0.0).unwrap();
        assert_eq!(d.locate(Point2D::new(50.0, 50.0)), None);
        assert!(d.locate(Point2D::new(50.0, 20.0)).is_some());
        assert!(d.locate(Point2D::new(20.0, 50.0)).is_some());
        assert!(d.locate(Point2D::new(80.0, 50.0)).is_some());
    }



    /// A full-height vertical barrier with a gap in it. Returns the decomposition and whether a
    /// cell in the gap exists, and whether the left half reaches the right half by walking
    /// right-portals.

    #[test]
    fn cells_are_convex_and_non_negative_in_area() {
        let d = decompose(&space(vec![square(40.0, 40.0, 60.0, 60.0), square(10.0, 10.0, 20.0, 20.0)]), 3.0).unwrap();
        for cell in d.cells() {
            let c = cell.corners();
            for i in 0..4 {
                let o = orient2d(c[i], c[(i + 1) % 4], c[(i + 2) % 4]);
                assert!(
                    o == Orientation::CounterClockwise || o == Orientation::Collinear,
                    "cell {c:?} is not convex"
                );
            }
            assert!(cell.area() >= 0.0);
        }
    }

    #[test]
    fn events_are_sorted_and_distinct() {
        let d = decompose(&space(vec![square(40.0, 40.0, 60.0, 60.0), square(10.0, 10.0, 20.0, 20.0)]), 0.0).unwrap();
        for w in d.xs.windows(2) {
            assert!(w[0] < w[1], "event xs not strictly increasing: {w:?}");
        }
    }

    #[test]
    fn sort_f64_is_a_total_order() {
        let mut v = vec![1.0, -0.0, 0.0, f64::NAN, -1.0, 1.0, 2.0, -2.0];
        sort_f64_ascending(&mut v);
        assert_eq!(v[0], -2.0);
        assert_eq!(v[1], -1.0);
        assert_eq!(v[2].to_bits(), (-0.0f64).to_bits());
        assert_eq!(v[3].to_bits(), 0.0f64.to_bits());
        assert_eq!(v[4], 1.0);
        assert_eq!(v[5], 1.0);
        assert_eq!(v[6], 2.0);
        assert!(v[7].is_nan());
    }

    #[test]
    fn sort_f64_keys_are_monotonic_in_the_numeric_value() {
        // The specific failure this guards: the IEEE flip orders *unsigned*, so comparing the
        // keys as signed integers would place every non-negative value before every negative one.
        let ascending = [-3.0f64, -0.5, 0.0, 0.5, 3.0];
        let keys: Vec<u64> = ascending.iter().map(|x| f64_sort_key(*x)).collect();
        assert!(keys.windows(2).all(|w| w[0] < w[1]), "{keys:?}");
        let mut probe = vec![3.0, -0.5, 0.5, 0.0, -3.0];
        sort_f64_ascending(&mut probe);
        assert_eq!(probe, ascending.to_vec());
    }

    #[test]
    fn degenerate_workspace_is_rejected() {
        let bad = FreeSpace {
            workspace: BoundingBox::new(Point2D::ZERO, Point2D::ZERO),
            obstacles: vec![],
        };
        assert_eq!(decompose(&bad, 0.0).unwrap_err(), PathPlanError::DegenerateWorkspace);
    }

    #[test]
    fn a_negative_or_nan_margin_is_rejected() {
        let s = space(vec![]);
        assert!(matches!(decompose(&s, -1.0), Err(PathPlanError::InvalidConfig(_))));
        assert!(matches!(decompose(&s, f64::NAN), Err(PathPlanError::InvalidConfig(_))));
    }
}