Skip to main content

rayengine_core/pathfinding/
astar.rs

1use super::{
2    NavGrid, Neighborhood, PathError, PathOptions, cell_count, cell_of, check_bounds, checked_cost,
3    corner_open, index, offset, step_length,
4};
5use glam::UVec2;
6use std::{cmp::Ordering, collections::BinaryHeap};
7
8/// Result of an A* call.
9#[derive(Clone, Copy, Debug, PartialEq)]
10pub enum PathStatus {
11    /// The output path holds the start, every cell stepped through, and the
12    /// goal. `cost` is the summed cost of entering each cell after the start.
13    Found {
14        /// Total path cost.
15        cost: f32,
16    },
17    /// No path exists; the output path is empty.
18    Unreachable,
19    /// The expansion budget ran out. Call [`PathFinder::resume`] with the same
20    /// grid to continue; the output path is empty until the search finishes.
21    Pending,
22}
23
24#[derive(Clone, Copy, Debug, Default)]
25struct Node {
26    // Nodes from earlier searches have older stamps and count as unvisited.
27    stamp: u32,
28    closed: bool,
29    g: f32,
30    parent: u32,
31}
32
33#[derive(Clone, Copy, Debug)]
34struct Open {
35    f: f32,
36    g: f32,
37    index: u32,
38}
39
40impl Ord for Open {
41    // Reversed for a min-heap: lowest estimate, then nearest to the goal
42    // (largest cost so far), then lowest cell index.
43    fn cmp(&self, other: &Self) -> Ordering {
44        other
45            .f
46            .total_cmp(&self.f)
47            .then_with(|| self.g.total_cmp(&other.g))
48            .then_with(|| other.index.cmp(&self.index))
49    }
50}
51
52impl PartialOrd for Open {
53    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
54        Some(self.cmp(other))
55    }
56}
57
58impl PartialEq for Open {
59    fn eq(&self, other: &Self) -> bool {
60        self.cmp(other) == Ordering::Equal
61    }
62}
63
64impl Eq for Open {}
65
66#[derive(Clone, Copy, Debug)]
67struct Search {
68    size: UVec2,
69    start: u32,
70    goal: UVec2,
71    options: PathOptions,
72}
73
74/// Reusable A* search state for grids up to the largest size seen.
75///
76/// Buffers grow to the grid's cell count on first use and are reused; later
77/// searches on grids no larger than that do not allocate, apart from the open
78/// queue growing past its previous peak. Paths are written into a
79/// caller-owned vector so its capacity is reused too.
80///
81/// ```
82/// use rayengine_core::glam::UVec2;
83/// use rayengine_core::pathfinding::{CostGrid, PathFinder, PathOptions, PathStatus};
84///
85/// let mut grid = CostGrid::new(UVec2::new(5, 3), 1.0);
86/// grid.block_rect(UVec2::new(2, 0), UVec2::new(2, 1));
87/// let mut finder = PathFinder::new();
88/// let mut path = Vec::new();
89/// let status = finder
90///     .find_path(&grid, UVec2::new(0, 0), UVec2::new(4, 0), &PathOptions::default(), &mut path)
91///     .unwrap();
92/// assert!(matches!(status, PathStatus::Found { .. }));
93/// assert_eq!(path.first(), Some(&UVec2::new(0, 0)));
94/// assert_eq!(path.last(), Some(&UVec2::new(4, 0)));
95/// assert!(path.contains(&UVec2::new(2, 2)));
96/// ```
97#[derive(Clone, Debug, Default)]
98pub struct PathFinder {
99    nodes: Vec<Node>,
100    open: BinaryHeap<Open>,
101    stamp: u32,
102    expanded: u64,
103    search: Option<Search>,
104}
105
106impl PathFinder {
107    /// Creates a finder with no buffers allocated.
108    pub fn new() -> Self {
109        Self::default()
110    }
111
112    /// Allocates buffers for grids up to `size` ahead of the first search.
113    pub fn reserve(&mut self, size: UVec2) -> Result<(), PathError> {
114        let count = cell_count(size)?;
115        if self.nodes.len() < count {
116            self.nodes.resize(count, Node::default());
117        }
118        Ok(())
119    }
120
121    /// Searches from `start` to `goal`, replacing `path` with the result.
122    ///
123    /// Any pending search is cancelled. The start cell is never checked for
124    /// walkability, so an agent standing in a blocked cell can still leave it.
125    /// A blocked goal is unreachable, except that `start == goal` is always
126    /// found with cost zero. With [`PathOptions::budget`] the search
127    /// may return [`PathStatus::Pending`]; continue it with
128    /// [`resume`](Self::resume).
129    ///
130    /// Errors for invalid options, out-of-bounds cells, oversized grids, or an
131    /// invalid cell cost met during the search; errors cancel the search.
132    pub fn find_path<G: NavGrid + ?Sized>(
133        &mut self,
134        grid: &G,
135        start: UVec2,
136        goal: UVec2,
137        options: &PathOptions,
138        path: &mut Vec<UVec2>,
139    ) -> Result<PathStatus, PathError> {
140        self.search = None;
141        self.expanded = 0;
142        path.clear();
143        options.validate()?;
144        let size = grid.size();
145        self.reserve(size)?;
146        check_bounds(size, start)?;
147        check_bounds(size, goal)?;
148        if start == goal {
149            path.push(start);
150            return Ok(PathStatus::Found { cost: 0.0 });
151        }
152        if checked_cost(grid, goal, options.min_cost)?.is_none() {
153            return Ok(PathStatus::Unreachable);
154        }
155
156        self.stamp = self.stamp.wrapping_add(1);
157        if self.stamp == 0 {
158            // Stamps wrapped: old nodes could look current, so forget them all.
159            self.nodes.fill(Node::default());
160            self.stamp = 1;
161        }
162        self.open.clear();
163        let start_index = index(size, start);
164        self.nodes[start_index] = Node {
165            stamp: self.stamp,
166            closed: false,
167            g: 0.0,
168            parent: u32::MAX,
169        };
170        self.push(start_index as u32, 0.0, heuristic(start, goal, options));
171        self.search = Some(Search {
172            size,
173            start: start_index as u32,
174            goal,
175            options: *options,
176        });
177        self.run(grid, options.budget, path)
178    }
179
180    /// Continues a search that returned [`PathStatus::Pending`], expanding at
181    /// most `budget` cells (`None` for no limit).
182    ///
183    /// Pass the same, unchanged grid. Errors with [`PathError::NoSearch`] when
184    /// nothing is pending, [`PathError::GridResized`] (keeping the search
185    /// pending) when the grid size changed, or [`PathError::InvalidOptions`]
186    /// for a zero budget.
187    pub fn resume<G: NavGrid + ?Sized>(
188        &mut self,
189        grid: &G,
190        budget: Option<u32>,
191        path: &mut Vec<UVec2>,
192    ) -> Result<PathStatus, PathError> {
193        path.clear();
194        let search = self.search.ok_or(PathError::NoSearch)?;
195        if budget == Some(0) {
196            return Err(PathError::InvalidOptions);
197        }
198        if grid.size() != search.size {
199            return Err(PathError::GridResized);
200        }
201        self.run(grid, budget, path)
202    }
203
204    /// Whether a budgeted search is waiting for [`resume`](Self::resume).
205    pub fn is_pending(&self) -> bool {
206        self.search.is_some()
207    }
208
209    /// Drops a pending search. Buffers keep their capacity.
210    pub fn cancel(&mut self) {
211        self.search = None;
212    }
213
214    /// Cells expanded by the current or most recent search, across resumes.
215    pub fn expanded(&self) -> u64 {
216        self.expanded
217    }
218
219    /// Buffer capacities and addresses, to check reuse without allocation.
220    #[cfg(test)]
221    pub(super) fn buffers(&self) -> (usize, *const u8, usize) {
222        (
223            self.nodes.capacity(),
224            self.nodes.as_ptr().cast(),
225            self.open.capacity(),
226        )
227    }
228
229    #[cfg(test)]
230    pub(super) fn set_stamp(&mut self, stamp: u32) {
231        self.stamp = stamp;
232    }
233
234    fn push(&mut self, index: u32, g: f32, h: f32) {
235        self.open.push(Open { f: g + h, g, index });
236    }
237
238    fn run<G: NavGrid + ?Sized>(
239        &mut self,
240        grid: &G,
241        mut budget: Option<u32>,
242        path: &mut Vec<UVec2>,
243    ) -> Result<PathStatus, PathError> {
244        let Some(search) = self.search else {
245            return Err(PathError::NoSearch);
246        };
247        let Search {
248            size,
249            goal,
250            options,
251            ..
252        } = search;
253        let goal_index = index(size, goal) as u32;
254        let rule = options.neighborhood.corner_rule();
255        loop {
256            if budget == Some(0) {
257                return Ok(PathStatus::Pending);
258            }
259            let Some(open) = self.open.pop() else {
260                self.search = None;
261                return Ok(PathStatus::Unreachable);
262            };
263            let node = &mut self.nodes[open.index as usize];
264            // Skip queue entries superseded by a cheaper route.
265            if node.closed || open.g > node.g {
266                continue;
267            }
268            node.closed = true;
269            self.expanded += 1;
270            if let Some(remaining) = &mut budget {
271                *remaining -= 1;
272            }
273            if open.index == goal_index {
274                self.reconstruct(size, search.start, goal_index, path);
275                self.search = None;
276                return Ok(PathStatus::Found { cost: open.g });
277            }
278            let cell = cell_of(size, open.index as usize);
279            for &step in options.neighborhood.steps() {
280                let Some(next) = offset(size, cell, step) else {
281                    continue;
282                };
283                let next_index = index(size, next);
284                let known = self.nodes[next_index];
285                let current = known.stamp == self.stamp;
286                if current && known.closed {
287                    continue;
288                }
289                if !corner_open(grid, cell, step, rule) {
290                    continue;
291                }
292                let cost = match checked_cost(grid, next, options.min_cost) {
293                    Ok(Some(cost)) => cost,
294                    Ok(None) => continue,
295                    Err(error) => {
296                        self.search = None;
297                        return Err(error);
298                    }
299                };
300                let g = open.g + cost * step_length(step);
301                if !current || g < known.g {
302                    self.nodes[next_index] = Node {
303                        stamp: self.stamp,
304                        closed: false,
305                        g,
306                        parent: open.index,
307                    };
308                    self.push(next_index as u32, g, heuristic(next, goal, &options));
309                }
310            }
311        }
312    }
313
314    fn reconstruct(&self, size: UVec2, start: u32, goal: u32, path: &mut Vec<UVec2>) {
315        let mut current = goal;
316        loop {
317            path.push(cell_of(size, current as usize));
318            if current == start {
319                break;
320            }
321            current = self.nodes[current as usize].parent;
322        }
323        path.reverse();
324    }
325}
326
327/// Lower bound of the remaining cost: Manhattan or octile distance scaled by
328/// the cheapest possible cell.
329fn heuristic(cell: UVec2, goal: UVec2, options: &PathOptions) -> f32 {
330    let delta = (cell.as_ivec2() - goal.as_ivec2()).abs().as_vec2();
331    let distance = match options.neighborhood {
332        Neighborhood::Four => delta.x + delta.y,
333        Neighborhood::Eight(_) => {
334            delta.max_element() + (std::f32::consts::SQRT_2 - 1.0) * delta.min_element()
335        }
336    };
337    distance * options.min_cost
338}