rayengine_core/pathfinding/
astar.rs1use 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#[derive(Clone, Copy, Debug, PartialEq)]
10pub enum PathStatus {
11 Found {
14 cost: f32,
16 },
17 Unreachable,
19 Pending,
22}
23
24#[derive(Clone, Copy, Debug, Default)]
25struct Node {
26 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 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#[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 pub fn new() -> Self {
109 Self::default()
110 }
111
112 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 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 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 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 pub fn is_pending(&self) -> bool {
206 self.search.is_some()
207 }
208
209 pub fn cancel(&mut self) {
211 self.search = None;
212 }
213
214 pub fn expanded(&self) -> u64 {
216 self.expanded
217 }
218
219 #[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 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
327fn 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}