condor_geometry/

polygonal.rs

//! Polygon obstacle scenes and free-space predicates for the polygonal lane.
//!
//! Substrate shared by online [`crate::continuous::PolygonPathfinder`] solvers
//! and source-rooted prepared maps: a [`WorldBounds`] rectangle, simple
//! [`Polygon`] obstacle rings, and walkability / segment legality under a
//! geometry epsilon. **Free space** is the closed world rectangle minus strict
//! obstacle interiors; points on sealed world-boundary edges (obstacle edges
//! collinear with the world frame) are non-traversable for endpoints and
//! segments. Embedded public packs and oracle adapters remain in the facade.
//!
//! # Contract
//!
//! Build a [`PolygonScene`] from a validated [`WorldBounds`] and disjoint
//! [`Polygon`] obstacles, then use its walkability predicates as the shared
//! legality rule for online solvers and prepared maps. This module owns runtime
//! geometry only; embedded public packs are a facade concern.

const EPSILON: f64 = 1e-9;

/// Continuous 2D point in polygon-scene coordinates (not grid cells).
pub use condor_core::Point2;

/// Axis-aligned rectangular world extent that bounds a [`PolygonScene`].
///
/// Free space is the closed rectangle minus obstacle interiors. Constructors
/// do not enforce `min < max`; call [`WorldBounds::validate`] before search
/// setup.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct WorldBounds {
    /// Inclusive lower-left corner of the axis-aligned world rectangle.
    pub min: Point2,
    /// Inclusive upper-right corner of the axis-aligned world rectangle.
    pub max: Point2,
}

/// Static geometry or endpoint validation failure for polygonal scenes.
///
/// Variants describe invariants required before online search or prepared-map
/// preprocess.
#[derive(Debug, Clone, PartialEq, thiserror::Error)]
#[non_exhaustive]
pub enum PolygonValidationError {
    /// `min` is not strictly less than `max` on both axes (zero or negative area).
    #[error("polygon scene world bounds must have positive area")]
    InvalidWorldBounds,
    /// Obstacle ring has fewer than three vertices.
    #[error("polygon obstacle {obstacle_index} must have at least three vertices (found {actual})")]
    TooFewVertices {
        /// Index into [`PolygonScene::obstacles`].
        obstacle_index: usize,
        /// Observed vertex count.
        actual: usize,
    },
    /// Two vertices of the same obstacle coincide within geometry epsilon.
    #[error(
        "polygon obstacle {obstacle_index} repeats vertices {first_vertex_index} and {second_vertex_index}"
    )]
    DuplicateVertices {
        /// Index into [`PolygonScene::obstacles`].
        obstacle_index: usize,
        /// First repeated vertex index.
        first_vertex_index: usize,
        /// Second repeated vertex index.
        second_vertex_index: usize,
    },
    /// Obstacle has near-zero signed area (degenerate ring).
    #[error("polygon obstacle {obstacle_index} must have non-zero area")]
    ZeroArea {
        /// Index into [`PolygonScene::obstacles`].
        obstacle_index: usize,
    },
    /// Obstacle vertex lies outside the inclusive world rectangle.
    #[error(
        "polygon obstacle {obstacle_index} vertex {vertex_index} {vertex:?} must stay inside world bounds"
    )]
    VertexOutsideBounds {
        /// Index into [`PolygonScene::obstacles`].
        obstacle_index: usize,
        /// Offending vertex index within the obstacle ring.
        vertex_index: usize,
        /// Offending vertex coordinates.
        vertex: Point2,
    },
    /// Non-adjacent edges of the same obstacle properly intersect.
    #[error(
        "polygon obstacle {obstacle_index} edges starting at vertices {first_edge_start_index} and {second_edge_start_index} intersect"
    )]
    SelfIntersection {
        /// Index into [`PolygonScene::obstacles`].
        obstacle_index: usize,
        /// Start vertex index of the first intersecting edge.
        first_edge_start_index: usize,
        /// Start vertex index of the second intersecting edge.
        second_edge_start_index: usize,
    },
    /// Two obstacles share interior points, edge crossings, or nested vertices.
    #[error("polygon obstacles {left_index} and {right_index} must be disjoint")]
    ObstaclesOverlap {
        /// First overlapping obstacle index.
        left_index: usize,
        /// Second overlapping obstacle index.
        right_index: usize,
    },
    /// Start, goal, or source is outside free space (including sealed boundary).
    #[error("polygon scene {endpoint:?} endpoint {point:?} must lie in traversable free space")]
    EndpointNotTraversable {
        /// Which endpoint role failed traversability.
        endpoint: PolygonEndpoint,
        /// Rejected continuous point.
        point: Point2,
    },
}

