condor-pathfinding-geometry 0.4.0

//! Exact polygonal pathfinder using a visibility graph over scene vertices.
//!
//! # Surface
//!
//! Online [`crate::continuous::PolygonPathfinder`] baseline for continuous free
//! space: no prepared surface—the graph is rebuilt every query. Each search:
//!
//! 1. rejects non-walkable endpoints with typed errors;
//! 2. builds nodes = `{start, goal}` ∪ obstacle vertices;
//! 3. inserts undirected edges for every pair that passes
//!    [`PolygonScene::segment_is_walkable`] with Euclidean edge cost;
//! 4. runs Dijkstra from start to goal.
//!
//! # Cost and behavior
//!
//! Optimal under Condor's `f64` / epsilon walkability predicates (same cost
//! contract as TFS). Prefer TFS on sparse pillar forests; denser vertex sets
//! pay more here for all-pairs visibility tests. For many goals from one fixed
//! source, use [`crate::shortest_path_map`] instead of re-running this online.
//!
//! # Examples
//!
//! ```
//! use condor_geometry::{
//!     continuous::PolygonPathfinder,
//!     polygonal::{Point2, PolygonScene, PolygonSearchRequest, WorldBounds},
//!     visibility_graph::VisibilityGraph,
//! };
//!
//! let scene = PolygonScene {
//!     world_bounds: WorldBounds::new(Point2::new(0.0, 0.0), Point2::new(3.0, 3.0)),
//!     obstacles: Vec::new(),
//! };
//! let request = PolygonSearchRequest::new(Point2::new(0.5, 0.5), Point2::new(2.5, 2.5));
//! let result = VisibilityGraph.search(&scene, request).expect("request is valid");
//! assert!(result.is_found());
//! ```

use std::cmp::Ordering;
use std::collections::BinaryHeap;

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

const EPSILON: f64 = 1e-9;

/// Online exact continuous pathfinder: visibility graph + Dijkstra.
///
/// **Cost model**: Euclidean polyline length of free-space segments.
/// **Exactness**: optimal under Condor's `f64` / epsilon walkability predicates
/// (same contract as topological fracture search).
///
/// Invalid start/goal (non-walkable by [`PolygonScene::is_walkable`]) return
/// [`PolygonSearchError`](crate::continuous::PolygonSearchError). `start == goal` yields a
/// one-point zero-cost path. Static geometry failures or sealed endpoints that
/// pass the looser walkability check still produce no-path rather than panic.
#[derive(Debug, Clone, Copy, Default)]
pub struct VisibilityGraph;

impl PolygonPathfinder for VisibilityGraph {
    fn name(&self) -> &'static str {
        "visibility-graph"
    }

    /// Exact free-space path via per-query visibility graph + Dijkstra.
    ///
    /// See the type-level contract for cost, exactness, and endpoint errors.
    fn search(&self, scene: &PolygonScene, request: PolygonSearchRequest) -> PolygonSearchResult {
        if !scene.is_walkable(request.start) {
            return Err(crate::continuous::PolygonSearchError::InvalidStart {
                point: request.start,
            });
        }
        if !scene.is_walkable(request.goal) {
            return Err(crate::continuous::PolygonSearchError::InvalidGoal {
                point: request.goal,
            });
        }
        if scene.validate(request).is_err() {
            return crate::continuous::not_found(0);
        }

        if points_equal(request.start, request.goal) {
            return crate::continuous::found(
                PolygonPath::from_points(vec![request.start])
                    .expect("polygon path contains at least one point"),
                1,
            );
        }

        let nodes = collect_nodes(scene, request);
        let adjacency = build_visibility_edges(scene, &nodes);
        let (cost, predecessors, visited_nodes) =
            match shortest_path(&adjacency, 0, 1, request.budget) {
                Ok(outcome) => outcome,
                Err(reason) => return Err(crate::continuous::budget_error(reason)),
            };

        match cost {
            Some(goal_cost) => {
                let points = reconstruct_path(&nodes, &predecessors, 1);
                crate::continuous::found(
                    PolygonPath::from_points_with_cost(points, goal_cost)
                        .expect("polygon path contains at least one point"),
                    visited_nodes,
                )
            }
            None => crate::continuous::not_found(visited_nodes),
        }
    }
}

fn collect_nodes(scene: &PolygonScene, request: PolygonSearchRequest) -> Vec<Point2> {
    let mut nodes = vec![request.start, request.goal];
    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
}

type ShortestPathOutcome = (Option<f64>, Vec<Option<usize>>, usize);

fn shortest_path(
    adjacency: &[Vec<(usize, f64)>],
    start_index: usize,
    goal_index: usize,
    budget: condor_core::SearchBudget,
) -> Result<ShortestPathOutcome, condor_core::BudgetExhausted> {
    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();
    let mut visited_nodes = 0usize;
    let watch = condor_core::BudgetWatch::start(budget);

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

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

        closed[entry.node_index] = true;
        visited_nodes += 1;

        if entry.node_index == goal_index {
            return Ok((Some(entry.cost), predecessors, visited_nodes));
        }

        watch.check(visited_nodes)?;

        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,
                });
            }
        }
    }

    Ok((None, predecessors, visited_nodes))
}

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::VisibilityGraph;
    use crate::continuous::PolygonPathfinder;
    use crate::polygonal::{Point2, Polygon, PolygonScene, PolygonSearchRequest, WorldBounds};

    #[test]
    fn visibility_graph_finds_direct_path_in_open_space() {
        let scene = PolygonScene {
            world_bounds: WorldBounds::new(Point2::new(0.0, 0.0), Point2::new(10.0, 10.0)),
            obstacles: Vec::new(),
        };
        let request = PolygonSearchRequest::new(Point2::new(1.0, 1.0), Point2::new(9.0, 1.0));

        let result = VisibilityGraph.search(&scene, request);

        assert!(result.as_ref().expect("valid search request").is_found());
        let path = result
            .as_ref()
            .expect("valid search request")
            .path()
            .expect("path should be present");
        assert_eq!(path.points(), &[request.start, request.goal]);
        assert!((path.cost() - 8.0).abs() <= 1e-9);
    }

    #[test]
    fn visibility_graph_reports_no_path_for_separator() {
        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 request = PolygonSearchRequest::new(Point2::new(2.0, 5.0), Point2::new(8.0, 5.0));

        let result = VisibilityGraph.search(&scene, request);

        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);
    }
}