use alloc::vec::Vec;
use crate::error::PathPlanError;
pub mod clearance;
pub mod point;
pub mod polygon;
pub mod predicates;
pub use point::Point2D;
pub use polygon::{BoundingBox, Polygon};
pub use predicates::{Orientation, incircle, orient2d};
#[derive(Clone, Debug, PartialEq)]
pub struct FreeSpace {
pub workspace: BoundingBox,
pub obstacles: Vec<Polygon>,
}
impl FreeSpace {
pub fn new(workspace: BoundingBox, obstacles: Vec<Polygon>) -> Result<Self, PathPlanError> {
if workspace.is_degenerate() {
return Err(PathPlanError::DegenerateWorkspace);
}
Ok(Self { workspace, obstacles })
}
pub fn contains(&self, p: Point2D) -> bool {
self.workspace.contains(p) && !self.obstacles.iter().any(|o| o.contains(p))
}
}
pub type Line = (f64, f64);
pub fn segment_within_band(
a: Point2D,
b: Point2D,
lower: Line,
upper: Line,
x0: f64,
x1: f64,
) -> bool {
let (u, v) = (x0.min(x1), x0.max(x1));
if u > v {
return false;
}
let slope = if b.x == a.x { 0.0 } else { (b.y - a.y) / (b.x - a.x) };
let segment = (slope, a.y - slope * a.x);
above(segment, lower, u, v) && above(upper, segment, u, v)
}
fn above(probe: Line, bound: Line, lo: f64, hi: f64) -> bool {
let m = probe.0 - bound.0;
let c = probe.1 - bound.1;
let at = |x: f64| {
let v = m * x + c;
let scale = m.abs() * x.abs() + c.abs() + 1.0;
v >= -1e-9 * scale
};
if m >= 0.0 {
at(lo)
} else {
at(hi)
}
}
pub const EPS: f64 = 1e-12;
#[cfg(test)]
mod band_tests {
use super::*;
fn line(m: f64, b: f64) -> Line {
(m, b)
}
#[test]
fn a_segment_inside_a_band_is_accepted() {
assert!(segment_within_band(
Point2D::new(0.0, 5.0),
Point2D::new(10.0, 7.0),
line(0.0, 0.0),
line(0.0, 10.0),
0.0,
10.0
));
}
#[test]
fn a_segment_leaving_the_band_is_rejected() {
assert!(!segment_within_band(
Point2D::new(0.0, 5.0),
Point2D::new(10.0, 15.0),
line(0.0, 0.0),
line(0.0, 10.0),
0.0,
10.0
));
}
#[test]
fn a_segment_starting_exactly_on_the_lower_bound_is_accepted() {
let a = Point2D::new(15.769401554340265, 77.23059844565974);
let b = Point2D::new(90.0, 90.0);
let lower = line(0.0, 77.23059844565974);
let upper = line(0.0, 97.76940155434026);
assert!(segment_within_band(a, b, lower, upper, 15.769401554340265, 16.230598445659734));
}
#[test]
fn a_segment_starting_exactly_on_a_sloped_bound_is_accepted() {
let a = Point2D::new(1.0, 1.0);
let b = Point2D::new(9.0, 9.0);
assert!(segment_within_band(a, b, line(1.0, 0.0), line(0.0, 20.0), 1.0, 9.0));
}
#[test]
fn a_segment_touching_a_bound_is_accepted_either_side() {
let a = Point2D::new(0.0, 5.0);
let b = Point2D::new(10.0, 5.0);
assert!(segment_within_band(a, b, line(0.0, 5.0), line(0.0, 9.0), 0.0, 10.0));
assert!(segment_within_band(a, b, line(0.0, 1.0), line(0.0, 5.0), 0.0, 10.0));
}
#[test]
fn a_vertical_segment_is_tested_by_its_ordinate_range() {
assert!(segment_within_band(
Point2D::new(5.0, 2.0),
Point2D::new(5.0, 8.0),
line(0.0, 1.0),
line(0.0, 9.0),
5.0,
5.0
));
assert!(!segment_within_band(
Point2D::new(5.0, 0.0),
Point2D::new(5.0, 8.0),
line(0.0, 1.0),
line(0.0, 9.0),
5.0,
5.0
));
}
#[test]
fn a_partial_range_is_tested_only_where_it_is_asked() {
let a = Point2D::new(0.0, 0.5);
let b = Point2D::new(10.0, 5.0);
assert!(segment_within_band(a, b, line(0.0, 0.0), line(0.0, 10.0), 0.0, 1.0));
assert!(!segment_within_band(a, b, line(0.0, 0.0), line(0.0, 1.0), 0.0, 10.0));
}
}