/// Endpoint role carried by [`PolygonValidationError::EndpointNotTraversable`].
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[non_exhaustive]
pub enum PolygonEndpoint {
    /// Request start for pair search.
    Start,
    /// Request goal for pair search or map query.
    Goal,
    /// Fixed source for source-rooted shortest-path maps.
    Source,
}

impl WorldBounds {
    /// Constructs bounds without validating area. Prefer [`WorldBounds::validate`].
    #[must_use]
    pub const fn new(min: Point2, max: Point2) -> Self {
        Self { min, max }
    }

    /// Inclusive containment with geometry epsilon (points on the rectangle edge count as inside).
    #[must_use]
    pub fn contains(self, point: Point2) -> bool {
        point.x >= self.min.x - EPSILON
            && point.x <= self.max.x + EPSILON
            && point.y >= self.min.y - EPSILON
            && point.y <= self.max.y + EPSILON
    }

    /// Ensures the rectangle has strictly positive width and height.
    ///
    /// # Errors
    ///
    /// Returns [`PolygonValidationError::InvalidWorldBounds`] when the bounds do
    /// not have positive area.
    pub fn validate(self) -> Result<(), PolygonValidationError> {
        if self.min.x >= self.max.x || self.min.y >= self.max.y {
            return Err(PolygonValidationError::InvalidWorldBounds);
        }

        Ok(())
    }
}

/// Simple polygon obstacle: closed ordered vertex ring, not necessarily convex.
///
/// Does not auto-validate. Scene validation requires at least three distinct
/// vertices, non-zero area, vertices inside world bounds, no self-intersections,
/// and pairwise disjointness with other obstacles.
#[derive(Debug, Clone, PartialEq)]
pub struct Polygon {
    vertices: Vec<Point2>,
}

impl Polygon {
    /// Builds an obstacle from a vertex ring. Does not check geometry invariants.
    #[must_use]
    pub fn new(vertices: Vec<Point2>) -> Self {
        Self { vertices }
    }

    /// Obstacle ring vertices in declaration order (not rewound by constructors).
    #[must_use]
    pub fn vertices(&self) -> &[Point2] {
        &self.vertices
    }

    /// Shoelace signed area (positive for counter-clockwise rings in this coordinate system).
    ///
    /// Absolute value near zero is treated as degenerate during validation.
    #[must_use]
    pub fn signed_area(&self) -> f64 {
        let mut area = 0.0;
        for (a, b) in polygon_edges(&self.vertices) {
            area += (a.x * b.y) - (b.x * a.y);
        }
        area / 2.0
    }

    /// Ray-cast interior test: true only for strict interior points.
    ///
    /// Points on any edge of the boundary ring return `false` (boundary is free
    /// space unless the edge is sealed against the world boundary).
    #[must_use]
    pub fn contains_point_strict(&self, point: Point2) -> bool {
        if polygon_edges(self.vertices()).any(|(start, end)| point_on_segment(point, start, end)) {
            return false;
        }

        let mut inside = false;
        for (start, end) in polygon_edges(self.vertices()) {
            let crosses = ((start.y > point.y) != (end.y > point.y))
                && (point.x
                    < ((end.x - start.x) * (point.y - start.y) / (end.y - start.y)) + start.x);
            if crosses {
                inside = !inside;
            }
        }
        inside
    }

    /// Validates this obstacle as if it were obstacle index `0` in `world_bounds`.
    ///
    /// # Errors
    ///
    /// Returns a [`PolygonValidationError`] describing the violated polygon
    /// geometry invariant.
    pub fn validate(&self, world_bounds: WorldBounds) -> Result<(), PolygonValidationError> {
        self.validate_at(0, world_bounds)
    }

