condor_geometry/

shortest_path_map.rs

//! Source-rooted shortest-path maps for repeated polygonal goal queries.
//!
//! # Lifecycle
//!
//! This is the prepared surface for continuous free space with a **fixed
//! source** (not the online pair-search [`crate::continuous::PolygonPathfinder`]
//! trait):
//!
//! 1. [`PolygonShortestPathMapBuilder::preprocess`] validates the source via
//!    [`PolygonScene::validate_source`](crate::polygonal::PolygonScene::validate_source)
//!    and builds a prepared map (visibility-graph distances from that source).
//! 2. [`PolygonShortestPathMap::query`] answers many goals without rebuilding
//!    the source-rooted structure, stitching the best terminal vertex to the
//!    goal with a final walkable segment.
//!
//! Cost and exactness match the online visibility-graph solver under the same
//! free-space predicates. Arbitrary changing start–goal pairs still use online
//! pathfinders; this API is many goals / one source only.

use std::{cmp::Ordering, collections::BinaryHeap};

use crate::continuous::{PolygonPath, PolygonSearchResult};
use crate::polygonal::{Point2, PolygonScene, PolygonValidationError};

const EPSILON: f64 = 1e-9;

/// Prepared map that answers shortest-path queries from a fixed source.
///
/// Built once via [`PolygonShortestPathMapBuilder`]; subsequent `query` calls
/// must use goals in the same scene geometry the map was prepared from.
pub trait PolygonShortestPathMap {
    /// Stable algorithm / builder identity for reports and portfolios.
    fn name(&self) -> &'static str;

    /// Scene-space source the map was rooted at during preprocess.
    fn source(&self) -> Point2;

    /// Shortest free-space path from [`Self::source`] to `goal`.
    ///
    /// Invalid goals (non-traversable free space, including sealed boundary)
    /// return [`PolygonSearchError::InvalidGoal`](crate::continuous::PolygonSearchError::InvalidGoal).
    /// Reachable goals return a Euclidean polyline; unreachable valid goals
    /// return no-path (`NoPath` outcome), not a validation error.
    fn query(&self, goal: Point2) -> PolygonSearchResult;
}

/// Preprocesses a polygon scene into a source-rooted shortest-path map.
///
/// Implementations own the expensive visibility / distance work so that
/// [`PolygonShortestPathMap::query`] stays cheap relative to online pair search.
pub trait PolygonShortestPathMapBuilder {
    /// Prepared map type produced by this builder.
    type Map: PolygonShortestPathMap;

    /// Stable builder identity (usually matches the prepared map name).
    fn name(&self) -> &'static str;

    /// Build a map rooted at `source` for repeated goal queries on `scene`.
    ///
    /// # Errors
    ///
    /// Returns [`PolygonShortestPathMapBuildError`] when static scene geometry
    /// or the source endpoint fails validation (including sealed-boundary sources).
    fn preprocess(
        &self,
        scene: &PolygonScene,
        source: Point2,
    ) -> Result<Self::Map, PolygonShortestPathMapBuildError>;
}

/// Preprocess failed before a prepared map could be produced.
#[derive(Debug, Clone, PartialEq, thiserror::Error)]
#[non_exhaustive]
pub enum PolygonShortestPathMapBuildError {
    /// Scene or source rejected by [`PolygonScene::validate_source`].
    #[error("invalid source-rooted polygon scene: {source}")]
    InvalidScene {
        #[from]
        source: PolygonValidationError,
    },
}

/// Visibility-graph builder for source-rooted continuous shortest-path maps.
///
/// Preprocess: nodes = `{source}` ∪ obstacle vertices; undirected walkable
/// edges with Euclidean cost; multi-target Dijkstra from the source.
/// Query: minimize `dist[node] + dist(node, goal)` over nodes visible to the goal.
#[derive(Debug, Clone, Copy, Default)]
pub struct ContinuousShortestPathMap;

/// Precomputed visibility-graph distances from one fixed source.
///
/// Owns a cloned [`PolygonScene`] snapshot plus node distances and predecessor
/// chains. Thread-safe for concurrent `query` if shared behind a shared reference
/// (no interior mutability).
#[derive(Debug, Clone)]
pub struct PreparedContinuousShortestPathMap {
    scene: PolygonScene,
    source: Point2,
    nodes: Vec<Point2>,
    distances: Vec<f64>,
    predecessors: Vec<Option<usize>>,
}

impl PolygonShortestPathMapBuilder for ContinuousShortestPathMap {
    type Map = PreparedContinuousShortestPathMap;

    fn name(&self) -> &'static str {
        "continuous-shortest-path-map"
    }

    fn preprocess(
        &self,
        scene: &PolygonScene,
        source: Point2,
    ) -> Result<Self::Map, PolygonShortestPathMapBuildError> {
        scene.validate_source(source)?;

        let nodes = collect_nodes(scene, source);
        let adjacency = build_visibility_edges(scene, &nodes);
        let (distances, predecessors) = shortest_paths_from_source(&adjacency, 0);

        Ok(PreparedContinuousShortestPathMap {
            scene: scene.clone(),
            source,
            nodes,
            distances,
            predecessors,
        })
    }
}

