condor_navmesh/algorithms/
channel_search.rs

1//! Static [`NavmeshPathfinder`]: cell-corridor BFS + funnel string-pull.
2//!
3//! # Surface
4//!
5//! One-shot over a validated static [`Navmesh`]. No
6//! preprocess, no cross-query cache. For multi-query prepared routing use TRA*
7//! builders in [`super::tra_star`]. For availability overlays, materialize a
8//! static snapshot first—this solver never reads [`DynamicNavmeshState`](crate::DynamicNavmeshState).
9//!
10//! # Cost and behavior
11//!
12//! **Pipeline**: connectivity precheck → BFS cell corridor → portal chain →
13//! midpoint seeds → [`pull_string`](crate::navmesh::funnel::pull_string) → walkability check.
14//!
15//! **Cost / stats**: returned path length is Euclidean geometric (no per-cell
16//! weights); `visited_nodes` counts BFS cell expansions.
17
18use std::collections::VecDeque;
19
20use crate::navmesh::points_equal;
21use crate::{
22    Navmesh, NavmeshPathfinder, NavmeshQuery, NavmeshQueryResult, NavmeshSearchResult, Point2,
23    PolygonPath,
24};
25
26/// Stateless corridor-BFS pathfinder with portal-midpoint funnel seeding.
27///
28/// Prefer when each query is independent and preprocess cost is not amortized.
29/// Invalid endpoints map to [`NavmeshSearchError`](crate::NavmeshSearchError);
30/// disconnected cell graphs yield no-path with zero expansions when the
31/// connectivity precheck fails.
32#[derive(Debug, Clone, Copy, Default, PartialEq, Eq)]
33pub struct ChannelSearch;
34
35impl NavmeshPathfinder for ChannelSearch {
36    fn name(&self) -> &'static str {
37        "channel-search"
38    }
39
40    fn search(&self, navmesh: &Navmesh, query: NavmeshQuery) -> NavmeshSearchResult {
41        let (start_cell, goal_cell) = match navmesh.query(query) {
42            NavmeshQueryResult::Connected {
43                start_cell,
44                goal_cell,
45            } => (start_cell, goal_cell),
46            NavmeshQueryResult::InvalidStart => {
47                return Err(crate::NavmeshSearchError::InvalidStart { point: query.start });
48            }
49            NavmeshQueryResult::InvalidGoal => {
50                return Err(crate::NavmeshSearchError::InvalidGoal { point: query.goal });
51            }
52            NavmeshQueryResult::NoPath { .. } => return crate::navmesh::search_not_found(0),
53        };
54
55        if points_equal(query.start, query.goal) {
56            return crate::navmesh::search_found(
57                PolygonPath::from_points(vec![query.start])
58                    .expect("polygon path contains at least one point"),
59                1,
60            );
61        }
62
63        let (Some(cells), visited_nodes) =
64            search_cell_corridor(navmesh, start_cell, goal_cell, query.budget)?
65        else {
66            return crate::navmesh::search_not_found(0);
67        };
68
69        let Some(corridor) = crate::navmesh::corridor::NavmeshCorridor::from_cells(
70            navmesh,
71            query.start,
72            query.goal,
73            &cells,
74        ) else {
75            return crate::navmesh::search_not_found(visited_nodes);
76        };
77
78        let initial_points = corridor
79            .portals
80            .iter()
81            .map(portal_midpoint)
82            .collect::<Vec<_>>();
83        let adapted_points =
84            crate::navmesh::funnel::pull_string(navmesh, &corridor, initial_points);
85
86        if adapted_points.len() >= 2 && !navmesh.path_is_walkable(&adapted_points) {
87            return crate::navmesh::search_not_found(visited_nodes);
88        }
89
90        crate::navmesh::search_found(
91            PolygonPath::from_points(adapted_points)
92                .expect("polygon path contains at least one point"),
93            visited_nodes,
94        )
95    }
96}
97
98/// Budgeted BFS over cell adjacency; returns a cell sequence or no corridor.
99pub(crate) fn search_cell_corridor(
100    navmesh: &Navmesh,
101    start_cell: usize,
102    goal_cell: usize,
103    budget: condor_core::SearchBudget,
104) -> Result<(Option<Vec<usize>>, usize), crate::NavmeshSearchError> {
105    let cell_count = navmesh.cells().len();
106    if start_cell >= cell_count || goal_cell >= cell_count {
107        return Ok((None, 0));
108    }
109
110    let mut seen = vec![false; cell_count];
111    let mut parents = vec![None; cell_count];
112    let mut frontier = VecDeque::from([start_cell]);
113    let mut visited_nodes = 0;
114    let watch = condor_core::BudgetWatch::start(budget);
115
116    seen[start_cell] = true;
117    parents[start_cell] = Some(start_cell);
118
119    while let Some(cell_index) = frontier.pop_front() {
120        visited_nodes += 1;
121        if cell_index == goal_cell {
122            return Ok((
123                reconstruct_cell_path(&parents, start_cell, goal_cell),
124                visited_nodes,
125            ));
126        }
127
128        watch.check(visited_nodes)?;
129
130        let mut neighbors = navmesh.neighbors(cell_index);
131        neighbors.sort_unstable();
132        for neighbor in neighbors {
133            if neighbor >= seen.len() || seen[neighbor] {
134                continue;
135            }
136
137            seen[neighbor] = true;
138            parents[neighbor] = Some(cell_index);
139            frontier.push_back(neighbor);
140        }
141    }
142
143    Ok((None, visited_nodes))
144}
145
146fn reconstruct_cell_path(
147    parents: &[Option<usize>],
148    start_cell: usize,
149    goal_cell: usize,
150) -> Option<Vec<usize>> {
151    let mut cells = vec![goal_cell];
152    let mut current = goal_cell;
153
154    while current != start_cell {
155        let parent = parents[current]?;
156        cells.push(parent);
157        current = parent;
158    }
159
160    cells.reverse();
161    Some(cells)
162}
163
164/// Midpoint of a portal segment used as the default funnel seed.
165pub(crate) fn portal_midpoint(portal: &crate::NavmeshPortal) -> Point2 {
166    Point2::new(
167        (portal.start.x + portal.end.x) / 2.0,
168        (portal.start.y + portal.end.y) / 2.0,
169    )
170}
condor_navmesh/algorithms/channel_search.rs

condor_navmesh/algorithms/
channel_search.rs
