condor_geometry/
continuous.rs

1//! Continuous polygonal path types and the [`PolygonPathfinder`] search trait.
2//!
3//! Online exact solvers in this owner crate (visibility graph, TFS) share one
4//! contract: free-space geometry in a [`PolygonScene`], Euclidean polyline
5//! cost on [`PolygonPath`], and [`SearchOutcome`] found/no-path with
6//! [`PolygonSearchStats`]. Invalid free-space endpoints are
7//! [`PolygonSearchError`] (`Err`); unreachable but valid endpoints are
8//! `Ok(NoPath)`. Scene geometry validation
9//! ([`PolygonValidationError`](crate::polygonal::PolygonValidationError)) is a
10//! separate static/prep surface—not returned from `search` directly.
11//!
12//! # Prepared alternative
13//!
14//! Source-rooted repeated queries use prepared maps in
15//! [`crate::shortest_path_map`], not this trait.
16
17use crate::polygonal::{Point2, PolygonScene, PolygonSearchRequest};
18use condor_core::{BudgetExhausted, SearchOutcome, SearchPathCost, SearchVisitStats};
19use std::{error::Error, fmt};
20
21/// Polygon path construction failed (empty polyline or invariant violation).
22#[derive(Debug, Clone, Copy, PartialEq, Eq, thiserror::Error)]
23#[non_exhaustive]
24pub enum PolygonPathBuildError {
25    /// Caller supplied an empty vertex list; paths require at least one point.
26    #[error("polygon paths must contain at least one point")]
27    Empty,
28}
29
30/// Non-empty free-space polyline in scene coordinates with Euclidean cost.
31#[derive(Debug, Clone, PartialEq)]
32pub struct PolygonPath {
33    points: Vec<Point2>,
34    cost: f64,
35}
36
37impl PolygonPath {
38    /// Builds a path and sets cost to the sum of consecutive Euclidean segments.
39    ///
40    /// # Errors
41    ///
42    /// Returns [`PolygonPathBuildError::Empty`] when `points` is empty.
43    pub fn from_points(points: Vec<Point2>) -> Result<Self, PolygonPathBuildError> {
44        if points.is_empty() {
45            return Err(PolygonPathBuildError::Empty);
46        }
47        let cost = points
48            .windows(2)
49            .map(|pair| pair[0].distance_to(pair[1]))
50            .sum();
51        Ok(Self { points, cost })
52    }
53
54    /// Builds a path with an explicit cost (callers must ensure consistency with the polyline).
55    ///
56    /// # Errors
57    ///
58    /// Returns [`PolygonPathBuildError::Empty`] when `points` is empty.
59    pub fn from_points_with_cost(
60        points: Vec<Point2>,
61        cost: f64,
62    ) -> Result<Self, PolygonPathBuildError> {
63        if points.is_empty() {
64            return Err(PolygonPathBuildError::Empty);
65        }
66        Ok(Self { points, cost })
67    }
68
69    /// Ordered free-space vertices from start through goal (inclusive).
70    #[must_use]
71    pub fn points(&self) -> &[Point2] {
72        &self.points
73    }
74
75    /// Vertex count (always ≥ 1 for constructed paths).
76    #[must_use]
77    pub fn len(&self) -> usize {
78        self.points.len()
79    }
80
81    /// Always `false` for successfully constructed paths (`from_points*` reject empty).
82    #[must_use]
83    pub fn is_empty(&self) -> bool {
84        self.points.is_empty()
85    }
86
87    /// First vertex.
88    ///
89    /// # Panics
90    ///
91    /// Panics if the path is empty (cannot occur for values built via `from_points*`).
92    #[must_use]
93    pub fn start(&self) -> Point2 {
94        self.points
95            .first()
96            .copied()
97            .expect("polygon paths must contain at least one point")
98    }
99
100    /// Last vertex.
101    ///
102    /// # Panics
103    ///
104    /// Panics if the path is empty (cannot occur for values built via `from_points*`).
105    #[must_use]
106    pub fn goal(&self) -> Point2 {
107        self.points
108            .last()
109            .copied()
110            .expect("polygon paths must contain at least one point")
111    }
112
113    /// Euclidean polyline length, or the explicit value from `from_points_with_cost`.
114    #[must_use]
115    pub const fn cost(&self) -> f64 {
116        self.cost
117    }
118}
119
120/// Work counters for polygonal search (typically expanded graph vertices).
121///
122/// `visited_nodes` meaning is solver-specific (visibility nodes vs taut path
123/// keys); comparable within one algorithm, not across solvers.
124#[derive(Debug, Clone, Copy, Default, PartialEq)]
125pub struct PolygonSearchStats {
126    /// Algorithm-defined visit / expansion count for this search (lane-specific).
127    pub visited_nodes: usize,
128}
129
130/// Request rejected before search, or budget hard stop mid-search.
131///
132/// Pathfinder-level endpoint errors and [`Self::BudgetExhausted`]. Unreachable but
133/// walkable endpoints produce [`SearchOutcome::NoPath`], not these variants. Static
134/// scene defects use [`PolygonValidationError`](crate::polygonal::PolygonValidationError)
135/// on `validate*` / preprocess paths instead.
136#[non_exhaustive]
137#[derive(Debug, Clone, Copy, PartialEq)]
138pub enum PolygonSearchError {
139    /// Start fails [`PolygonScene::is_walkable`].
140    InvalidStart {
141        /// Off-free-space start that was rejected.
142        point: Point2,
143    },
144    /// Goal fails [`PolygonScene::is_walkable`].
145    InvalidGoal {
146        /// Off-free-space goal that was rejected.
147        point: Point2,
148    },
149    /// Caller search budget was exhausted before found/no-path completed.
150    BudgetExhausted(BudgetExhausted),
151}
152
153impl fmt::Display for PolygonSearchError {
154    fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
155        match self {
156            Self::InvalidStart { point } => write!(formatter, "invalid polygon start: {point:?}"),
157            Self::InvalidGoal { point } => write!(formatter, "invalid polygon goal: {point:?}"),
158            Self::BudgetExhausted(reason) => write!(formatter, "{reason}"),
159        }
160    }
161}
162
163impl Error for PolygonSearchError {}
164
165/// Polygonal search return type: validation / budget `Err`, or found/no-path with stats.
166///
167/// `Ok(Found)` and `Ok(NoPath)` are completed searches; `Err` is not a proof of
168/// unreachability (invalid endpoints or exhausted [`condor_core::SearchBudget`]).
169pub type PolygonSearchResult =
170    Result<SearchOutcome<PolygonPath, PolygonSearchStats>, PolygonSearchError>;
171
172/// Builds `Ok(Found)` with the given polyline and expansion stats.
173pub(crate) const fn found(path: PolygonPath, visited_nodes: usize) -> PolygonSearchResult {
174    Ok(SearchOutcome::found(
175        path,
176        PolygonSearchStats { visited_nodes },
177    ))
178}
179
180/// Builds `Ok(NoPath)` with expansion stats after exhaustive free-space search.
181pub(crate) const fn not_found(visited_nodes: usize) -> PolygonSearchResult {
182    Ok(SearchOutcome::no_path(PolygonSearchStats { visited_nodes }))
183}
184
185impl SearchPathCost for PolygonPath {
186    type Cost = f64;
187
188    fn path_cost(&self) -> Self::Cost {
189        self.cost()
190    }
191}
192
193impl SearchVisitStats for PolygonSearchStats {
194    fn visited_nodes(&self) -> usize {
195        self.visited_nodes
196    }
197}
198
199/// Online exact pathfinder over a static polygonal obstacle scene.
200///
201/// Pair-search surface for one start–goal query. Implementations reject
202/// endpoints that fail [`PolygonScene::is_walkable`] with [`PolygonSearchError`],
203/// honor optional [`condor_core::SearchBudget`] on the request, and otherwise return
204/// found/no-path with Euclidean polyline cost. Endpoints that pass the looser
205/// walkability check but fail full [`PolygonScene::validate`] (for example sealed
206/// boundary) typically yield `Ok(NoPath)` rather than `Err`—match the baseline
207/// solvers when implementing. For many goals from one fixed source, use
208/// [`crate::shortest_path_map`] instead of this online trait.
209pub trait PolygonPathfinder {
210    /// Stable algorithm identifier for benchmarks, logs, and solver portfolios.
211    fn name(&self) -> &'static str;
212
213    /// Runs one search on `scene` for `request`.
214    ///
215    /// Returns `Err` for non-walkable endpoints or exhausted budgets, `Ok(NoPath)`
216    /// when no free-space route exists (including some sealed-endpoint cases after
217    /// validation), and `Ok(Found)` with a non-empty polyline when a route is found.
218    ///
219    /// # Errors
220    ///
221    /// Returns [`PolygonSearchError`] when an endpoint is non-walkable or the caller
222    /// budget is exhausted before found/no-path completes.
223    fn search(&self, scene: &PolygonScene, request: PolygonSearchRequest) -> PolygonSearchResult;
224}
225
226/// Maps a shared budget failure into [`PolygonSearchError::BudgetExhausted`].
227#[doc(hidden)]
228pub const fn budget_error(reason: BudgetExhausted) -> PolygonSearchError {
229    PolygonSearchError::BudgetExhausted(reason)
230}
231
232#[cfg(test)]
233mod tests {
234    use super::PolygonPath;
235    use crate::polygonal::Point2;
236
237    #[test]
238    fn polygon_path_computes_euclidean_cost_from_points() {
239        let path = PolygonPath::from_points(vec![
240            Point2::new(1.0, 1.0),
241            Point2::new(4.0, 5.0),
242            Point2::new(7.0, 5.0),
243        ])
244        .expect("polygon path contains at least one point");
245
246        assert_eq!(path.start(), Point2::new(1.0, 1.0));
247        assert_eq!(path.goal(), Point2::new(7.0, 5.0));
248        assert!((path.cost() - 8.0).abs() <= 1e-9);
249        assert_eq!(path.len(), 3);
250        assert!(!path.is_empty());
251    }
252}
condor_geometry/continuous.rs

condor_geometry/
continuous.rs