impl PolygonShortestPathMap for PreparedContinuousShortestPathMap {
    fn name(&self) -> &'static str {
        "continuous-shortest-path-map"
    }

    fn source(&self) -> Point2 {
        self.source
    }

    fn query(&self, goal: Point2) -> PolygonSearchResult {
        if points_equal(self.source, goal) {
            return crate::continuous::found(
                PolygonPath::from_points(vec![self.source])
                    .expect("polygon path contains at least one point"),
                1,
            );
        }

        if self.scene.validate_goal(goal).is_err() {
            return Err(crate::continuous::PolygonSearchError::InvalidGoal { point: goal });
        }

        let mut visited_nodes = 0usize;
        let mut best_terminal = None;
        let mut best_cost = f64::INFINITY;

        for (node_index, &node) in self.nodes.iter().enumerate() {
            visited_nodes += 1;

            if !self.distances[node_index].is_finite()
                || !self.scene.segment_is_walkable(node, goal)
            {
                continue;
            }

            let candidate_cost = self.distances[node_index] + node.distance_to(goal);
            if candidate_cost + EPSILON < best_cost {
                best_cost = candidate_cost;
                best_terminal = Some(node_index);
            }
        }

        match best_terminal {
            Some(node_index) => {
                let mut points = reconstruct_path(&self.nodes, &self.predecessors, node_index);
                if !points_equal(
                    *points.last().expect("source-rooted path must be non-empty"),
                    goal,
                ) {
                    points.push(goal);
                }

                crate::continuous::found(
                    PolygonPath::from_points_with_cost(points, best_cost)
                        .expect("polygon path contains at least one point"),
                    visited_nodes,
                )
            }
            None => crate::continuous::not_found(visited_nodes),
        }
    }
}

fn collect_nodes(scene: &PolygonScene, source: Point2) -> Vec<Point2> {
    let mut nodes = vec![source];
    for obstacle in &scene.obstacles {
        for &vertex in obstacle.vertices() {
            if !nodes.iter().any(|point| points_equal(*point, vertex)) {
                nodes.push(vertex);
            }
        }
    }
    nodes
}

fn build_visibility_edges(scene: &PolygonScene, nodes: &[Point2]) -> Vec<Vec<(usize, f64)>> {
    let mut adjacency = vec![Vec::new(); nodes.len()];

    for left_index in 0..nodes.len() {
        for right_index in (left_index + 1)..nodes.len() {
            let start = nodes[left_index];
            let end = nodes[right_index];
            if scene.segment_is_walkable(start, end) {
                let cost = start.distance_to(end);
                adjacency[left_index].push((right_index, cost));
                adjacency[right_index].push((left_index, cost));
            }
        }
    }

    adjacency
}

fn shortest_paths_from_source(
    adjacency: &[Vec<(usize, f64)>],
    source_index: usize,
) -> (Vec<f64>, Vec<Option<usize>>) {
    let mut distances = vec![f64::INFINITY; adjacency.len()];
    let mut predecessors = vec![None; adjacency.len()];
    let mut closed = vec![false; adjacency.len()];
    let mut frontier = BinaryHeap::new();

    distances[source_index] = 0.0;
    frontier.push(HeapEntry {
        node_index: source_index,
        cost: 0.0,
    });

    while let Some(entry) = frontier.pop() {
        if closed[entry.node_index] {
            continue;
        }

        closed[entry.node_index] = true;

        for &(neighbor_index, edge_cost) in &adjacency[entry.node_index] {
            if closed[neighbor_index] {
                continue;
            }

            let next_cost = entry.cost + edge_cost;
            if next_cost + EPSILON < distances[neighbor_index] {
                distances[neighbor_index] = next_cost;
                predecessors[neighbor_index] = Some(entry.node_index);
                frontier.push(HeapEntry {
                    node_index: neighbor_index,
                    cost: next_cost,
                });
            }
        }
    }

    (distances, predecessors)
}

fn reconstruct_path(
    nodes: &[Point2],
    predecessors: &[Option<usize>],
    goal_index: usize,
) -> Vec<Point2> {
    let mut reversed = Vec::new();
    let mut current = Some(goal_index);

    while let Some(index) = current {
        reversed.push(nodes[index]);
        current = predecessors[index];
    }

    reversed.reverse();
    reversed
}

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

#[derive(Debug, Clone, Copy, PartialEq)]
struct HeapEntry {
    node_index: usize,
    cost: f64,
}

impl Eq for HeapEntry {}

impl Ord for HeapEntry {
    fn cmp(&self, other: &Self) -> Ordering {
        other
            .cost
            .total_cmp(&self.cost)
            .then_with(|| other.node_index.cmp(&self.node_index))
    }
}

