mod astar;
mod field;
mod follow;
mod smooth;
pub use astar::{PathFinder, PathStatus};
pub use field::DistanceField;
pub use follow::PathFollower;
pub use smooth::{line_of_sight, smooth_path};
use crate::collision::Aabb2;
use glam::{IVec2, UVec2, Vec2};
use std::fmt;
pub trait NavGrid {
fn size(&self) -> UVec2;
fn cost(&self, cell: UVec2) -> Option<f32>;
fn walkable(&self, cell: UVec2) -> bool {
self.cost(cell).is_some()
}
}
impl<G: NavGrid + ?Sized> NavGrid for &G {
fn size(&self) -> UVec2 {
(**self).size()
}
fn cost(&self, cell: UVec2) -> Option<f32> {
(**self).cost(cell)
}
fn walkable(&self, cell: UVec2) -> bool {
(**self).walkable(cell)
}
}
#[derive(Clone, Copy, Debug)]
pub struct GridFn<F> {
size: UVec2,
cost: F,
}
impl<F: Fn(UVec2) -> Option<f32>> GridFn<F> {
pub fn new(size: UVec2, cost: F) -> Self {
Self { size, cost }
}
}
impl<F: Fn(UVec2) -> Option<f32>> NavGrid for GridFn<F> {
fn size(&self) -> UVec2 {
self.size
}
fn cost(&self, cell: UVec2) -> Option<f32> {
(self.cost)(cell)
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct CostGrid {
size: UVec2,
costs: Vec<f32>,
}
impl CostGrid {
pub fn new(size: UVec2, cost: f32) -> Self {
assert!(cost.is_finite() && cost > 0.0, "invalid cell cost");
let count = cell_count(size).expect("grid is too large");
Self {
size,
costs: vec![cost; count],
}
}
pub fn set(&mut self, cell: UVec2, cost: Option<f32>) {
assert!(cell.cmplt(self.size).all(), "cell is outside the grid");
let cost = match cost {
Some(cost) => {
assert!(cost.is_finite() && cost > 0.0, "invalid cell cost");
cost
}
None => f32::INFINITY,
};
self.costs[index(self.size, cell)] = cost;
}
pub fn block_rect(&mut self, min: UVec2, max: UVec2) {
if self.size.cmpeq(UVec2::ZERO).any() {
return;
}
let max = max.min(self.size - 1);
for y in min.y..=max.y {
for x in min.x..=max.x {
self.set(UVec2::new(x, y), None);
}
}
}
}
impl NavGrid for CostGrid {
fn size(&self) -> UVec2 {
self.size
}
fn cost(&self, cell: UVec2) -> Option<f32> {
let cost = self.costs[index(self.size, cell)];
cost.is_finite().then_some(cost)
}
}
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)]
pub enum DiagonalRule {
Always,
IfEitherOpen,
#[default]
IfBothOpen,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
pub enum Neighborhood {
Four,
Eight(DiagonalRule),
}
impl Default for Neighborhood {
fn default() -> Self {
Self::Eight(DiagonalRule::default())
}
}
impl Neighborhood {
pub(crate) fn corner_rule(self) -> DiagonalRule {
match self {
Self::Four => DiagonalRule::IfBothOpen,
Self::Eight(rule) => rule,
}
}
pub(crate) fn steps(self) -> &'static [IVec2] {
match self {
Self::Four => &STEPS[..4],
Self::Eight(_) => &STEPS,
}
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PathOptions {
pub neighborhood: Neighborhood,
pub min_cost: f32,
pub budget: Option<u32>,
}
impl Default for PathOptions {
fn default() -> Self {
Self {
neighborhood: Neighborhood::default(),
min_cost: 1.0,
budget: None,
}
}
}
impl PathOptions {
pub(crate) fn validate(&self) -> Result<(), PathError> {
let budget_ok = self.budget != Some(0);
if self.min_cost.is_finite() && self.min_cost > 0.0 && budget_ok {
Ok(())
} else {
Err(PathError::InvalidOptions)
}
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum PathError {
OutOfBounds(UVec2),
InvalidCost {
cell: UVec2,
cost: f32,
},
InvalidOptions,
GridTooLarge(UVec2),
NoSearch,
GridResized,
}
impl fmt::Display for PathError {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::OutOfBounds(cell) => write!(f, "cell {cell} is outside the grid"),
Self::InvalidCost { cell, cost } => write!(f, "cell {cell} has invalid cost {cost}"),
Self::InvalidOptions => write!(f, "path options are invalid"),
Self::GridTooLarge(size) => write!(f, "grid {size} has too many cells"),
Self::NoSearch => write!(f, "no path search is pending"),
Self::GridResized => write!(f, "grid size changed during a pending search"),
}
}
}
impl std::error::Error for PathError {}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct GridLayout {
pub origin: Vec2,
pub cell_size: Vec2,
}
impl GridLayout {
pub fn new(origin: Vec2, cell_size: Vec2) -> Self {
assert!(origin.is_finite() && cell_size.is_finite() && cell_size.min_element() > 0.0);
Self { origin, cell_size }
}
pub fn cell_center(&self, cell: UVec2) -> Vec2 {
self.origin + (cell.as_vec2() + 0.5) * self.cell_size
}
pub fn cell_bounds(&self, cell: UVec2) -> Aabb2 {
Aabb2 {
min: self.origin + cell.as_vec2() * self.cell_size,
max: self.origin + (cell + 1).as_vec2() * self.cell_size,
}
}
pub fn clearance(&self, half_size: Vec2) -> Vec2 {
half_size / self.cell_size
}
pub fn cell_at(&self, point: Vec2, size: UVec2) -> Option<UVec2> {
let estimate = ((point - self.origin) / self.cell_size).floor();
let mut cell = UVec2::ZERO;
for axis in 0..2 {
let edge = |i: u32| self.origin[axis] + i as f32 * self.cell_size[axis];
let count = size[axis];
if !point[axis].is_finite() || point[axis] < edge(0) || point[axis] >= edge(count) {
return None;
}
let mut i = estimate[axis].clamp(0.0, (count - 1) as f32) as u32;
while i > 0 && point[axis] < edge(i) {
i -= 1;
}
while i + 1 < count && point[axis] >= edge(i + 1) {
i += 1;
}
cell[axis] = i;
}
Some(cell)
}
}
const STEPS: [IVec2; 8] = [
IVec2::new(1, 0),
IVec2::new(0, 1),
IVec2::new(-1, 0),
IVec2::new(0, -1),
IVec2::new(1, 1),
IVec2::new(-1, 1),
IVec2::new(-1, -1),
IVec2::new(1, -1),
];
pub(crate) fn cell_count(size: UVec2) -> Result<usize, PathError> {
let count = u64::from(size.x) * u64::from(size.y);
if count < u64::from(u32::MAX) {
Ok(count as usize)
} else {
Err(PathError::GridTooLarge(size))
}
}
pub(crate) fn index(size: UVec2, cell: UVec2) -> usize {
cell.y as usize * size.x as usize + cell.x as usize
}
pub(crate) fn cell_of(size: UVec2, index: usize) -> UVec2 {
let width = size.x as usize;
UVec2::new((index % width) as u32, (index / width) as u32)
}
pub(crate) fn offset(size: UVec2, cell: UVec2, step: IVec2) -> Option<UVec2> {
let next = cell.as_ivec2() + step;
(next.cmpge(IVec2::ZERO).all() && next.as_uvec2().cmplt(size).all()).then(|| next.as_uvec2())
}
pub(crate) fn step_length(step: IVec2) -> f32 {
if step.x != 0 && step.y != 0 {
std::f32::consts::SQRT_2
} else {
1.0
}
}
pub(crate) fn corner_open<G: NavGrid + ?Sized>(
grid: &G,
cell: UVec2,
step: IVec2,
rule: DiagonalRule,
) -> bool {
if step.x == 0 || step.y == 0 || rule == DiagonalRule::Always {
return true;
}
let side = |offset_step| {
offset(grid.size(), cell, offset_step).is_some_and(|side| grid.walkable(side))
};
let (a, b) = (side(IVec2::new(step.x, 0)), side(IVec2::new(0, step.y)));
match rule {
DiagonalRule::Always => true,
DiagonalRule::IfEitherOpen => a || b,
DiagonalRule::IfBothOpen => a && b,
}
}
pub(crate) fn checked_cost<G: NavGrid + ?Sized>(
grid: &G,
cell: UVec2,
min_cost: f32,
) -> Result<Option<f32>, PathError> {
match grid.cost(cell) {
Some(cost) if !(cost.is_finite() && cost > 0.0 && cost >= min_cost) => {
Err(PathError::InvalidCost { cell, cost })
}
cost => Ok(cost),
}
}
pub(crate) fn check_bounds(size: UVec2, cell: UVec2) -> Result<(), PathError> {
if cell.cmplt(size).all() {
Ok(())
} else {
Err(PathError::OutOfBounds(cell))
}
}
#[cfg(test)]
mod tests;