condor-pathfinding-navmesh 0.4.0

//! Optional Polyanya-backed [`NavmeshPathfinder`] (`feature = "polyanya"`).
//!
//! # Surface
//!
//! Static one-shot over a validated [`Navmesh`]—same
//! [`NavmeshPathfinder`] contract as channel search /
//! TA*. Not a prepared preprocess/query builder (use TRA* for multi-query
//! prepared routing) and not a dynamic availability consumer.
//!
//! Adapts Condor's convex-cell mesh into the external Polyanya representation,
//! runs the library path query, maps the triangle corridor back to Condor cells,
//! then string-pulls for a walkable free-space polyline.
//!
//! # Pipeline
//!
//! 1. Connectivity precheck via [`Navmesh::query`]
//! 2. [`PolyanyaMeshAdapter`] conversion + bake
//! 3. External path → cell corridor
//! 4. [`pull_string`](crate::navmesh::funnel::pull_string) funnel
//! 5. Final [`Navmesh::path_is_walkable`] check

use glam::Vec2;

use crate::navmesh::adapter::{PolyanyaMeshAdapter, points_equal, to_external_vec2};
use crate::{
    Navmesh, NavmeshPathfinder, NavmeshQuery, NavmeshQueryResult, NavmeshSearchResult, Point2,
    PolygonPath,
};

/// [`NavmeshPathfinder`] that routes through Polyanya then Condor string-pulling.
///
/// **Role**: exact-style continuous path on a static navmesh cell graph.
/// **Cost**: Euclidean polyline length after funneling.
///
/// Mesh-adaptation failures are returned as
/// [`NavmeshSearchError::PolyanyaMeshAdapter`](crate::navmesh::NavmeshSearchError::PolyanyaMeshAdapter),
/// preserving the underlying [`PolyanyaMeshAdapterError`](crate::navmesh::adapter::PolyanyaMeshAdapterError).
/// Invalid start/goal (outside walkable cells) use the standard navmesh search
/// errors. Failed external path, corridor build, or post-funnel walkability
/// yields no-path with whatever expansion count was observed.
#[derive(Debug, Clone, Copy, Default, PartialEq, Eq)]
pub struct Polyanya;

impl NavmeshPathfinder for Polyanya {
    fn name(&self) -> &'static str {
        "polyanya"
    }

    /// Route `query` on `navmesh` through Polyanya + funnel validation.
    ///
    /// See the type-level docs for pipeline stages and error mapping.
    fn search(&self, navmesh: &Navmesh, query: NavmeshQuery) -> NavmeshSearchResult {
        match navmesh.query(query) {
            NavmeshQueryResult::InvalidStart => {
                return Err(crate::NavmeshSearchError::InvalidStart { point: query.start });
            }
            NavmeshQueryResult::InvalidGoal => {
                return Err(crate::NavmeshSearchError::InvalidGoal { point: query.goal });
            }
            NavmeshQueryResult::NoPath { .. } => return crate::navmesh::search_not_found(0),
            NavmeshQueryResult::Connected { .. } => {}
        }

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

        let watch = condor_core::BudgetWatch::start(query.budget);
        // Polyanya is an opaque external search; honor wall-clock before/after the call.
        // Expansion caps apply to Condor-owned expansion loops, not third-party internals.
        watch.check(0)?;

        let adapter = PolyanyaMeshAdapter::from_navmesh(navmesh)?;
        let triangle_to_cell = adapter.triangle_to_cell().to_vec();
        let mut mesh = adapter.into_external_mesh()?;
        mesh.bake();

        let Some(path) = mesh.path(to_external_vec2(query.start), to_external_vec2(query.goal))
        else {
            watch.check(0)?;
            return crate::navmesh::search_not_found(0);
        };

        watch.check(0)?;
        let visited_nodes = path.polygons().len();

        let mut cell_indices: Vec<usize> = path
            .polygons()
            .iter()
            .map(|&(_, triangle_idx)| triangle_to_cell[triangle_idx as usize])
            .collect();
        cell_indices.dedup();

        let Some(corridor) = crate::navmesh::corridor::NavmeshCorridor::from_cells(
            navmesh,
            query.start,
            query.goal,
            &cell_indices,
        ) else {
            return crate::navmesh::search_not_found(visited_nodes);
        };

        let initial_points = path.path.into_iter().map(from_external_vec2).collect();
        let adapted_points =
            crate::navmesh::funnel::pull_string(navmesh, &corridor, initial_points);

        if adapted_points.len() < 2 || !navmesh.path_is_walkable(&adapted_points) {
            return crate::navmesh::search_not_found(visited_nodes);
        }

        crate::navmesh::search_found(
            PolygonPath::from_points(adapted_points)
                .expect("polygon path contains at least one point"),
            visited_nodes,
        )
    }
}

fn from_external_vec2(point: Vec2) -> Point2 {
    Point2::new(f64::from(point.x), f64::from(point.y))
}