Skip to main content

rayengine_core/
pathfinding.rs

1//! Grid navigation: A* paths, line-of-sight smoothing, path following and
2//! distance fields over caller-owned 2D grids.
3//!
4//! Searches read cells through [`NavGrid`], so tilemaps, voxel layers and game
5//! arrays can be searched in place without copying. [`PathFinder`] and
6//! [`DistanceField`] keep their buffers between queries: once their capacity
7//! covers the grid, queries do not allocate. [`GridLayout`] maps cells to world
8//! positions for [`PathFollower`] and collision geometry.
9//!
10//! Moving into a cell costs that cell's [`NavGrid::cost`] multiplied by the
11//! step length: `1` orthogonally and `√2` diagonally. The start cell's cost is
12//! never charged. Results are deterministic for the same grid and inputs.
13
14mod astar;
15mod field;
16mod follow;
17mod smooth;
18
19pub use astar::{PathFinder, PathStatus};
20pub use field::DistanceField;
21pub use follow::PathFollower;
22pub use smooth::{line_of_sight, smooth_path};
23
24use crate::collision::Aabb2;
25use glam::{IVec2, UVec2, Vec2};
26use std::fmt;
27
28/// Read-only access to a bounded navigation grid.
29///
30/// Implement this for an existing tilemap, voxel slice or array so searches
31/// borrow its cells instead of copying them. Cell `(x, y)` exists for
32/// `x < size().x` and `y < size().y`.
33pub trait NavGrid {
34    /// Width and height in cells.
35    fn size(&self) -> UVec2;
36
37    /// Cost of entering `cell`, or `None` when it is blocked.
38    ///
39    /// Only called for cells inside [`size`](Self::size). Searches reject a
40    /// cost that is not finite or is below [`PathOptions::min_cost`] (or not
41    /// positive for distance fields and smoothing) with
42    /// [`PathError::InvalidCost`].
43    fn cost(&self, cell: UVec2) -> Option<f32>;
44
45    /// Whether `cell` can be entered, for checks that need no cost: corner
46    /// rules, line of sight and clearance, and which cells get a distance
47    /// field entry. Overrides are only a faster path and must return exactly
48    /// `cost(cell).is_some()`, the default.
49    fn walkable(&self, cell: UVec2) -> bool {
50        self.cost(cell).is_some()
51    }
52}
53
54impl<G: NavGrid + ?Sized> NavGrid for &G {
55    fn size(&self) -> UVec2 {
56        (**self).size()
57    }
58
59    fn cost(&self, cell: UVec2) -> Option<f32> {
60        (**self).cost(cell)
61    }
62
63    fn walkable(&self, cell: UVec2) -> bool {
64        (**self).walkable(cell)
65    }
66}
67
68/// Adapts a size and cost callback to [`NavGrid`] without storing cells.
69///
70/// ```
71/// use rayengine_core::glam::UVec2;
72/// use rayengine_core::pathfinding::{GridFn, NavGrid};
73///
74/// let walls = [[false, true], [false, false]];
75/// let grid = GridFn::new(UVec2::new(2, 2), |cell: UVec2| {
76///     (!walls[cell.y as usize][cell.x as usize]).then_some(1.0)
77/// });
78/// assert!(!grid.walkable(UVec2::new(1, 0)));
79/// ```
80#[derive(Clone, Copy, Debug)]
81pub struct GridFn<F> {
82    size: UVec2,
83    cost: F,
84}
85
86impl<F: Fn(UVec2) -> Option<f32>> GridFn<F> {
87    /// Wraps `cost`, which is called only for cells inside `size`.
88    pub fn new(size: UVec2, cost: F) -> Self {
89        Self { size, cost }
90    }
91}
92
93impl<F: Fn(UVec2) -> Option<f32>> NavGrid for GridFn<F> {
94    fn size(&self) -> UVec2 {
95        self.size
96    }
97
98    fn cost(&self, cell: UVec2) -> Option<f32> {
99        (self.cost)(cell)
100    }
101}
102
103/// Owned row-major grid of per-cell costs.
104#[derive(Clone, Debug, PartialEq)]
105pub struct CostGrid {
106    size: UVec2,
107    // `f32::INFINITY` marks a blocked cell.
108    costs: Vec<f32>,
109}
110
111impl CostGrid {
112    /// Creates a grid where every cell costs `cost`.
113    /// Panics if `cost` is not finite and positive or the grid is too large.
114    pub fn new(size: UVec2, cost: f32) -> Self {
115        assert!(cost.is_finite() && cost > 0.0, "invalid cell cost");
116        let count = cell_count(size).expect("grid is too large");
117        Self {
118            size,
119            costs: vec![cost; count],
120        }
121    }
122
123    /// Sets a cell's cost, or blocks it with `None`.
124    /// Panics if `cell` is outside the grid or `cost` is not finite and positive.
125    pub fn set(&mut self, cell: UVec2, cost: Option<f32>) {
126        assert!(cell.cmplt(self.size).all(), "cell is outside the grid");
127        let cost = match cost {
128            Some(cost) => {
129                assert!(cost.is_finite() && cost > 0.0, "invalid cell cost");
130                cost
131            }
132            None => f32::INFINITY,
133        };
134        self.costs[index(self.size, cell)] = cost;
135    }
136
137    /// Blocks every cell in the inclusive rectangle `min..=max`, clamped to the grid.
138    pub fn block_rect(&mut self, min: UVec2, max: UVec2) {
139        if self.size.cmpeq(UVec2::ZERO).any() {
140            return;
141        }
142        let max = max.min(self.size - 1);
143        for y in min.y..=max.y {
144            for x in min.x..=max.x {
145                self.set(UVec2::new(x, y), None);
146            }
147        }
148    }
149}
150
151impl NavGrid for CostGrid {
152    fn size(&self) -> UVec2 {
153        self.size
154    }
155
156    fn cost(&self, cell: UVec2) -> Option<f32> {
157        let cost = self.costs[index(self.size, cell)];
158        cost.is_finite().then_some(cost)
159    }
160}
161
162/// Rule for diagonal moves past the two orthogonally adjacent cells.
163#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)]
164pub enum DiagonalRule {
165    /// Diagonals may pass between two blocked cells.
166    Always,
167    /// At least one orthogonally adjacent cell must be walkable.
168    IfEitherOpen,
169    /// Both orthogonally adjacent cells must be walkable: no corner cutting.
170    #[default]
171    IfBothOpen,
172}
173
174/// Cells reachable in one step.
175#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
176pub enum Neighborhood {
177    /// Orthogonal moves only.
178    Four,
179    /// Orthogonal and diagonal moves, with diagonals limited by a rule.
180    Eight(DiagonalRule),
181}
182
183impl Default for Neighborhood {
184    fn default() -> Self {
185        Self::Eight(DiagonalRule::default())
186    }
187}
188
189impl Neighborhood {
190    /// Rule for line segments passing exactly through a cell corner. Four-way
191    /// movement never cuts corners.
192    pub(crate) fn corner_rule(self) -> DiagonalRule {
193        match self {
194            Self::Four => DiagonalRule::IfBothOpen,
195            Self::Eight(rule) => rule,
196        }
197    }
198
199    pub(crate) fn steps(self) -> &'static [IVec2] {
200        match self {
201            Self::Four => &STEPS[..4],
202            Self::Eight(_) => &STEPS,
203        }
204    }
205}
206
207/// A* search settings.
208#[derive(Clone, Copy, Debug, PartialEq)]
209pub struct PathOptions {
210    /// Allowed moves and diagonal corner rule.
211    pub neighborhood: Neighborhood,
212    /// Lowest cost any cell can have; it scales the distance heuristic so
213    /// paths stay optimal. Must be finite and positive. Default: `1.0`.
214    pub min_cost: f32,
215    /// Maximum cells expanded per call before returning
216    /// [`PathStatus::Pending`]; `None` searches to completion. Must not be zero.
217    pub budget: Option<u32>,
218}
219
220impl Default for PathOptions {
221    fn default() -> Self {
222        Self {
223            neighborhood: Neighborhood::default(),
224            min_cost: 1.0,
225            budget: None,
226        }
227    }
228}
229
230impl PathOptions {
231    pub(crate) fn validate(&self) -> Result<(), PathError> {
232        let budget_ok = self.budget != Some(0);
233        if self.min_cost.is_finite() && self.min_cost > 0.0 && budget_ok {
234            Ok(())
235        } else {
236            Err(PathError::InvalidOptions)
237        }
238    }
239}
240
241/// Invalid navigation input.
242#[derive(Clone, Copy, Debug, PartialEq)]
243pub enum PathError {
244    /// A start, goal or query cell is outside the grid.
245    OutOfBounds(UVec2),
246    /// A grid reported a cost that is nonfinite or below the allowed minimum.
247    InvalidCost {
248        /// Cell with the invalid cost.
249        cell: UVec2,
250        /// The reported cost.
251        cost: f32,
252    },
253    /// `min_cost` is not finite and positive, or `budget` is zero.
254    InvalidOptions,
255    /// The grid has more cells than a search can index.
256    GridTooLarge(UVec2),
257    /// [`PathFinder::resume`] was called without a pending search.
258    NoSearch,
259    /// The grid size changed while a search was pending.
260    GridResized,
261}
262
263impl fmt::Display for PathError {
264    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
265        match self {
266            Self::OutOfBounds(cell) => write!(f, "cell {cell} is outside the grid"),
267            Self::InvalidCost { cell, cost } => write!(f, "cell {cell} has invalid cost {cost}"),
268            Self::InvalidOptions => write!(f, "path options are invalid"),
269            Self::GridTooLarge(size) => write!(f, "grid {size} has too many cells"),
270            Self::NoSearch => write!(f, "no path search is pending"),
271            Self::GridResized => write!(f, "grid size changed during a pending search"),
272        }
273    }
274}
275
276impl std::error::Error for PathError {}
277
278/// Maps grid cells to world-space cells.
279///
280/// Cell `(x, y)` covers `origin + (x, y) * cell_size` to one cell further along
281/// both axes. In 3D, use the vector's `y` as world `z`. Searches measure
282/// distance in cells, so with non-square cells a path is shortest in cell
283/// steps rather than in world units.
284#[derive(Clone, Copy, Debug, PartialEq)]
285pub struct GridLayout {
286    /// World position of cell `(0, 0)`'s minimum corner.
287    pub origin: Vec2,
288    /// Width and height of one cell in world units. Must be finite and positive.
289    pub cell_size: Vec2,
290}
291
292impl GridLayout {
293    /// Creates a layout. Panics for a nonfinite origin or a nonpositive or
294    /// nonfinite cell size.
295    pub fn new(origin: Vec2, cell_size: Vec2) -> Self {
296        assert!(origin.is_finite() && cell_size.is_finite() && cell_size.min_element() > 0.0);
297        Self { origin, cell_size }
298    }
299
300    /// World position of a cell's center.
301    pub fn cell_center(&self, cell: UVec2) -> Vec2 {
302        self.origin + (cell.as_vec2() + 0.5) * self.cell_size
303    }
304
305    /// World bounds of a cell, for example as a static collider. Neighboring
306    /// cells share exact edges.
307    pub fn cell_bounds(&self, cell: UVec2) -> Aabb2 {
308        Aabb2 {
309            min: self.origin + cell.as_vec2() * self.cell_size,
310            max: self.origin + (cell + 1).as_vec2() * self.cell_size,
311        }
312    }
313
314    /// A body's clearance in cell units, for
315    /// [`smooth_path`] and [`line_of_sight`]: its half size divided by the
316    /// cell size, such as `layout.clearance(body.half_size)`.
317    pub fn clearance(&self, half_size: Vec2) -> Vec2 {
318        half_size / self.cell_size
319    }
320
321    /// Cell containing `point` in a grid of `size`, or `None` outside it.
322    /// Points on a shared edge belong to the cell with the larger coordinate.
323    pub fn cell_at(&self, point: Vec2, size: UVec2) -> Option<UVec2> {
324        let estimate = ((point - self.origin) / self.cell_size).floor();
325        let mut cell = UVec2::ZERO;
326        for axis in 0..2 {
327            // Compare against the same edges as `cell_bounds`, correcting
328            // rounding in the division.
329            let edge = |i: u32| self.origin[axis] + i as f32 * self.cell_size[axis];
330            let count = size[axis];
331            if !point[axis].is_finite() || point[axis] < edge(0) || point[axis] >= edge(count) {
332                return None;
333            }
334            let mut i = estimate[axis].clamp(0.0, (count - 1) as f32) as u32;
335            while i > 0 && point[axis] < edge(i) {
336                i -= 1;
337            }
338            while i + 1 < count && point[axis] >= edge(i + 1) {
339                i += 1;
340            }
341            cell[axis] = i;
342        }
343        Some(cell)
344    }
345}
346
347/// Orthogonal steps first, then diagonals; this order breaks exact ties.
348const STEPS: [IVec2; 8] = [
349    IVec2::new(1, 0),
350    IVec2::new(0, 1),
351    IVec2::new(-1, 0),
352    IVec2::new(0, -1),
353    IVec2::new(1, 1),
354    IVec2::new(-1, 1),
355    IVec2::new(-1, -1),
356    IVec2::new(1, -1),
357];
358
359pub(crate) fn cell_count(size: UVec2) -> Result<usize, PathError> {
360    // Leave `u32::MAX` free as a sentinel and keep indices in `u32`.
361    let count = u64::from(size.x) * u64::from(size.y);
362    if count < u64::from(u32::MAX) {
363        Ok(count as usize)
364    } else {
365        Err(PathError::GridTooLarge(size))
366    }
367}
368
369pub(crate) fn index(size: UVec2, cell: UVec2) -> usize {
370    cell.y as usize * size.x as usize + cell.x as usize
371}
372
373pub(crate) fn cell_of(size: UVec2, index: usize) -> UVec2 {
374    let width = size.x as usize;
375    UVec2::new((index % width) as u32, (index / width) as u32)
376}
377
378pub(crate) fn offset(size: UVec2, cell: UVec2, step: IVec2) -> Option<UVec2> {
379    let next = cell.as_ivec2() + step;
380    (next.cmpge(IVec2::ZERO).all() && next.as_uvec2().cmplt(size).all()).then(|| next.as_uvec2())
381}
382
383pub(crate) fn step_length(step: IVec2) -> f32 {
384    if step.x != 0 && step.y != 0 {
385        std::f32::consts::SQRT_2
386    } else {
387        1.0
388    }
389}
390
391/// Whether a move from `cell` along `step` passes the corner rule. Both side
392/// cells are inside the grid whenever the destination is.
393pub(crate) fn corner_open<G: NavGrid + ?Sized>(
394    grid: &G,
395    cell: UVec2,
396    step: IVec2,
397    rule: DiagonalRule,
398) -> bool {
399    if step.x == 0 || step.y == 0 || rule == DiagonalRule::Always {
400        return true;
401    }
402    let side = |offset_step| {
403        offset(grid.size(), cell, offset_step).is_some_and(|side| grid.walkable(side))
404    };
405    let (a, b) = (side(IVec2::new(step.x, 0)), side(IVec2::new(0, step.y)));
406    match rule {
407        DiagonalRule::Always => true,
408        DiagonalRule::IfEitherOpen => a || b,
409        DiagonalRule::IfBothOpen => a && b,
410    }
411}
412
413/// Reads a cost, rejecting values that are nonfinite or below `min_cost`.
414/// A zero `min_cost` still requires positive costs.
415pub(crate) fn checked_cost<G: NavGrid + ?Sized>(
416    grid: &G,
417    cell: UVec2,
418    min_cost: f32,
419) -> Result<Option<f32>, PathError> {
420    match grid.cost(cell) {
421        Some(cost) if !(cost.is_finite() && cost > 0.0 && cost >= min_cost) => {
422            Err(PathError::InvalidCost { cell, cost })
423        }
424        cost => Ok(cost),
425    }
426}
427
428pub(crate) fn check_bounds(size: UVec2, cell: UVec2) -> Result<(), PathError> {
429    if cell.cmplt(size).all() {
430        Ok(())
431    } else {
432        Err(PathError::OutOfBounds(cell))
433    }
434}
435
436#[cfg(test)]
437mod tests;