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;
#[derive(Clone, Copy, Debug)]
pub(crate) struct Edge {
a: Point2D,
b: Point2D,
obstacle: Option<u32>,
index: u32,
}
impl Edge {
#[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)
}
#[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)
}
#[inline]
fn covers(&self, lo: f64, hi: f64) -> bool {
let (a, b) = self.x_span();
a <= lo && hi <= b
}
}
pub fn decompose(space: &FreeSpace, margin: f64) -> Result<Decomposition, PathPlanError> {
let mut out = Decomposition::empty();
decompose_into(&mut out, space, margin, &[])?;
Ok(out)
}
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)
}
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() {
return Ok(());
}
out.working.reset();
collect_edges(&mut out.working, &workspace, &forbidden);
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 {
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(());
}
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);
}
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)
};
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);
}
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);
}
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);
}
}
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(())
}
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,
});
}
}
pub(crate) fn sort_f64_ascending(values: &mut [f64]) {
values.sort_by_key(|x| f64_sort_key(*x));
}
#[inline]
pub(crate) fn f64_sort_key(x: f64) -> u64 {
let bits = x.to_bits();
if bits >> 63 == 0 { bits | (1u64 << 63) } else { !bits }
}
fn sort_by_y_at(edges: &mut [Edge], x: f64) {
edges.sort_by(|e1, e2| {
let (p, q) = if e1.a.x <= e1.b.x { (e1.a, e1.b) } else { (e1.b, e1.a) };
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 => {
(e1.obstacle, e1.index).cmp(&(e2.obstacle, e2.index))
}
}
});
}
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),
(false, true) => above - 1.0,
(true, false) => below + 1.0,
(false, false) => 0.0,
};
out.push((i, Point2D::new(x, y)));
}
}
fn apply_margin(space: &FreeSpace, margin: f64) -> Result<(BoundingBox, Vec<Polygon>), PathPlanError> {
Ok((space.workspace.eroded(margin), forbidden_rectangles(&space.obstacles, margin)?))
}
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)
}
fn is_free(forbidden: &[Polygon], p: Point2D) -> bool {
!forbidden.iter().any(|o| o.contains(p))
}
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))),
}
}
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] });
}
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() {
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");
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();
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() {
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();
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");
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());
}
#[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() {
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(_))));
}
}