    fn validate_at(
        &self,
        obstacle_index: usize,
        world_bounds: WorldBounds,
    ) -> Result<(), PolygonValidationError> {
        if self.vertices.len() < 3 {
            return Err(PolygonValidationError::TooFewVertices {
                obstacle_index,
                actual: self.vertices.len(),
            });
        }

        if let Some((first_vertex_index, second_vertex_index)) =
            duplicate_vertex_indices(self.vertices())
        {
            return Err(PolygonValidationError::DuplicateVertices {
                obstacle_index,
                first_vertex_index,
                second_vertex_index,
            });
        }

        if self.signed_area().abs() <= EPSILON {
            return Err(PolygonValidationError::ZeroArea { obstacle_index });
        }

        for (vertex_index, vertex) in self.vertices().iter().enumerate() {
            if !world_bounds.contains(*vertex) {
                return Err(PolygonValidationError::VertexOutsideBounds {
                    obstacle_index,
                    vertex_index,
                    vertex: *vertex,
                });
            }
        }

        if let Some((first_edge_start_index, second_edge_start_index)) =
            self_intersection_edge_indices(self.vertices())
        {
            return Err(PolygonValidationError::SelfIntersection {
                obstacle_index,
                first_edge_start_index,
                second_edge_start_index,
            });
        }

        Ok(())
    }
}

/// Start/goal pair for online polygonal search.
///
/// Coordinates are continuous scene space. Callers validate endpoints through
/// the scene (or accept pathfinder-reported invalid-endpoint errors). Optional
/// [`condor_core::SearchBudget`] caps expansions and/or wall-clock time; default
/// is unlimited.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct PolygonSearchRequest {
    /// Continuous start endpoint in scene coordinates.
    pub start: Point2,
    /// Continuous goal endpoint in scene coordinates.
    pub goal: Point2,
    /// Optional expansion / wall-clock caps for this query (default unlimited).
    pub budget: condor_core::SearchBudget,
}

impl PolygonSearchRequest {
    /// Builds a request with an unlimited budget without validating walkability.
    #[must_use]
    pub const fn new(start: Point2, goal: Point2) -> Self {
        Self {
            start,
            goal,
            budget: condor_core::SearchBudget::UNLIMITED,
        }
    }

    /// Returns a copy of this request with the given budget.
    #[must_use]
    pub const fn with_budget(mut self, budget: condor_core::SearchBudget) -> Self {
        self.budget = budget;
        self
    }
}

/// Bounded free space with polygon obstacles and walkability predicates.
///
/// Public fields are the static scene snapshot: world rectangle plus obstacle
/// rings. Pathfinders and prepared maps treat geometry as immutable for a query.
/// Prefer [`Self::validate_static`] / [`Self::validate`] before trusting a hand-
/// built scene; constructors do not enforce invariants.
#[derive(Debug, Clone, PartialEq)]
pub struct PolygonScene {
    /// Axis-aligned world rectangle that bounds free space.
    pub world_bounds: WorldBounds,
    /// Simple polygon obstacle rings; free space is the world rectangle minus their strict interiors.
    pub obstacles: Vec<Polygon>,
}

impl PolygonScene {
    /// Point free-space test: in bounds and not strictly inside any obstacle.
    ///
    /// Does **not** reject sealed world-boundary edges by itself; endpoint
    /// validation (`validate`, `validate_source`, `validate_goal`) and
    /// [`Self::segment_is_walkable`] apply the sealed-edge rule.
    #[must_use]
    pub fn is_walkable(&self, point: Point2) -> bool {
        self.world_bounds.contains(point)
            && !self
                .obstacles
                .iter()
                .any(|obstacle| obstacle.contains_point_strict(point))
    }

