condor_grid/

any_angle.rs

//! Any-angle grid search contracts for continuous endpoints on a blocked [`Grid`].
//!
//! [`AnyAnglePathfinder`] implementations run one Euclidean search between mutually
//! visible grid vertices. Invalid endpoints return [`AnyAngleSearchError`]; valid
//! but disconnected endpoints return [`SearchOutcome::NoPath`]. Use [`crate::ThetaStar`],
//! [`crate::LazyThetaStar`], or the curated [`crate::Anya`] entrypoint for one query;
//! use [`crate::PreparedAnyAngleGrid`] when many exact queries share one static map.
//! Geometry helpers and exact-oracle internals stay private to this owner crate.

/// Private continuous-on-grid geometry helpers (LOS, vertex snap, edge costs).
pub(crate) mod geometry;

use std::{error::Error, fmt};

use condor_core::{BudgetExhausted, Point2, SearchBudget};

use crate::{
    grid::Grid,
    search::{SearchOutcome, SearchPathCost, SearchVisitStats},
};

/// Start and goal endpoints for an any-angle grid search.
///
/// Validity (grid-aligned free vertices) is checked at search time, not construction.
/// Optional [`SearchBudget`] caps expansions and/or wall-clock time; default is unlimited.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct AnyAngleSearchRequest {
    /// Continuous-space start (canonicalized to a grid vertex at search time).
    pub start: Point2,
    /// Continuous-space goal (canonicalized to a grid vertex at search time).
    pub goal: Point2,
    /// Optional expansion / wall-clock caps for this query (default unlimited).
    pub budget: SearchBudget,
}

impl AnyAngleSearchRequest {
    /// Pairs continuous start and goal with an unlimited budget; validity is checked at search time.
    #[must_use]
    pub fn new(start: Point2, goal: Point2) -> Self {
        Self {
            start,
            goal,
            budget: SearchBudget::UNLIMITED,
        }
    }

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

/// Non-empty polyline in grid coordinates with Euclidean traversal cost.
#[derive(Debug, Clone, PartialEq)]
pub struct AnyAnglePath {
    points: Vec<Point2>,
    cost: f64,
}

/// Error returned when any-angle path construction violates invariants.
#[derive(Debug, Clone, Copy, PartialEq, Eq, thiserror::Error)]
#[non_exhaustive]
pub enum AnyAnglePathBuildError {
    /// `from_points*` require at least one vertex.
    #[error("any-angle paths must contain at least one point")]
    Empty,
}

impl AnyAnglePath {
    /// Builds a path and sets cost to the sum of consecutive Euclidean segments.
    ///
    /// # Errors
    ///
    /// Returns [`AnyAnglePathBuildError::Empty`] when `points` is empty.
    pub fn from_points(points: Vec<Point2>) -> Result<Self, AnyAnglePathBuildError> {
        if points.is_empty() {
            return Err(AnyAnglePathBuildError::Empty);
        }
        let cost = points
            .windows(2)
            .map(|pair| pair[0].distance_to(pair[1]))
            .sum();
        Ok(Self { points, cost })
    }

    /// Builds a path with an explicit cost (callers must ensure consistency with geometry).
    ///
    /// # Errors
    ///
    /// Returns [`AnyAnglePathBuildError::Empty`] when `points` is empty.
    pub fn from_points_with_cost(
        points: Vec<Point2>,
        cost: f64,
    ) -> Result<Self, AnyAnglePathBuildError> {
        if points.is_empty() {
            return Err(AnyAnglePathBuildError::Empty);
        }
        Ok(Self { points, cost })
    }

    /// Ordered continuous vertices from start through goal (inclusive).
    #[must_use]
    pub fn points(&self) -> &[Point2] {
        &self.points
    }

    /// Vertex count (always ≥ 1 for constructed paths).
    #[must_use]
    pub fn len(&self) -> usize {
        self.points.len()
    }

    /// Always `false` for successfully constructed paths (`from_points*` reject empty).
    #[must_use]
    pub fn is_empty(&self) -> bool {
        self.points.is_empty()
    }

    /// First vertex.
    ///
    /// # Panics
    ///
    /// Panics if the path is empty (cannot occur for values built via `from_points*`).
    #[must_use]
    pub fn start(&self) -> Point2 {
        self.points[0]
    }

    /// Last vertex.
    ///
    /// # Panics
    ///
    /// Panics if the path is empty (cannot occur for values built via `from_points*`).
    #[must_use]
    pub fn goal(&self) -> Point2 {
        self.points[self.points.len() - 1]
    }

