use alloc::vec::Vec;
use super::point::Point2D;
use super::predicates::{Orientation, orient2d};
use crate::error::{InvalidObstacleReason, PathPlanError};
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct BoundingBox {
pub min: Point2D,
pub max: Point2D,
}
impl BoundingBox {
pub fn new(a: Point2D, b: Point2D) -> Self {
Self { min: Point2D::new(a.x.min(b.x), a.y.min(b.y)), max: Point2D::new(a.x.max(b.x), a.y.max(b.y)) }
}
#[inline]
pub fn width(&self) -> f64 {
self.max.x - self.min.x
}
#[inline]
pub fn height(&self) -> f64 {
self.max.y - self.min.y
}
#[inline]
pub fn area(&self) -> f64 {
self.width() * self.height()
}
#[inline]
pub fn is_degenerate(&self) -> bool {
!(self.width() > 0.0 && self.height() > 0.0)
}
#[inline]
pub fn contains(&self, p: Point2D) -> bool {
p.x >= self.min.x && p.x <= self.max.x && p.y >= self.min.y && p.y <= self.max.y
}
#[inline]
pub fn with(mut self, p: Point2D) -> Self {
self.min.x = self.min.x.min(p.x);
self.min.y = self.min.y.min(p.y);
self.max.x = self.max.x.max(p.x);
self.max.y = self.max.y.max(p.y);
self
}
pub fn corners(&self) -> [Point2D; 4] {
[
self.min,
Point2D::new(self.max.x, self.min.y),
self.max,
Point2D::new(self.min.x, self.max.y),
]
}
pub fn eroded(&self, d: f64) -> Self {
Self {
min: Point2D::new(self.min.x + d, self.min.y + d),
max: Point2D::new(self.max.x - d, self.max.y - d),
}
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct Polygon {
vertices: Vec<Point2D>,
}
impl Polygon {
pub fn new(mut vertices: Vec<Point2D>) -> Result<Self, PathPlanError> {
if vertices.iter().any(|p| !p.is_finite()) {
return Err(Self::err(InvalidObstacleReason::NaNOrInfinite));
}
for i in 0..vertices.len() {
if vertices[i] == vertices[(i + 1) % vertices.len()] {
return Err(Self::err(InvalidObstacleReason::ZeroLengthEdge));
}
}
if vertices.len() < 3 {
return Err(Self::err(InvalidObstacleReason::TooFewVertices));
}
for i in 0..vertices.len() {
for j in (i + 1)..vertices.len() {
if vertices[i] == vertices[j] {
return Err(Self::err(InvalidObstacleReason::RepeatedVertex));
}
}
}
if !is_convex_ring(&vertices) && is_self_intersecting(&vertices) {
return Err(Self::err(InvalidObstacleReason::SelfIntersecting));
}
let area = signed_area(&vertices);
if area == 0.0 {
return Err(Self::err(InvalidObstacleReason::DegenerateArea));
}
if area < 0.0 {
vertices.reverse();
}
Ok(Self { vertices })
}
pub fn new_unchecked(vertices: Vec<Point2D>) -> Self {
assert!(deduped_len(&vertices) >= 3, "degenerate ring");
Self { vertices }
}
#[inline]
pub fn vertices(&self) -> &[Point2D] {
&self.vertices
}
#[inline]
pub fn len(&self) -> usize {
self.vertices.len()
}
#[inline]
pub fn is_empty(&self) -> bool {
self.vertices.is_empty()
}
pub fn bounds(&self) -> BoundingBox {
let mut b = BoundingBox { min: Point2D::new(f64::INFINITY, f64::INFINITY), max: Point2D::new(f64::NEG_INFINITY, f64::NEG_INFINITY) };
for p in &self.vertices {
b.min.x = b.min.x.min(p.x);
b.min.y = b.min.y.min(p.y);
b.max.x = b.max.x.max(p.x);
b.max.y = b.max.y.max(p.y);
}
b
}
pub fn contains(&self, p: Point2D) -> bool {
if !self.bounds().contains(p) {
return false;
}
for i in 0..self.vertices.len() {
let a = self.vertices[i];
let b = self.vertices[(i + 1) % self.vertices.len()];
if on_segment(a, b, p) {
return true;
}
}
let mut inside = false;
for i in 0..self.vertices.len() {
let a = self.vertices[i];
let b = self.vertices[(i + 1) % self.vertices.len()];
let upward = a.y <= p.y && p.y < b.y;
let downward = b.y <= p.y && p.y < a.y;
if upward && is_left(a, b, p) || downward && is_right(a, b, p) {
inside = !inside;
}
}
inside
}
pub fn area(&self) -> f64 {
signed_area(&self.vertices)
}
fn err(reason: InvalidObstacleReason) -> PathPlanError {
PathPlanError::InvalidObstacle { reason }
}
}
#[inline]
fn is_left(a: Point2D, b: Point2D, c: Point2D) -> bool {
orient2d(a, b, c) == Orientation::CounterClockwise
}
#[inline]
fn is_right(a: Point2D, b: Point2D, c: Point2D) -> bool {
orient2d(a, b, c) == Orientation::Clockwise
}
pub fn on_segment(a: Point2D, b: Point2D, p: Point2D) -> bool {
if orient2d(a, b, p) != Orientation::Collinear {
return false;
}
p.x >= a.x.min(b.x) && p.x <= a.x.max(b.x) && p.y >= a.y.min(b.y) && p.y <= a.y.max(b.y)
}
fn signed_area(vertices: &[Point2D]) -> f64 {
let mut acc = 0.0;
for i in 0..vertices.len() {
let a = vertices[i];
let b = vertices[(i + 1) % vertices.len()];
acc += a.cross(b);
}
acc / 2.0
}
fn deduped_len(vertices: &[Point2D]) -> usize {
let mut n = 0usize;
for i in 0..vertices.len() {
if !vertices[..i].contains(&vertices[i]) {
n += 1;
}
}
n
}
fn is_convex_ring(vertices: &[Point2D]) -> bool {
let n = vertices.len();
if n < 4 {
return true;
}
let mut sign = 0i8;
for i in 0..n {
let o = orient2d(vertices[i], vertices[(i + 1) % n], vertices[(i + 2) % n]);
let s = match o {
Orientation::Clockwise => -1,
Orientation::CounterClockwise => 1,
Orientation::Collinear => continue,
};
if sign == 0 {
sign = s;
} else if sign != s {
return false;
}
}
true
}
fn is_self_intersecting(vertices: &[Point2D]) -> bool {
let n = vertices.len();
if n < 4 {
return false;
}
for i in 0..n {
for j in (i + 1)..n {
let adjacent = j == i + 1 || (i == 0 && j == n - 1);
let a0 = vertices[i];
let a1 = vertices[(i + 1) % n];
let b0 = vertices[j];
let b1 = vertices[(j + 1) % n];
if adjacent {
let shared = if j == i + 1 { a1 == b0 } else { a0 == b1 };
if !shared {
return true;
}
} else if segments_intersect(a0, a1, b0, b1) {
return true;
}
}
}
false
}
pub fn segments_cross_properly(a0: Point2D, a1: Point2D, b0: Point2D, b1: Point2D) -> bool {
if a0 == b0 || a0 == b1 || a1 == b0 || a1 == b1 {
return false;
}
if !segments_intersect(a0, a1, b0, b1) {
return false;
}
!on_segment(a0, a1, b0)
&& !on_segment(a0, a1, b1)
&& !on_segment(b0, b1, a0)
&& !on_segment(b0, b1, a1)
}
pub fn segments_intersect(a0: Point2D, a1: Point2D, b0: Point2D, b1: Point2D) -> bool {
let d1 = orient2d(a0, a1, b0);
let d2 = orient2d(a0, a1, b1);
let d3 = orient2d(b0, b1, a0);
let d4 = orient2d(b0, b1, a1);
if d1 != d2 && d3 != d4 {
return true;
}
(d1.is_collinear() && on_segment(a0, a1, b0))
|| (d2.is_collinear() && on_segment(a0, a1, b1))
|| (d3.is_collinear() && on_segment(b0, b1, a0))
|| (d4.is_collinear() && on_segment(b0, b1, a1))
}
#[cfg(test)]
mod tests {
use super::*;
use alloc::vec;
fn square() -> Vec<Point2D> {
vec![
Point2D::new(0.0, 0.0),
Point2D::new(2.0, 0.0),
Point2D::new(2.0, 2.0),
Point2D::new(0.0, 2.0),
]
}
#[test]
fn accepts_a_square_and_is_ccw() {
let poly = Polygon::new(square()).unwrap();
assert_eq!(poly.area(), 4.0);
assert_eq!(poly.len(), 4);
}
#[test]
fn reverses_clockwise_input() {
let mut v = square();
v.reverse();
let poly = Polygon::new(v).unwrap();
assert_eq!(poly.area(), 4.0);
assert_eq!(poly.vertices()[0], Point2D::new(0.0, 0.0));
}
#[test]
fn rejects_non_finite() {
let mut v = square();
v[1].x = f64::NAN;
assert_eq!(
Polygon::new(v).unwrap_err(),
PathPlanError::InvalidObstacle { reason: InvalidObstacleReason::NaNOrInfinite }
);
}
#[test]
fn rejects_zero_length_edge() {
let v = vec![
Point2D::new(0.0, 0.0),
Point2D::new(0.0, 0.0),
Point2D::new(2.0, 0.0),
Point2D::new(0.0, 2.0),
];
assert_eq!(
Polygon::new(v).unwrap_err(),
PathPlanError::InvalidObstacle { reason: InvalidObstacleReason::ZeroLengthEdge }
);
}
#[test]
fn rejects_too_few_vertices() {
let v = vec![Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0), Point2D::new(0.0, 1.0), Point2D::new(0.0, 0.0)];
assert_eq!(
Polygon::new(v).unwrap_err(),
PathPlanError::InvalidObstacle { reason: InvalidObstacleReason::ZeroLengthEdge }
);
assert_eq!(
Polygon::new(vec![Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)]).unwrap_err(),
PathPlanError::InvalidObstacle { reason: InvalidObstacleReason::TooFewVertices }
);
}
#[test]
fn rejects_repeated_vertex() {
let v = vec![
Point2D::new(0.0, 0.0),
Point2D::new(2.0, 0.0),
Point2D::new(2.0, 2.0),
Point2D::new(0.0, 2.0),
Point2D::new(2.0, 0.0),
];
assert_eq!(
Polygon::new(v).unwrap_err(),
PathPlanError::InvalidObstacle { reason: InvalidObstacleReason::RepeatedVertex }
);
}
#[test]
fn rejects_self_intersection() {
let v = vec![
Point2D::new(0.0, 0.0),
Point2D::new(2.0, 2.0),
Point2D::new(2.0, 0.0),
Point2D::new(0.0, 2.0),
];
assert_eq!(
Polygon::new(v).unwrap_err(),
PathPlanError::InvalidObstacle { reason: InvalidObstacleReason::SelfIntersecting }
);
}
#[test]
fn rejects_zero_area() {
let v = vec![
Point2D::new(0.0, 0.0),
Point2D::new(1.0, 0.0),
Point2D::new(2.0, 0.0),
Point2D::new(0.0, 1.0),
];
let collinear = vec![
Point2D::new(0.0, 0.0),
Point2D::new(1.0, 0.0),
Point2D::new(2.0, 0.0),
];
assert!(Polygon::new(v).is_ok());
assert_eq!(
Polygon::new(collinear).unwrap_err(),
PathPlanError::InvalidObstacle { reason: InvalidObstacleReason::DegenerateArea }
);
}
#[test]
fn contains_interior_exterior_and_boundary() {
let poly = Polygon::new(square()).unwrap();
assert!(poly.contains(Point2D::new(1.0, 1.0)));
assert!(!poly.contains(Point2D::new(3.0, 1.0)));
assert!(poly.contains(Point2D::new(0.0, 1.0)));
assert!(poly.contains(Point2D::new(1.0, 0.0)));
assert!(poly.contains(Point2D::new(2.0, 2.0)));
}
#[test]
fn bounds_and_area() {
let poly = Polygon::new(square()).unwrap();
assert_eq!(poly.bounds(), BoundingBox::new(Point2D::ZERO, Point2D::new(2.0, 2.0)));
assert!((poly.area() - 4.0).abs() < 1e-12);
}
#[test]
fn segment_intersection() {
let a0 = Point2D::new(0.0, 0.0);
let a1 = Point2D::new(4.0, 0.0);
assert!(segments_intersect(a0, a1, Point2D::new(1.0, -1.0), Point2D::new(1.0, 1.0)));
assert!(!segments_intersect(a0, a1, Point2D::new(0.0, 1.0), Point2D::new(4.0, 1.0)));
assert!(segments_intersect(a0, a1, Point2D::new(2.0, 0.0), Point2D::new(2.0, 1.0)));
}
#[test]
fn bounding_box_helpers() {
let b = BoundingBox::new(Point2D::new(2.0, 4.0), Point2D::new(0.0, 0.0));
assert_eq!(b.min, Point2D::ZERO);
assert_eq!(b.max, Point2D::new(2.0, 4.0));
assert_eq!(b.width(), 2.0);
assert_eq!(b.height(), 4.0);
assert_eq!(b.area(), 8.0);
assert!(!b.is_degenerate());
assert!(b.contains(Point2D::new(1.0, 1.0)));
assert!(!b.contains(Point2D::new(3.0, 1.0)));
assert!(b.eroded(0.5) == BoundingBox::new(Point2D::new(0.5, 0.5), Point2D::new(1.5, 3.5)));
assert_eq!(b.corners()[0], Point2D::ZERO);
assert_eq!(b.corners()[2], b.max);
}
#[test]
fn degenerate_box_detected() {
let b = BoundingBox { min: Point2D::new(1.0, 1.0), max: Point2D::new(1.0, 5.0) };
assert!(b.is_degenerate());
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Rect {
pub min: Point2D,
pub max: Point2D,
}
impl Rect {
#[inline]
pub fn is_degenerate(&self) -> bool {
!(self.max.x > self.min.x && self.max.y > self.min.y)
}
pub fn outset(&self, d: f64) -> Self {
Rect {
min: Point2D::new(self.min.x - d, self.min.y - d),
max: Point2D::new(self.max.x + d, self.max.y + d),
}
}
pub fn corners(&self) -> [Point2D; 4] {
[
self.min,
Point2D::new(self.max.x, self.min.y),
self.max,
Point2D::new(self.min.x, self.max.y),
]
}
}
pub fn is_rectilinear(vertices: &[Point2D]) -> bool {
vertices.iter().enumerate().all(|(i, a)| {
let b = vertices[(i + 1) % vertices.len()];
a.x == b.x || a.y == b.y
})
}
pub fn rectilinear_rectangles(vertices: &[Point2D]) -> Option<Vec<Rect>> {
if !is_rectilinear(vertices) {
return None;
}
let mut ys: Vec<f64> = vertices.iter().map(|p| p.y).collect();
sort_f64_unique(&mut ys);
let _ = &ys;
if ys.len() < 2 {
return None;
}
let mut rects: Vec<Rect> = Vec::with_capacity(vertices.len());
for window in ys.windows(2) {
let (lo, hi) = (window[0], window[1]);
let mid = 0.5 * (lo + hi);
let mut crossings: Vec<f64> = Vec::new();
for i in 0..vertices.len() {
let a = vertices[i];
let b = vertices[(i + 1) % vertices.len()];
if a.x == b.x && a.y != b.y {
let (low, high) = (a.y.min(b.y), a.y.max(b.y));
if mid >= low && mid < high {
crossings.push(a.x);
}
}
}
crossings.sort_by(|p, q| {
if p < q { core::cmp::Ordering::Less } else { core::cmp::Ordering::Greater }
});
for pair in crossings.chunks_exact(2) {
let rect = Rect { min: Point2D::new(pair[0], lo), max: Point2D::new(pair[1], hi) };
if !rect.is_degenerate() {
rects.push(rect);
}
}
}
Some(rects)
}
fn sort_f64_unique(values: &mut Vec<f64>) {
values.sort_by(|a, b| {
if a < b { core::cmp::Ordering::Less } else { core::cmp::Ordering::Greater }
});
values.dedup();
}