use alloc::vec;
use alloc::vec::Vec;
use crate::decomp::sweep::forbidden_rectangles;
use crate::error::PathPlanError;
use crate::geom::point::Point2D;
use crate::router::scratch::Scratch;
use crate::router::RouteRequest;
#[derive(Clone, Debug, Default, PartialEq)]
pub struct OrthogonalPolyline {
pub points: Vec<Point2D>,
}
impl OrthogonalPolyline {
pub fn len(&self) -> usize {
self.points.len()
}
pub fn is_empty(&self) -> bool {
self.points.is_empty()
}
pub fn is_axis_aligned(&self) -> bool {
self.points.windows(2).all(|w| (w[0].x == w[1].x) ^ (w[0].y == w[1].y))
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
struct Cell {
i: usize,
j: usize,
}
pub fn route_orthogonal(
scratch: &mut Scratch,
req: &RouteRequest,
) -> Result<OrthogonalPolyline, PathPlanError> {
req.validate()?;
let margin = req.config.margin;
let workspace = req.workspace.eroded(margin);
if workspace.is_degenerate() {
return Err(PathPlanError::NoPathFound);
}
let forbidden = forbidden_rectangles(&req.obstacles, margin)?;
let grid = Grid::build(&workspace, &forbidden);
let start = req.start.point;
let goal = req.goal.point;
let start_cell = grid.locate(start).ok_or(PathPlanError::NoPathFound)?;
let goal_cell = grid.locate(goal).ok_or(PathPlanError::NoPathFound)?;
let path = grid.astar(&mut scratch.grid, start_cell, goal_cell)?;
let mut polyline = grid.to_polyline(&path, start, goal);
polyline.points = simplify(core::mem::take(&mut polyline.points));
if polyline.points.first() != Some(&start) {
polyline.points.insert(0, start);
}
if polyline.points.last() != Some(&goal) {
polyline.points.push(goal);
}
let clearance = req.clearance();
for w in polyline.points.windows(2) {
for k in 0..=8 {
let q = w[0].lerp(w[1], k as f64 / 8.0);
if !clearance.is_free(q) {
return Err(PathPlanError::NoPathFound);
}
}
}
Ok(polyline)
}
struct Grid {
xs: Vec<f64>,
ys: Vec<f64>,
free: Vec<bool>,
nx: usize,
}
impl Grid {
fn build(workspace: &crate::geom::BoundingBox, forbidden: &[crate::geom::Polygon]) -> Self {
let mut xs = vec![workspace.min.x, workspace.max.x];
let mut ys = vec![workspace.min.y, workspace.max.y];
for obstacle in forbidden {
let b = obstacle.bounds();
xs.push(b.min.x);
xs.push(b.max.x);
ys.push(b.min.y);
ys.push(b.max.y);
}
let (wx0, wx1) = (workspace.min.x, workspace.max.x);
let (wy0, wy1) = (workspace.min.y, workspace.max.y);
xs.retain(|x| *x >= wx0 && *x <= wx1);
ys.retain(|y| *y >= wy0 && *y <= wy1);
crate::decomp::sweep::sort_f64_ascending(&mut xs);
crate::decomp::sweep::sort_f64_ascending(&mut ys);
xs.dedup();
ys.dedup();
let nx = xs.len().saturating_sub(1);
let ny = ys.len().saturating_sub(1);
let mut free = vec![false; nx * ny];
for j in 0..ny {
for i in 0..nx {
let cx = 0.5 * (xs[i] + xs[i + 1]);
let cy = 0.5 * (ys[j] + ys[j + 1]);
free[j * nx + i] = !forbidden.iter().any(|o| o.contains(Point2D::new(cx, cy)));
}
}
Self { xs, ys, free, nx }
}
fn is_free(&self, c: Cell) -> bool {
c.i < self.nx && c.j < self.ys.len() - 1 && self.free[c.j * self.nx + c.i]
}
fn locate(&self, p: Point2D) -> Option<Cell> {
let i = upper_bound(&self.xs, p.x)?.checked_sub(1)?;
let j = upper_bound(&self.ys, p.y)?.checked_sub(1)?;
let c = Cell { i, j };
self.is_free(c).then_some(c)
}
fn astar(&self, scratch: &mut GridScratch, start: Cell, goal: Cell) -> Result<Vec<Cell>, PathPlanError> {
let n = self.free.len();
scratch.reset(n);
let index = |c: Cell| c.j * self.nx + c.i;
let h = |c: Cell| {
(self.xs[c.i] - self.xs[goal.i]).abs() + (self.ys[c.j] - self.ys[goal.j]).abs()
};
scratch.g[index(start)] = 0.0;
scratch.push(((h(start), index(start) as u64), start, 0.0));
while let Some(((f, _), c, g)) = scratch.pop() {
let k = index(c);
if scratch.closed[k] {
continue;
}
scratch.closed[k] = true;
if c == goal {
return Ok(scratch.reconstruct(self.nx, start, goal));
}
for next in [Cell { i: c.i + 1, j: c.j }, Cell { i: c.i.wrapping_sub(1), j: c.j }, Cell { i: c.i, j: c.j + 1 }, Cell { i: c.i, j: c.j.wrapping_sub(1) }] {
if !self.is_free(next) {
continue;
}
let nk = index(next);
if scratch.closed[nk] {
continue;
}
let step = if next.i != c.i {
(self.xs[next.i] - self.xs[c.i]).abs()
} else {
(self.ys[next.j] - self.ys[c.j]).abs()
};
let tentative = g + step;
if tentative < scratch.g[nk] {
scratch.g[nk] = tentative;
scratch.from[nk] = index(c) as u32;
let _ = f;
scratch.push(((h(next), nk as u64), next, tentative));
}
}
}
Err(PathPlanError::NoPathFound)
}
fn centre(&self, c: Cell) -> Point2D {
Point2D::new(0.5 * (self.xs[c.i] + self.xs[c.i + 1]), 0.5 * (self.ys[c.j] + self.ys[c.j + 1]))
}
fn to_polyline(&self, path: &[Cell], start: Point2D, goal: Point2D) -> OrthogonalPolyline {
if path.len() <= 1 {
let corner = Point2D::new(goal.x, start.y);
return OrthogonalPolyline { points: vec![start, corner, goal] };
}
let mut points = Vec::with_capacity(path.len() + 3);
points.push(start);
let push_entry = |points: &mut Vec<Point2D>, target: Point2D| {
if let Some(from) = points.last().copied() {
if from.x != target.x && from.y != target.y {
points.push(Point2D::new(target.x, from.y));
}
}
points.push(target);
};
for c in path {
push_entry(&mut points, self.centre(*c));
}
push_entry(&mut points, goal);
OrthogonalPolyline { points }
}
}
fn key_lt(a: (f64, u64), b: (f64, u64)) -> bool {
if a.0 < b.0 {
true
} else if a.0 > b.0 {
false
} else {
a.1 < b.1
}
}
fn upper_bound(values: &[f64], x: f64) -> Option<usize> {
match values.binary_search_by(|probe| {
if *probe < x {
core::cmp::Ordering::Less
} else if *probe > x {
core::cmp::Ordering::Greater
} else {
core::cmp::Ordering::Equal
}
}) {
Ok(i) => Some(i),
Err(0) => None,
Err(i) => Some(i),
}
}
fn simplify(points: Vec<Point2D>) -> Vec<Point2D> {
if points.len() < 3 {
return points;
}
let mut out: Vec<Point2D> = Vec::with_capacity(points.len());
out.push(points[0]);
for w in points.windows(3) {
let (a, b, c) = (w[0], w[1], w[2]);
let continues = (a.x == b.x && b.x == c.x) || (a.y == b.y && b.y == c.y);
if !continues {
out.push(b);
}
}
out.push(points[points.len() - 1]);
out
}
#[derive(Clone, Debug, Default)]
pub struct GridScratch {
g: Vec<f64>,
from: Vec<u32>,
closed: Vec<bool>,
open: Vec<((f64, u64), Cell, f64)>,
counter: u32,
}
impl GridScratch {
fn reset(&mut self, n: usize) {
self.g.clear();
self.g.resize(n, f64::INFINITY);
self.from.clear();
self.from.resize(n, u32::MAX);
self.closed.clear();
self.closed.resize(n, false);
self.open.clear();
self.counter = 0;
}
fn push(&mut self, entry: ((f64, u64), Cell, f64)) {
self.counter += 1;
self.open.push(entry);
let mut i = self.open.len() - 1;
while i > 0 {
let parent = (i - 1) / 2;
if key_lt(self.open[i].0, self.open[parent].0) {
self.open.swap(i, parent);
i = parent;
} else {
break;
}
}
}
fn pop(&mut self) -> Option<((f64, u64), Cell, f64)> {
let top = self.open.first().cloned()?;
let last = self.open.pop().expect("non-empty: `top` was read from it");
if !self.open.is_empty() {
self.open[0] = last;
let mut i = 0;
loop {
let (l, r) = (2 * i + 1, 2 * i + 2);
let mut largest = i;
if l < self.open.len() && key_lt(self.open[l].0, self.open[largest].0) {
largest = l;
}
if r < self.open.len() && key_lt(self.open[r].0, self.open[largest].0) {
largest = r;
}
if largest == i {
break;
}
self.open.swap(i, largest);
i = largest;
}
}
Some(top)
}
fn reconstruct(&self, nx: usize, start: Cell, goal: Cell) -> Vec<Cell> {
let index = |c: Cell| c.j * nx + c.i;
let mut out = vec![goal];
let mut cur = goal;
while cur != start {
cur = Cell { i: (self.from[index(cur)] as usize % nx), j: (self.from[index(cur)] as usize / nx) };
out.push(cur);
}
out.reverse();
out
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::geom::BoundingBox;
#[test]
fn a_single_cell_still_gives_an_l() {
let g = Grid { xs: vec![0.0, 100.0], ys: vec![0.0, 100.0], free: vec![true], nx: 1 };
let poly = g.to_polyline(
&[Cell { i: 0, j: 0 }],
Point2D::new(10.0, 20.0),
Point2D::new(80.0, 60.0),
);
assert_eq!(poly.points.len(), 3, "{poly:?}");
assert!(poly.is_axis_aligned());
}
#[test]
fn a_multi_cell_path_routes_through_centres() {
let g = Grid {
xs: vec![0.0, 100.0],
ys: vec![0.0, 50.0, 100.0],
free: vec![true; 2],
nx: 1,
};
let poly = g.to_polyline(
&[Cell { i: 0, j: 0 }, Cell { i: 0, j: 1 }],
Point2D::new(10.0, 20.0),
Point2D::new(80.0, 80.0),
);
assert!(poly.is_axis_aligned(), "{poly:?}");
assert!(poly.points.iter().all(|p| p.x >= 0.0 && p.x <= 100.0));
}
#[test]
fn upper_bound_finds_the_cell() {
let v = vec![0.0, 10.0, 20.0];
assert_eq!(upper_bound(&v, -1.0), None);
assert_eq!(upper_bound(&v, 0.0), Some(0));
assert_eq!(upper_bound(&v, 5.0), Some(1));
assert_eq!(upper_bound(&v, 10.0), Some(1));
assert_eq!(upper_bound(&v, 25.0), Some(3));
}
#[test]
fn simplify_drops_collinear_runs() {
let pts = vec![
Point2D::new(0.0, 0.0),
Point2D::new(1.0, 0.0),
Point2D::new(2.0, 0.0),
Point2D::new(2.0, 1.0),
Point2D::new(2.0, 2.0),
];
assert_eq!(
simplify(pts),
vec![Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0), Point2D::new(2.0, 2.0)]
);
}
#[test]
fn the_grid_marks_blocked_cells() {
let obstacle = crate::geom::Polygon::new(alloc::vec![
Point2D::new(40.0, 40.0),
Point2D::new(60.0, 40.0),
Point2D::new(60.0, 60.0),
Point2D::new(40.0, 60.0),
])
.unwrap();
let ws = BoundingBox::new(Point2D::ZERO, Point2D::new(100.0, 100.0));
let g = Grid::build(&ws, alloc::slice::from_ref(&obstacle));
assert!(g.is_free(Cell { i: 0, j: 0 }));
assert!(!g.is_free(Cell { i: 1, j: 1 }), "the box's cell must be blocked");
assert!(g.is_free(Cell { i: 0, j: 1 }));
assert!(g.is_free(Cell { i: 1, j: 0 }));
}
}