use alloc::vec;
use alloc::vec::Vec;
use super::Corridor;
use crate::decomp::Decomposition;
use crate::error::PathPlanError;
use crate::geom::point::Point2D;
use crate::geom::{Line, segment_within_band};
use crate::geom::EPS;
#[derive(Clone, Debug, Default, PartialEq)]
pub struct TautPath {
pub knots: Vec<Point2D>,
}
impl TautPath {
#[inline]
pub fn len(&self) -> usize {
self.knots.len()
}
#[inline]
pub fn is_empty(&self) -> bool {
self.knots.is_empty()
}
}
#[inline]
fn triarea2(s: f64, a: Point2D, b: Point2D, c: Point2D) -> f64 {
s * ((b.x - a.x) * (c.y - a.y) - (c.x - a.x) * (b.y - a.y))
}
pub fn string_pull(
decomp: &Decomposition,
corridor: &Corridor,
start: Point2D,
goal: Point2D,
) -> Result<TautPath, PathPlanError> {
if corridor.cells.is_empty() {
return Err(PathPlanError::NoPathFound);
}
if corridor.cells.len() == 1 {
return Ok(TautPath { knots: vec![start, goal] });
}
let mut xs: Vec<f64> = Vec::with_capacity(corridor.portals.len() + 2);
xs.push(start.x);
for id in &corridor.portals {
xs.push(decomp.portals()[*id as usize].x);
}
xs.push(goal.x);
let mut signs: Vec<f64> = Vec::with_capacity(xs.len());
for k in 0..xs.len() {
signs.push(if k + 1 < xs.len() && xs[k + 1] >= xs[k] { -1.0 } else { 1.0 });
}
if let Some(last) = signs.len().checked_sub(2).and_then(|i| signs.get(i).copied()) {
if let Some(slot) = signs.last_mut() {
*slot = last;
}
}
let mut portals: Vec<(Point2D, Point2D)> = Vec::with_capacity(xs.len());
for k in 0..xs.len() {
if k == 0 || k + 1 == xs.len() {
let point = if k == 0 { start } else { goal };
portals.push((point, point));
continue;
}
let p = decomp.portals()[corridor.portals[k - 1] as usize];
portals.push((p.top(), p.bottom()));
}
let mut path: Vec<Point2D> = Vec::new();
let mut apex = start;
let mut left_from = 0usize;
let mut right_from = 0usize;
let mut left = start;
let mut right = start;
let mut i = 0usize;
while i < portals.len() {
let (new_left, new_right) = portals[i];
let s = signs[i];
if triarea2(s, apex, right, new_right) <= 0.0 {
if apex == right || triarea2(s, apex, left, new_right) > 0.0 {
right = new_right;
right_from = i;
} else {
path.push(left);
apex = left;
i = left_from + 1;
left = apex;
right = apex;
left_from = i;
right_from = i;
continue;
}
}
if triarea2(s, apex, left, new_left) >= 0.0 {
if apex == left || triarea2(s, apex, right, new_left) < 0.0 {
left = new_left;
left_from = i;
} else {
path.push(right);
apex = right;
i = right_from + 1;
left = apex;
right = apex;
left_from = i;
right_from = i;
continue;
}
}
i += 1;
}
path.push(goal);
path.insert(0, start);
let path = drop_redundant_knots(path)?;
if !inside_corridor(decomp, &corridor.cells, &path.knots) {
return Err(PathPlanError::NoPathFound);
}
Ok(path)
}
fn drop_redundant_knots(mut knots: Vec<Point2D>) -> Result<TautPath, PathPlanError> {
if knots.len() < 2 {
return Err(PathPlanError::NoPathFound);
}
knots.dedup();
if knots.len() < 2 {
return Ok(TautPath { knots });
}
let mut out: Vec<Point2D> = Vec::with_capacity(knots.len());
out.push(knots[0]);
for w in knots.windows(3) {
let (a, b, c) = (w[0], w[1], w[2]);
let degenerate =
(b - a).is_zero() || (c - b).is_zero() || (b - a).cross(c - b).abs() <= 0.0;
if !degenerate {
out.push(b);
}
}
out.push(knots[knots.len() - 1]);
out.dedup();
Ok(TautPath { knots: out })
}
fn inside_corridor(
decomp: &Decomposition,
cells: &[crate::decomp::CellId],
knots: &[Point2D],
) -> bool {
let mut bounds: Vec<(f64, f64, Line, Line)> = cells
.iter()
.map(|c| {
let cell = decomp.cells()[*c as usize];
(
cell.bl.x,
cell.br.x,
(line_of(cell.bl, cell.br)),
(line_of(cell.tl, cell.tr)),
)
})
.collect();
bounds.sort_by(|a, b| a.0.total_cmp(&b.0));
for seg in knots.windows(2) {
let (a, b) = (seg[0], seg[1]);
let (lo, hi) = (a.x.min(b.x), a.x.max(b.x));
let mut covered: Vec<(f64, f64)> = Vec::new();
for (cell_lo, cell_hi, lower, upper) in &bounds {
if *cell_lo > hi {
break;
}
if *cell_hi < lo {
continue;
}
let (u, v) = (lo.max(*cell_lo), hi.min(*cell_hi));
if !segment_within_band(a, b, *lower, *upper, u, v) {
continue;
}
match covered.last_mut() {
Some(last) if last.1 >= u - EPS => last.1 = last.1.max(v),
_ => covered.push((u, v)),
}
}
let mut reach = lo;
for (u, v) in &covered {
if *u > reach + EPS {
return false;
}
reach = reach.max(*v);
}
if reach < hi - EPS {
return false;
}
}
true
}
fn line_of(p: Point2D, q: Point2D) -> Line {
let dx = q.x - p.x;
if dx == 0.0 {
return (f64::INFINITY, 0.0);
}
let slope = (q.y - p.y) / dx;
(slope, p.y - slope * p.x)
}
#[cfg(test)]
mod tests {
use super::*;
use crate::decomp::cell::{Cell, Portal, PortalId};
use crate::decomp::Decomposition;
use alloc::vec;
fn slot_corridor() -> (Decomposition, Corridor) {
let cells = vec![
Cell { bl: Point2D::new(25.0, 47.0), br: Point2D::new(31.0, 47.0), tr: Point2D::new(31.0, 99.0), tl: Point2D::new(25.0, 99.0) },
Cell { bl: Point2D::new(23.0, 1.0), br: Point2D::new(25.0, 1.0), tr: Point2D::new(25.0, 99.0), tl: Point2D::new(23.0, 99.0) },
Cell { bl: Point2D::new(17.0, 39.0), br: Point2D::new(23.0, 39.0), tr: Point2D::new(23.0, 41.0), tl: Point2D::new(17.0, 41.0) },
Cell { bl: Point2D::new(15.0, 1.0), br: Point2D::new(17.0, 1.0), tr: Point2D::new(17.0, 99.0), tl: Point2D::new(15.0, 99.0) },
Cell { bl: Point2D::new(9.0, 47.0), br: Point2D::new(15.0, 47.0), tr: Point2D::new(15.0, 99.0), tl: Point2D::new(9.0, 99.0) },
];
let portals = vec![
Portal { x: 25.0, lo: 47.0, hi: 99.0, left: 1, right: 0 },
Portal { x: 23.0, lo: 39.0, hi: 41.0, left: 2, right: 1 },
Portal { x: 17.0, lo: 39.0, hi: 41.0, left: 3, right: 2 },
Portal { x: 15.0, lo: 47.0, hi: 99.0, left: 4, right: 3 },
];
let mut d = Decomposition::empty();
d.build_from_parts(vec![9.0, 15.0, 17.0, 23.0, 25.0, 31.0], cells, portals);
let corridor = Corridor {
cells: vec![0, 1, 2, 3, 4],
portals: vec![0 as PortalId, 1, 2, 3],
};
(d, corridor)
}
#[test]
fn the_funnel_touches_both_ends_of_a_slot() {
let (d, corridor) = slot_corridor();
let start = Point2D::new(30.0, 50.0);
let goal = Point2D::new(10.0, 50.0);
let path = string_pull(&d, &corridor, start, goal).unwrap();
let want = vec![
start,
Point2D::new(25.0, 47.0),
Point2D::new(23.0, 41.0),
Point2D::new(17.0, 41.0),
Point2D::new(15.0, 47.0),
goal,
];
assert_eq!(path.knots, want);
}
#[test]
fn the_funnel_stays_inside_the_slot_corridor() {
let (d, corridor) = slot_corridor();
let start = Point2D::new(30.0, 50.0);
let goal = Point2D::new(10.0, 50.0);
let path = string_pull(&d, &corridor, start, goal).unwrap();
for seg in path.knots.windows(2) {
let steps = ((seg[0].distance(seg[1]) * 20.0).ceil() as usize).max(1);
for k in 0..=steps {
let p = seg[0].lerp(seg[1], k as f64 / steps as f64);
assert!(
corridor.cells.iter().any(|c| d.cells()[*c as usize].contains(p)),
"sample {p:?} on {:?}..{:?} leaves the corridor",
seg[0],
seg[1]
);
}
}
}
}