Skip to main content

rayengine_core/pathfinding/
field.rs

1use super::{
2    NavGrid, Neighborhood, PathError, cell_count, cell_of, check_bounds, checked_cost, corner_open,
3    index, offset, step_length,
4};
5use glam::UVec2;
6use std::{cmp::Ordering, collections::BinaryHeap};
7
8#[derive(Clone, Copy, Debug)]
9struct Open {
10    distance: f32,
11    index: u32,
12}
13
14impl Ord for Open {
15    // Reversed for a min-heap: nearest first, then lowest cell index.
16    fn cmp(&self, other: &Self) -> Ordering {
17        other
18            .distance
19            .total_cmp(&self.distance)
20            .then_with(|| other.index.cmp(&self.index))
21    }
22}
23
24impl PartialOrd for Open {
25    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
26        Some(self.cmp(other))
27    }
28}
29
30impl PartialEq for Open {
31    fn eq(&self, other: &Self) -> bool {
32        self.cmp(other) == Ordering::Equal
33    }
34}
35
36impl Eq for Open {}
37
38/// Cost from every cell to the nearest of one or more goals (a Dijkstra map).
39///
40/// One computation serves any number of agents chasing the same target:
41/// each agent calls [`next_step`](Self::next_step) for its cell. Distances use
42/// the same costs and corner rules as [`PathFinder`](super::PathFinder), so a
43/// chain of next steps follows an optimal path. Unlike `PathFinder`, which
44/// lets an agent leave a blocked start cell, blocked cells have no distance
45/// and no next step. Buffers are reused; computing
46/// again for a grid no larger than before does not allocate, apart from the
47/// queue growing past its previous peak.
48///
49/// ```
50/// use rayengine_core::glam::UVec2;
51/// use rayengine_core::pathfinding::{CostGrid, DistanceField, Neighborhood};
52///
53/// let mut grid = CostGrid::new(UVec2::new(3, 3), 1.0);
54/// grid.block_rect(UVec2::new(1, 0), UVec2::new(1, 1));
55/// let mut field = DistanceField::new();
56/// field.compute(&grid, [UVec2::new(2, 0)], Neighborhood::Four).unwrap();
57/// assert_eq!(field.distance(UVec2::new(0, 0)), Some(6.0));
58/// assert_eq!(field.next_step(&grid, UVec2::new(0, 0)), Some(UVec2::new(0, 1)));
59/// assert_eq!(field.distance(UVec2::new(1, 0)), None);
60/// ```
61#[derive(Clone, Debug, Default)]
62pub struct DistanceField {
63    size: UVec2,
64    neighborhood: Neighborhood,
65    distances: Vec<f32>,
66    open: BinaryHeap<Open>,
67}
68
69impl DistanceField {
70    /// Creates an empty field with no buffers allocated.
71    pub fn new() -> Self {
72        Self::default()
73    }
74
75    /// Recomputes distances to the nearest walkable goal.
76    ///
77    /// Blocked goals are ignored; with no walkable goals every cell is
78    /// unreachable. Costs must be finite and positive. On error the field is
79    /// left empty, as if computed for a zero-sized grid.
80    pub fn compute<G: NavGrid + ?Sized>(
81        &mut self,
82        grid: &G,
83        goals: impl IntoIterator<Item = UVec2>,
84        neighborhood: Neighborhood,
85    ) -> Result<(), PathError> {
86        let result = self.fill(grid, goals, neighborhood);
87        if result.is_err() {
88            self.size = UVec2::ZERO;
89            self.distances.clear();
90        }
91        self.open.clear();
92        result
93    }
94
95    fn fill<G: NavGrid + ?Sized>(
96        &mut self,
97        grid: &G,
98        goals: impl IntoIterator<Item = UVec2>,
99        neighborhood: Neighborhood,
100    ) -> Result<(), PathError> {
101        let size = grid.size();
102        let count = cell_count(size)?;
103        self.size = size;
104        self.neighborhood = neighborhood;
105        self.distances.clear();
106        self.distances.resize(count, f32::INFINITY);
107        self.open.clear();
108        for goal in goals {
109            check_bounds(size, goal)?;
110            let goal_index = index(size, goal);
111            if checked_cost(grid, goal, 0.0)?.is_some() && self.distances[goal_index] != 0.0 {
112                self.distances[goal_index] = 0.0;
113                self.open.push(Open {
114                    distance: 0.0,
115                    index: goal_index as u32,
116                });
117            }
118        }
119        let rule = neighborhood.corner_rule();
120        while let Some(open) = self.open.pop() {
121            let current = open.index as usize;
122            if open.distance > self.distances[current] {
123                continue;
124            }
125            let cell = cell_of(size, current);
126            // Neighbors pay this cell's cost to step into it.
127            let Some(cost) = checked_cost(grid, cell, 0.0)? else {
128                continue;
129            };
130            for &step in neighborhood.steps() {
131                let Some(from) = offset(size, cell, step) else {
132                    continue;
133                };
134                // Corner rules are symmetric, so check the move toward `cell`.
135                if !corner_open(grid, from, -step, rule) || !grid.walkable(from) {
136                    continue;
137                }
138                let from_index = index(size, from);
139                let distance = open.distance + cost * step_length(step);
140                if distance < self.distances[from_index] {
141                    self.distances[from_index] = distance;
142                    self.open.push(Open {
143                        distance,
144                        index: from_index as u32,
145                    });
146                }
147            }
148        }
149        Ok(())
150    }
151
152    /// Size of the grid the field was computed for.
153    pub fn size(&self) -> UVec2 {
154        self.size
155    }
156
157    /// Cost from `cell` to the nearest goal, or `None` when the cell is
158    /// blocked, unreachable, or outside the computed grid.
159    pub fn distance(&self, cell: UVec2) -> Option<f32> {
160        if !cell.cmplt(self.size).all() {
161            return None;
162        }
163        let distance = self.distances[index(self.size, cell)];
164        distance.is_finite().then_some(distance)
165    }
166
167    /// The neighbor to move to from `cell` along an optimal path, or `None`
168    /// at a goal, from an unreachable cell, or for a grid of another size.
169    ///
170    /// Pass the grid the field was computed from. Exact ties prefer
171    /// orthogonal moves in the order +x, +y, -x, -y.
172    pub fn next_step<G: NavGrid + ?Sized>(&self, grid: &G, cell: UVec2) -> Option<UVec2> {
173        if grid.size() != self.size {
174            return None;
175        }
176        let here = self.distance(cell)?;
177        let rule = self.neighborhood.corner_rule();
178        let mut best: Option<(f32, UVec2)> = None;
179        for &step in self.neighborhood.steps() {
180            let Some(next) = offset(self.size, cell, step) else {
181                continue;
182            };
183            let (Some(distance), Some(cost)) = (self.distance(next), grid.cost(next)) else {
184                continue;
185            };
186            if !corner_open(grid, cell, step, rule) {
187                continue;
188            }
189            let total = distance + cost * step_length(step);
190            if best.is_none_or(|(best, _)| total < best) {
191                best = Some((total, next));
192            }
193        }
194        // Only step when it makes progress, so goals and stale grids stop.
195        best.filter(|&(_, next)| self.distances[index(self.size, next)] < here)
196            .map(|(_, next)| next)
197    }
198}