use alloc::vec::Vec;
use crate::decomp::{CellId, Decomposition};
use crate::error::PathPlanError;
use crate::geom::clearance::Clearance;
use crate::geom::point::Point2D;
use crate::spline::CubicBezierSegment;
#[derive(Clone, Copy, Debug, PartialEq)]
struct HalfPlane {
a: f64,
b: f64,
c: f64,
}
impl HalfPlane {
#[inline]
fn value(&self, p: Point2D) -> f64 {
self.a * p.x + self.b * p.y + self.c
}
#[inline]
fn slack(&self) -> f64 {
-1e-9 * (self.a.abs() + self.b.abs()).max(1.0)
}
fn of_cell(cell: &crate::decomp::Cell) -> [HalfPlane; 4] {
let c = [cell.bl, cell.br, cell.tr, cell.tl];
let mut out = [HalfPlane { a: 0.0, b: 0.0, c: 0.0 }; 4];
for i in 0..4 {
let d = c[(i + 1) % 4] - c[i];
out[i] = HalfPlane { a: -d.y, b: d.x, c: d.y * c[i].x - d.x * c[i].y };
}
out
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct AdmissibleSet {
planes: Vec<HalfPlane>,
knot: Point2D,
vertices: Vec<Point2D>,
}
impl AdmissibleSet {
fn new(knot: Point2D, forward: &crate::decomp::Cell, backward: &crate::decomp::Cell) -> Self {
let mut planes = Vec::with_capacity(8);
for p in HalfPlane::of_cell(forward) {
planes.push(HalfPlane {
a: p.a,
b: p.b,
c: 3.0 * (p.a * knot.x + p.b * knot.y + p.c),
});
}
for p in HalfPlane::of_cell(backward) {
planes.push(HalfPlane {
a: -p.a,
b: -p.b,
c: 3.0 * (p.a * knot.x + p.b * knot.y + p.c),
});
}
let vertices = Self::solve_vertices(&planes);
Self { planes, knot, vertices }
}
pub fn knot(&self) -> Point2D {
self.knot
}
pub fn contains(&self, t: Point2D) -> bool {
self.planes.iter().all(|p| p.value(t) >= p.slack())
}
pub fn is_pinned(&self) -> bool {
self.vertices().is_empty()
}
fn solve_vertices(planes: &[HalfPlane]) -> Vec<Point2D> {
let n = planes.len();
let mut out: Vec<Point2D> = Vec::with_capacity(n);
for i in 0..n {
for j in (i + 1)..n {
let (p, q) = (planes[i], planes[j]);
let det = p.a * q.b - q.a * p.b;
if det.abs() < 1e-12 {
continue; }
let v = Point2D::new(
(p.b * q.c - q.b * p.c) / det,
(q.a * p.c - p.a * q.c) / det,
);
if !v.is_finite() {
continue;
}
let inside = planes.iter().all(|r| r.value(v) >= r.slack());
if inside && !out.iter().any(|w| w.distance(v) < 1e-9) {
out.push(v);
}
}
}
out.sort_by(|a, b| a.x.total_cmp(&b.x).then_with(|| a.y.total_cmp(&b.y)));
out
}
pub fn vertices(&self) -> &[Point2D] {
&self.vertices
}
pub fn project(&self, t: Point2D) -> Point2D {
if self.contains(t) {
return t;
}
let v = &self.vertices;
if v.is_empty() {
return Point2D::ZERO;
}
let mut best = v[0];
let mut best_d2 = f64::INFINITY;
for i in 0..v.len() {
let (a, b) = (v[i], v[(i + 1) % v.len()]);
let q = crate::corridor::closest_point_on_segment(t, a, b);
let d2 = q.distance_squared(t);
if d2 < best_d2 {
best_d2 = d2;
best = q;
}
}
for w in v {
let d2 = w.distance_squared(t);
if d2 < best_d2 {
best_d2 = d2;
best = *w;
}
}
best
}
}
pub fn closest_point_on_segment(p: Point2D, a: Point2D, b: Point2D) -> Point2D {
let ab = b - a;
let len2 = ab.norm_squared();
if len2 == 0.0 {
return a;
}
a + ab * (((p - a).dot(ab) / len2).clamp(0.0, 1.0))
}
pub fn segments_for(knots: &[Point2D], tangents: &[Point2D]) -> Vec<CubicBezierSegment> {
(0..knots.len().saturating_sub(1))
.map(|i| CubicBezierSegment {
p0: knots[i],
p1: knots[i] + tangents[i] / 3.0,
p2: knots[i + 1] - tangents[i + 1] / 3.0,
p3: knots[i + 1],
})
.collect()
}
#[derive(Clone, Debug, Default)]
pub struct AdmissibleTangents {
sets: Vec<AdmissibleSet>,
}
impl AdmissibleTangents {
pub fn get(&self, i: usize) -> Option<&AdmissibleSet> {
self.sets.get(i)
}
pub fn build(
decomp: &Decomposition,
corridor_cells: &[CellId],
knots: &[Point2D],
) -> Result<Self, PathPlanError> {
if knots.len() < 2 || corridor_cells.is_empty() {
return Err(PathPlanError::NoPathFound);
}
let mut start_cell: Vec<CellId> = Vec::with_capacity(knots.len() - 1);
let mut end_cell: Vec<CellId> = Vec::with_capacity(knots.len() - 1);
for i in 0..knots.len() - 1 {
let (a, b) = (knots[i], knots[i + 1]);
let forward = cell_entered_from(decomp, corridor_cells, a, b);
let backward = cell_entered_from(decomp, corridor_cells, b, a);
match (forward, backward) {
(Some(f), Some(k)) => {
start_cell.push(f);
end_cell.push(k);
}
_ => return Err(PathPlanError::NoPathFound),
}
}
let mut sets = Vec::with_capacity(knots.len());
for i in 0..knots.len() {
let forward_id = if i + 1 < knots.len() { start_cell[i] } else { end_cell[i - 1] };
let backward_id = if i > 0 { end_cell[i - 1] } else { start_cell[0] };
sets.push(AdmissibleSet::new(
knots[i],
&decomp.cells()[forward_id as usize],
&decomp.cells()[backward_id as usize],
));
}
Ok(Self { sets })
}
pub fn control_points(
&self,
knots: &[Point2D],
tangents: &[Point2D],
clearance: &Clearance,
) -> Result<Vec<CubicBezierSegment>, PathPlanError> {
if self.first_obstructed(knots, tangents, clearance).is_some() {
return Err(PathPlanError::NoPathFound);
}
Ok(segments_for(knots, tangents))
}
pub fn first_obstructed(
&self,
knots: &[Point2D],
tangents: &[Point2D],
clearance: &Clearance,
) -> Option<usize> {
self.first_obstructed_from(knots, tangents, clearance, 0)
}
pub fn first_obstructed_from(
&self,
knots: &[Point2D],
tangents: &[Point2D],
clearance: &Clearance,
from: usize,
) -> Option<usize> {
if knots.len() < 2 || tangents.len() != knots.len() {
return Some(0);
}
for (i, t) in tangents.iter().enumerate() {
match self.sets.get(i) {
Some(set) if set.contains(*t) => {}
_ => return Some(i.min(knots.len() - 2)),
}
}
for i in from..knots.len() - 1 {
let hull = [
knots[i],
knots[i] + tangents[i] / 3.0,
knots[i + 1] - tangents[i + 1] / 3.0,
knots[i + 1],
];
if !clearance.hull_is_free(&hull) {
return Some(i);
}
}
None
}
}
fn cell_entered_from(
decomp: &Decomposition,
corridor_cells: &[CellId],
a: Point2D,
b: Point2D,
) -> Option<CellId> {
for f in [1e-6, 1e-4, 1e-3, 1e-2, 0.25, 0.5] {
let p = a.lerp(b, f);
let hit = corridor_cells
.iter()
.copied()
.find(|c| decomp.cells()[*c as usize].contains(p));
if let Some(c) = hit {
return Some(c);
}
}
None
}