1mod 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
28pub trait NavGrid {
34 fn size(&self) -> UVec2;
36
37 fn cost(&self, cell: UVec2) -> Option<f32>;
44
45 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#[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 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#[derive(Clone, Debug, PartialEq)]
105pub struct CostGrid {
106 size: UVec2,
107 costs: Vec<f32>,
109}
110
111impl CostGrid {
112 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 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 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#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)]
164pub enum DiagonalRule {
165 Always,
167 IfEitherOpen,
169 #[default]
171 IfBothOpen,
172}
173
174#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
176pub enum Neighborhood {
177 Four,
179 Eight(DiagonalRule),
181}
182
183impl Default for Neighborhood {
184 fn default() -> Self {
185 Self::Eight(DiagonalRule::default())
186 }
187}
188
189impl Neighborhood {
190 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#[derive(Clone, Copy, Debug, PartialEq)]
209pub struct PathOptions {
210 pub neighborhood: Neighborhood,
212 pub min_cost: f32,
215 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#[derive(Clone, Copy, Debug, PartialEq)]
243pub enum PathError {
244 OutOfBounds(UVec2),
246 InvalidCost {
248 cell: UVec2,
250 cost: f32,
252 },
253 InvalidOptions,
255 GridTooLarge(UVec2),
257 NoSearch,
259 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#[derive(Clone, Copy, Debug, PartialEq)]
285pub struct GridLayout {
286 pub origin: Vec2,
288 pub cell_size: Vec2,
290}
291
292impl GridLayout {
293 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 pub fn cell_center(&self, cell: UVec2) -> Vec2 {
302 self.origin + (cell.as_vec2() + 0.5) * self.cell_size
303 }
304
305 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 pub fn clearance(&self, half_size: Vec2) -> Vec2 {
318 half_size / self.cell_size
319 }
320
321 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 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
347const 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 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
391pub(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
413pub(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;