    /// Whether the segment from `start` to `end` stays in free space.
    ///
    /// Rejects either endpoint when non-traversable (including sealed boundary),
    /// then samples every obstacle-edge intersection parameter and each open
    /// sub-interval midpoint. Visibility graphs and TFS use this as the edge
    /// legality predicate; cost is always Euclidean length of accepted segments.
    #[must_use]
    pub fn segment_is_walkable(&self, start: Point2, end: Point2) -> bool {
        if !point_is_traversable(self, start) || !point_is_traversable(self, end) {
            return false;
        }

        let mut parameters = vec![0.0, 1.0];
        for obstacle in &self.obstacles {
            for (edge_start, edge_end) in polygon_edges(obstacle.vertices()) {
                parameters.extend(segment_intersection_parameters(
                    start, end, edge_start, edge_end,
                ));
            }
        }

        sort_and_dedup_parameters(&mut parameters);

        for parameter in &parameters {
            let point = interpolate_segment(start, end, *parameter);
            if !point_is_traversable(self, point) {
                return false;
            }
        }

        for interval in parameters.windows(2) {
            let start_parameter = interval[0];
            let end_parameter = interval[1];
            if end_parameter - start_parameter <= EPSILON {
                continue;
            }

            let midpoint = interpolate_segment(start, end, (start_parameter + end_parameter) / 2.0);
            if !point_is_traversable(self, midpoint) {
                return false;
            }
        }

        true
    }

    /// Validates world bounds, obstacle geometry, and pairwise obstacle disjointness.
    ///
    /// # Errors
    ///
    /// Returns a [`PolygonValidationError`] for invalid bounds, obstacle
    /// geometry, or intersecting obstacles.
    pub fn validate_static(&self) -> Result<(), PolygonValidationError> {
        self.world_bounds.validate()?;

        for (obstacle_index, obstacle) in self.obstacles.iter().enumerate() {
            obstacle.validate_at(obstacle_index, self.world_bounds)?;
        }

        validate_obstacle_disjointness(&self.obstacles)?;

        Ok(())
    }

    /// Validates static scene invariants and that `source` is traversable.
    ///
    /// # Errors
    ///
    /// Returns a [`PolygonValidationError`] when static geometry or the source
    /// endpoint is invalid.
    pub fn validate_source(&self, source: Point2) -> Result<(), PolygonValidationError> {
        self.validate_static()?;
        validate_traversable_endpoint(self, source, PolygonEndpoint::Source)
    }

    /// Validates static scene invariants and that `goal` is traversable.
    ///
    /// # Errors
    ///
    /// Returns a [`PolygonValidationError`] when static geometry or the goal
    /// endpoint is invalid.
    pub fn validate_goal(&self, goal: Point2) -> Result<(), PolygonValidationError> {
        self.validate_static()?;
        validate_traversable_endpoint(self, goal, PolygonEndpoint::Goal)
    }

    /// Validates static scene invariants and that both endpoints are traversable.
    ///
    /// # Errors
    ///
    /// Returns a [`PolygonValidationError`] when static geometry or either
    /// endpoint is invalid.
    pub fn validate(&self, request: PolygonSearchRequest) -> Result<(), PolygonValidationError> {
        self.validate_static()?;
        validate_traversable_endpoint(self, request.start, PolygonEndpoint::Start)?;
        validate_traversable_endpoint(self, request.goal, PolygonEndpoint::Goal)?;
        Ok(())
    }
}

fn polygon_edges(vertices: &[Point2]) -> impl Iterator<Item = (Point2, Point2)> + '_ {
    vertices
        .iter()
        .copied()
        .zip(vertices.iter().copied().cycle().skip(1))
        .take(vertices.len())
}

fn sort_and_dedup_parameters(parameters: &mut Vec<f64>) {
    parameters.sort_by(f64::total_cmp);
    parameters.dedup_by(|left, right| (*left - *right).abs() <= EPSILON);
}

fn interpolate_segment(start: Point2, end: Point2, parameter: f64) -> Point2 {
    Point2::new(
        start.x + ((end.x - start.x) * parameter),
        start.y + ((end.y - start.y) * parameter),
    )
}

fn segment_intersection_parameters(
    a_start: Point2,
    a_end: Point2,
    b_start: Point2,
    b_end: Point2,
) -> Vec<f64> {
    let mut parameters = Vec::with_capacity(2);
    for point in [a_start, a_end, b_start, b_end] {
        if point_on_segment(point, a_start, a_end) && point_on_segment(point, b_start, b_end) {
            parameters.push(segment_parameter(point, a_start, a_end));
        }
    }

    if !parameters.is_empty() {
        sort_and_dedup_parameters(&mut parameters);
        return parameters;
    }

    if let Some(parameter) = proper_intersection_parameter(a_start, a_end, b_start, b_end) {
        parameters.push(parameter);
    }

    parameters
}