impl PartialOrd for HeapEntry {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

#[cfg(test)]
mod tests {
    use super::{ContinuousShortestPathMap, PolygonShortestPathMap, PolygonShortestPathMapBuilder};
    use crate::{
        continuous::PolygonPathfinder,
        polygonal::{Point2, Polygon, PolygonScene, PolygonSearchRequest, WorldBounds},
        visibility_graph::VisibilityGraph,
    };

    #[test]
    fn build_error_wraps_scene_validation_failures() {
        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, 0.0),
                Point2::new(6.0, 0.0),
                Point2::new(6.0, 10.0),
                Point2::new(4.0, 10.0),
            ])],
        };
        let builder = ContinuousShortestPathMap;

        let error = builder
            .preprocess(&scene, Point2::new(5.0, 0.0))
            .expect_err("sealed-boundary source should fail");

        assert_eq!(
            error,
            crate::shortest_path_map::PolygonShortestPathMapBuildError::InvalidScene {
                source: crate::polygonal::PolygonValidationError::EndpointNotTraversable {
                    endpoint: crate::polygonal::PolygonEndpoint::Source,
                    point: Point2::new(5.0, 0.0),
                },
            }
        );
    }

    #[test]
    fn source_rooted_map_surface_supports_repeated_goal_queries() {
        let scene = PolygonScene {
            world_bounds: WorldBounds::new(Point2::new(0.0, 0.0), Point2::new(10.0, 10.0)),
            obstacles: Vec::new(),
        };
        let builder = ContinuousShortestPathMap;
        let map = builder
            .preprocess(&scene, Point2::new(1.0, 1.0))
            .expect("shortest-path map should preprocess");

        assert_eq!(builder.name(), "continuous-shortest-path-map");
        assert_eq!(map.name(), "continuous-shortest-path-map");
        assert_eq!(map.source(), Point2::new(1.0, 1.0));

        let first = map.query(Point2::new(5.0, 1.0));
        let second = map.query(Point2::new(7.0, 4.0));

        assert!(first.as_ref().expect("valid search request").is_found());
        assert!(second.as_ref().expect("valid search request").is_found());
        assert_eq!(
            first
                .as_ref()
                .expect("valid search request")
                .path()
                .expect("path should be present")
                .points(),
            &[Point2::new(1.0, 1.0), Point2::new(5.0, 1.0)]
        );
        assert_eq!(
            second
                .as_ref()
                .expect("valid search request")
                .path()
                .expect("path should be present")
                .points(),
            &[Point2::new(1.0, 1.0), Point2::new(7.0, 4.0)]
        );
    }

    #[test]
    fn continuous_shortest_path_map_reuses_preprocessed_source_for_multiple_goals() {
        let scene = PolygonScene {
            world_bounds: WorldBounds::new(Point2::new(0.0, 0.0), Point2::new(12.0, 12.0)),
            obstacles: vec![Polygon::new(vec![
                Point2::new(4.0, 4.0),
                Point2::new(6.0, 4.0),
                Point2::new(6.0, 8.0),
                Point2::new(4.0, 8.0),
            ])],
        };
        let source = Point2::new(1.0, 1.0);
        let map = ContinuousShortestPathMap
            .preprocess(&scene, source)
            .expect("reusable shortest-path map should preprocess");
        let baseline = VisibilityGraph;

        for (goal, fixture_name) in [
            (Point2::new(10.0, 5.0), "multi-goal-bottom-detour"),
            (Point2::new(10.0, 10.0), "multi-goal-top-detour"),
        ] {
            let request = PolygonSearchRequest::new(source, goal);
            let result = map.query(goal).expect("test request should be valid");
            let baseline_result = baseline
                .search(&scene, request)
                .expect("test request should be valid");
            let path = result.path().expect("prepared map path should be found");
            let baseline_path = baseline_result
                .path()
                .expect("baseline path should be found");

            assert_eq!(path.points().first(), Some(&source), "{fixture_name}");
            assert_eq!(path.points().last(), Some(&goal), "{fixture_name}");
            assert!(
                path.points()
                    .windows(2)
                    .all(|pair| scene.segment_is_walkable(pair[0], pair[1])),
                "prepared map path should remain walkable for {fixture_name}"
            );
            assert!(
                (path.cost() - baseline_path.cost()).abs() <= 1e-9,
                "{} should match {} cost for the {} fixture",
                map.name(),
                baseline.name(),
                fixture_name
            );
        }
    }

    #[test]
    fn source_rooted_map_reports_no_path_for_separator_goal() {
        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, 0.0),
                Point2::new(6.0, 0.0),
                Point2::new(6.0, 10.0),
                Point2::new(4.0, 10.0),
            ])],
        };
        let map = ContinuousShortestPathMap
            .preprocess(&scene, Point2::new(2.0, 5.0))
            .expect("source should preprocess");

        let result = map.query(Point2::new(8.0, 5.0));

        assert!(!result.as_ref().expect("valid search request").is_found());
        assert!(
            result
                .as_ref()
                .expect("valid search request")
                .path()
                .is_none()
        );
        assert_eq!(result.as_ref().expect("valid search request").cost(), None);
    }
}