    /// Euclidean polyline length, or the explicit value from `from_points_with_cost`.
    #[must_use]
    pub const fn cost(&self) -> f64 {
        self.cost
    }
}

/// Work counters from an any-angle search (algorithm-defined node visits).
///
/// Semantics of `visited_nodes` vary by solver (vertex pops vs interval states vs
/// settled VG nodes) and must not be compared across algorithms without care.
#[derive(Debug, Clone, Copy, Default, PartialEq)]
pub struct AnyAngleSearchStats {
    /// Nodes the solver counts as expanded or settled (see algorithm docs).
    pub visited_nodes: usize,
}

/// Invalid any-angle search request or budget hard stop.
///
/// Unreachable but valid endpoints produce
/// [`SearchOutcome::NoPath`], not these variants. Budget exhaustion does not
/// prove unreachability.
#[non_exhaustive]
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum AnyAngleSearchError {
    /// Start is not a valid free-space position on the grid.
    InvalidStart { point: Point2 },
    /// Goal is not a valid free-space position on the grid.
    InvalidGoal { point: Point2 },
    /// Caller search budget was exhausted before found/no-path completed.
    BudgetExhausted(BudgetExhausted),
}

impl fmt::Display for AnyAngleSearchError {
    fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
        match self {
            Self::InvalidStart { point } => write!(formatter, "invalid any-angle start: {point:?}"),
            Self::InvalidGoal { point } => write!(formatter, "invalid any-angle goal: {point:?}"),
            Self::BudgetExhausted(reason) => write!(formatter, "{reason}"),
        }
    }
}

impl Error for AnyAngleSearchError {}

/// Validated result of an any-angle search.
pub type AnyAngleSearchResult =
    Result<SearchOutcome<AnyAnglePath, AnyAngleSearchStats>, AnyAngleSearchError>;

/// Builds a successful found outcome for owner-crate solvers.
pub(crate) const fn found(path: AnyAnglePath, visited_nodes: usize) -> AnyAngleSearchResult {
    Ok(SearchOutcome::found(
        path,
        AnyAngleSearchStats { visited_nodes },
    ))
}

/// Builds a completed no-path outcome (valid endpoints, no route).
pub(crate) const fn not_found(visited_nodes: usize) -> AnyAngleSearchResult {
    Ok(SearchOutcome::no_path(AnyAngleSearchStats {
        visited_nodes,
    }))
}

impl SearchPathCost for AnyAnglePath {
    type Cost = f64;

    fn path_cost(&self) -> Self::Cost {
        self.cost()
    }
}

impl SearchVisitStats for AnyAngleSearchStats {
    fn visited_nodes(&self) -> usize {
        self.visited_nodes
    }
}

/// Online any-angle algorithm entrypoint for static blocked grids.
///
/// Implementations search continuous coordinates with Euclidean edge costs between
/// mutually visible points (v0 no-corner-cut LOS, e.g. [`has_line_of_sight`]).
/// Invalid endpoints or exhausted [`SearchBudget`] return [`AnyAngleSearchError`];
/// unreachable but valid endpoints return [`SearchOutcome::NoPath`]. Prefer prepared
/// any-angle for repeated queries.
pub trait AnyAnglePathfinder {
    /// Stable algorithm identifier for benchmarks, logs, and solver portfolios.
    fn name(&self) -> &'static str;

    /// Runs one any-angle search on `grid` for `request`.
    ///
    /// Returns `Err` for invalid endpoints or exhausted budgets, `Ok(NoPath)` when
    /// no route exists, and `Ok(Found)` with a non-empty polyline when a route is found.
    fn search(&self, grid: &Grid, request: AnyAngleSearchRequest) -> AnyAngleSearchResult;
}

/// Maps a shared budget failure into [`AnyAngleSearchError::BudgetExhausted`].
#[doc(hidden)]
pub const fn budget_error(reason: BudgetExhausted) -> AnyAngleSearchError {
    AnyAngleSearchError::BudgetExhausted(reason)
}

/// Returns whether `start` and `end` are mutually visible on `grid`.
///
/// Coordinates are canonicalized to grid vertices. The check uses the v0
/// no-corner-cut geometry contract through an implementation independent from
/// the oracle's clipping predicate.
#[must_use]
pub fn has_line_of_sight(grid: &Grid, start: Point2, end: Point2) -> bool {
    geometry::sampling_segment_is_legal(grid, start, end)
}

/// Hidden exports for correctness tests and offline oracle capture.
#[doc(hidden)]
pub mod exact_oracle_v0 {
    pub use crate::algorithms::any_angle_visibility_graph::{
        AnyAngleOracleDiagnostics, AnyAngleSamplingReferenceOracle, AnyAngleVisibilityGraphOracle,
    };
    pub use crate::any_angle::geometry::{
        approximately_equal, canonicalize_grid_vertex, extract_boundary_edges, is_endpoint_valid,
        recompute_path_cost, retained_visibility_vertices, sampling_segment_is_legal,
        segment_is_legal, validate_path, validate_sampling_path,
    };
}