fn segment_parameter(point: Point2, start: Point2, end: Point2) -> f64 {
    let dx = end.x - start.x;
    let dy = end.y - start.y;
    if dx.abs() >= dy.abs() && dx.abs() > EPSILON {
        ((point.x - start.x) / dx).clamp(0.0, 1.0)
    } else if dy.abs() > EPSILON {
        ((point.y - start.y) / dy).clamp(0.0, 1.0)
    } else {
        0.0
    }
}

fn proper_intersection_parameter(
    a_start: Point2,
    a_end: Point2,
    b_start: Point2,
    b_end: Point2,
) -> Option<f64> {
    let o1 = orientation(a_start, a_end, b_start);
    let o2 = orientation(a_start, a_end, b_end);
    let o3 = orientation(b_start, b_end, a_start);
    let o4 = orientation(b_start, b_end, a_end);

    let properly_crosses = (o1 > EPSILON && o2 < -EPSILON || o1 < -EPSILON && o2 > EPSILON)
        && (o3 > EPSILON && o4 < -EPSILON || o3 < -EPSILON && o4 > EPSILON);
    if !properly_crosses {
        return None;
    }

    let a_dx = a_end.x - a_start.x;
    let a_dy = a_end.y - a_start.y;
    let b_dx = b_end.x - b_start.x;
    let b_dy = b_end.y - b_start.y;
    let denominator = cross(a_dx, a_dy, b_dx, b_dy);
    if denominator.abs() <= EPSILON {
        return None;
    }

    let offset_x = b_start.x - a_start.x;
    let offset_y = b_start.y - a_start.y;
    Some((cross(offset_x, offset_y, b_dx, b_dy) / denominator).clamp(0.0, 1.0))
}

fn validate_obstacle_disjointness(obstacles: &[Polygon]) -> Result<(), PolygonValidationError> {
    for (left_index, left) in obstacles.iter().enumerate() {
        for (right_index, right) in obstacles.iter().enumerate().skip(left_index + 1) {
            if polygons_intersect_or_overlap(left, right) {
                return Err(PolygonValidationError::ObstaclesOverlap {
                    left_index,
                    right_index,
                });
            }
        }
    }

    Ok(())
}

fn polygons_intersect_or_overlap(left: &Polygon, right: &Polygon) -> bool {
    polygon_edges(left.vertices()).any(|left_edge| {
        polygon_edges(right.vertices()).any(|right_edge| {
            segments_intersect(left_edge.0, left_edge.1, right_edge.0, right_edge.1)
        })
    }) || left
        .vertices()
        .iter()
        .copied()
        .any(|vertex| right.contains_point_strict(vertex))
        || right
            .vertices()
            .iter()
            .copied()
            .any(|vertex| left.contains_point_strict(vertex))
}

fn point_is_traversable(scene: &PolygonScene, point: Point2) -> bool {
    scene.world_bounds.contains(point)
        && !scene
            .obstacles
            .iter()
            .any(|obstacle| obstacle.contains_point_strict(point))
        && !point_on_sealed_boundary(scene, point)
}

fn validate_traversable_endpoint(
    scene: &PolygonScene,
    point: Point2,
    endpoint: PolygonEndpoint,
) -> Result<(), PolygonValidationError> {
    if !point_is_traversable(scene, point) {
        return Err(PolygonValidationError::EndpointNotTraversable { endpoint, point });
    }

    Ok(())
}

fn point_on_sealed_boundary(scene: &PolygonScene, point: Point2) -> bool {
    scene.obstacles.iter().any(|obstacle| {
        polygon_edges(obstacle.vertices()).any(|(start, end)| {
            point_on_segment(point, start, end)
                && edge_lies_on_world_boundary(start, end, scene.world_bounds)
        })
    })
}

fn edge_lies_on_world_boundary(start: Point2, end: Point2, bounds: WorldBounds) -> bool {
    ((start.x - bounds.min.x).abs() <= EPSILON && (end.x - bounds.min.x).abs() <= EPSILON)
        || ((start.x - bounds.max.x).abs() <= EPSILON && (end.x - bounds.max.x).abs() <= EPSILON)
        || ((start.y - bounds.min.y).abs() <= EPSILON && (end.y - bounds.min.y).abs() <= EPSILON)
        || ((start.y - bounds.max.y).abs() <= EPSILON && (end.y - bounds.max.y).abs() <= EPSILON)
}

fn duplicate_vertex_indices(vertices: &[Point2]) -> Option<(usize, usize)> {
    for (index, vertex) in vertices.iter().enumerate() {
        if let Some(second_index) = vertices
            .iter()
            .enumerate()
            .skip(index + 1)
            .find_map(|(second_index, other)| points_equal(*vertex, *other).then_some(second_index))
        {
            return Some((index, second_index));
        }
    }

    None
}

fn self_intersection_edge_indices(vertices: &[Point2]) -> Option<(usize, usize)> {
    let edge_count = vertices.len();
    for first_index in 0..edge_count {
        let first = (
            vertices[first_index],
            vertices[(first_index + 1) % edge_count],
        );
        for second_index in (first_index + 1)..edge_count {
            if edges_are_adjacent(first_index, second_index, edge_count) {
                continue;
            }

            let second = (
                vertices[second_index],
                vertices[(second_index + 1) % edge_count],
            );
            if segments_intersect(first.0, first.1, second.0, second.1) {
                return Some((first_index, second_index));
            }
        }
    }

    None
}

fn edges_are_adjacent(first_index: usize, second_index: usize, edge_count: usize) -> bool {
    first_index == second_index
        || (first_index + 1) % edge_count == second_index
        || (second_index + 1) % edge_count == first_index
}

fn segments_intersect(a_start: Point2, a_end: Point2, b_start: Point2, b_end: Point2) -> bool {
    let a_start_on_b = point_on_segment(a_start, b_start, b_end);
    let a_end_on_b = point_on_segment(a_end, b_start, b_end);
    let b_start_on_a = point_on_segment(b_start, a_start, a_end);
    let b_end_on_a = point_on_segment(b_end, a_start, a_end);
    if a_start_on_b || a_end_on_b || b_start_on_a || b_end_on_a {
        return true;
    }

    let o1 = orientation(a_start, a_end, b_start);
    let o2 = orientation(a_start, a_end, b_end);
    let o3 = orientation(b_start, b_end, a_start);
    let o4 = orientation(b_start, b_end, a_end);

    (o1 > EPSILON && o2 < -EPSILON || o1 < -EPSILON && o2 > EPSILON)
        && (o3 > EPSILON && o4 < -EPSILON || o3 < -EPSILON && o4 > EPSILON)
}

fn orientation(start: Point2, end: Point2, point: Point2) -> f64 {
    ((end.x - start.x) * (point.y - start.y)) - ((end.y - start.y) * (point.x - start.x))
}

fn cross(ax: f64, ay: f64, bx: f64, by: f64) -> f64 {
    (ax * by) - (ay * bx)
}

fn points_equal(left: Point2, right: Point2) -> bool {
    (left.x - right.x).abs() <= EPSILON && (left.y - right.y).abs() <= EPSILON
}

fn point_on_segment(point: Point2, start: Point2, end: Point2) -> bool {
    let cross =
        ((point.y - start.y) * (end.x - start.x)) - ((point.x - start.x) * (end.y - start.y));
    if cross.abs() > EPSILON {
        return false;
    }

    let dot = ((point.x - start.x) * (end.x - start.x)) + ((point.y - start.y) * (end.y - start.y));
    if dot < -EPSILON {
        return false;
    }

    let length_sq =
        ((end.x - start.x) * (end.x - start.x)) + ((end.y - start.y) * (end.y - start.y));
    dot <= length_sq + EPSILON
}

#[cfg(test)]
mod tests {
    use super::{
        Point2, Polygon, PolygonEndpoint, PolygonScene, PolygonSearchRequest,
        PolygonValidationError, WorldBounds,
    };

    #[test]
    fn scene_validation_reports_the_failing_obstacle_index() {
        let scene = PolygonScene {
            world_bounds: WorldBounds::new(Point2::new(0.0, 0.0), Point2::new(10.0, 10.0)),
            obstacles: vec![
                Polygon::new(vec![
                    Point2::new(1.0, 1.0),
                    Point2::new(2.0, 1.0),
                    Point2::new(1.0, 2.0),
                ]),
                Polygon::new(vec![Point2::new(4.0, 4.0), Point2::new(5.0, 4.0)]),
            ],
        };

        assert_eq!(
            scene.validate_static(),
            Err(PolygonValidationError::TooFewVertices {
                obstacle_index: 1,
                actual: 2,
            })
        );
    }

    #[test]
    fn polygon_validation_reports_duplicate_vertex_indices() {
        let polygon = Polygon::new(vec![
            Point2::new(1.0, 1.0),
            Point2::new(4.0, 1.0),
            Point2::new(4.0, 4.0),
            Point2::new(1.0, 1.0),
        ]);

        assert_eq!(
            polygon.validate(WorldBounds::new(
                Point2::new(0.0, 0.0),
                Point2::new(10.0, 10.0),
            )),
            Err(PolygonValidationError::DuplicateVertices {
                obstacle_index: 0,
                first_vertex_index: 0,
                second_vertex_index: 3,
            })
        );
    }

    #[test]
    fn polygon_validation_reports_intersecting_edge_indices() {
        let polygon = Polygon::new(vec![
            Point2::new(2.0, 7.0),
            Point2::new(4.0, 2.0),
            Point2::new(8.0, 7.0),
            Point2::new(2.0, 4.0),
            Point2::new(8.0, 4.0),
        ]);

        assert_eq!(
            polygon.validate(WorldBounds::new(
                Point2::new(0.0, 0.0),
                Point2::new(10.0, 10.0),
            )),
            Err(PolygonValidationError::SelfIntersection {
                obstacle_index: 0,
                first_edge_start_index: 0,
                second_edge_start_index: 2,
            })
        );
    }

    #[test]
    fn rejects_requests_that_start_on_a_sealed_boundary_edge() {
        let scene = sealed_boundary_scene();
        let request = PolygonSearchRequest::new(Point2::new(5.0, 0.0), Point2::new(2.0, 5.0));

        assert_eq!(
            scene.validate(request),
            Err(PolygonValidationError::EndpointNotTraversable {
                endpoint: PolygonEndpoint::Start,
                point: Point2::new(5.0, 0.0),
            })
        );
    }

    #[test]
    fn rejects_sources_that_start_on_a_sealed_boundary_edge() {
        assert_eq!(
            sealed_boundary_scene().validate_source(Point2::new(5.0, 0.0)),
            Err(PolygonValidationError::EndpointNotTraversable {
                endpoint: PolygonEndpoint::Source,
                point: Point2::new(5.0, 0.0),
            })
        );
    }

    #[test]
    fn rejects_goals_inside_an_obstacle_for_repeated_query_validation() {
        let scene = PolygonScene {
            world_bounds: WorldBounds::new(Point2::new(0.0, 0.0), Point2::new(10.0, 10.0)),
            obstacles: vec![Polygon::new(vec![
                Point2::new(4.0, 4.0),
                Point2::new(6.0, 4.0),
                Point2::new(6.0, 6.0),
                Point2::new(4.0, 6.0),
            ])],
        };

        assert_eq!(
            scene.validate_goal(Point2::new(5.0, 5.0)),
            Err(PolygonValidationError::EndpointNotTraversable {
                endpoint: PolygonEndpoint::Goal,
                point: Point2::new(5.0, 5.0),
            })
        );
    }

    #[test]
    fn rejects_segments_that_try_to_slide_along_a_sealed_boundary_edge() {
        let scene = sealed_boundary_scene();
        assert!(!scene.segment_is_walkable(Point2::new(4.5, 0.0), Point2::new(5.5, 0.0)));
    }

    fn sealed_boundary_scene() -> PolygonScene {
        PolygonScene {
            world_bounds: WorldBounds::new(Point2::new(0.0, 0.0), Point2::new(10.0, 10.0)),
            obstacles: vec![Polygon::new(vec![
                Point2::new(4.0, 0.0),
                Point2::new(6.0, 0.0),
                Point2::new(6.0, 10.0),
                Point2::new(4.0, 10.0),
            ])],
        }
    }
}