use super::{
NavGrid, Neighborhood, PathError, PathOptions, cell_count, cell_of, check_bounds, checked_cost,
corner_open, index, offset, step_length,
};
use glam::UVec2;
use std::{cmp::Ordering, collections::BinaryHeap};
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum PathStatus {
Found {
cost: f32,
},
Unreachable,
Pending,
}
#[derive(Clone, Copy, Debug, Default)]
struct Node {
stamp: u32,
closed: bool,
g: f32,
parent: u32,
}
#[derive(Clone, Copy, Debug)]
struct Open {
f: f32,
g: f32,
index: u32,
}
impl Ord for Open {
fn cmp(&self, other: &Self) -> Ordering {
other
.f
.total_cmp(&self.f)
.then_with(|| self.g.total_cmp(&other.g))
.then_with(|| other.index.cmp(&self.index))
}
}
impl PartialOrd for Open {
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
Some(self.cmp(other))
}
}
impl PartialEq for Open {
fn eq(&self, other: &Self) -> bool {
self.cmp(other) == Ordering::Equal
}
}
impl Eq for Open {}
#[derive(Clone, Copy, Debug)]
struct Search {
size: UVec2,
start: u32,
goal: UVec2,
options: PathOptions,
}
#[derive(Clone, Debug, Default)]
pub struct PathFinder {
nodes: Vec<Node>,
open: BinaryHeap<Open>,
stamp: u32,
expanded: u64,
search: Option<Search>,
}
impl PathFinder {
pub fn new() -> Self {
Self::default()
}
pub fn reserve(&mut self, size: UVec2) -> Result<(), PathError> {
let count = cell_count(size)?;
if self.nodes.len() < count {
self.nodes.resize(count, Node::default());
}
Ok(())
}
pub fn find_path<G: NavGrid + ?Sized>(
&mut self,
grid: &G,
start: UVec2,
goal: UVec2,
options: &PathOptions,
path: &mut Vec<UVec2>,
) -> Result<PathStatus, PathError> {
self.search = None;
self.expanded = 0;
path.clear();
options.validate()?;
let size = grid.size();
self.reserve(size)?;
check_bounds(size, start)?;
check_bounds(size, goal)?;
if start == goal {
path.push(start);
return Ok(PathStatus::Found { cost: 0.0 });
}
if checked_cost(grid, goal, options.min_cost)?.is_none() {
return Ok(PathStatus::Unreachable);
}
self.stamp = self.stamp.wrapping_add(1);
if self.stamp == 0 {
self.nodes.fill(Node::default());
self.stamp = 1;
}
self.open.clear();
let start_index = index(size, start);
self.nodes[start_index] = Node {
stamp: self.stamp,
closed: false,
g: 0.0,
parent: u32::MAX,
};
self.push(start_index as u32, 0.0, heuristic(start, goal, options));
self.search = Some(Search {
size,
start: start_index as u32,
goal,
options: *options,
});
self.run(grid, options.budget, path)
}
pub fn resume<G: NavGrid + ?Sized>(
&mut self,
grid: &G,
budget: Option<u32>,
path: &mut Vec<UVec2>,
) -> Result<PathStatus, PathError> {
path.clear();
let search = self.search.ok_or(PathError::NoSearch)?;
if budget == Some(0) {
return Err(PathError::InvalidOptions);
}
if grid.size() != search.size {
return Err(PathError::GridResized);
}
self.run(grid, budget, path)
}
pub fn is_pending(&self) -> bool {
self.search.is_some()
}
pub fn cancel(&mut self) {
self.search = None;
}
pub fn expanded(&self) -> u64 {
self.expanded
}
#[cfg(test)]
pub(super) fn buffers(&self) -> (usize, *const u8, usize) {
(
self.nodes.capacity(),
self.nodes.as_ptr().cast(),
self.open.capacity(),
)
}
#[cfg(test)]
pub(super) fn set_stamp(&mut self, stamp: u32) {
self.stamp = stamp;
}
fn push(&mut self, index: u32, g: f32, h: f32) {
self.open.push(Open { f: g + h, g, index });
}
fn run<G: NavGrid + ?Sized>(
&mut self,
grid: &G,
mut budget: Option<u32>,
path: &mut Vec<UVec2>,
) -> Result<PathStatus, PathError> {
let Some(search) = self.search else {
return Err(PathError::NoSearch);
};
let Search {
size,
goal,
options,
..
} = search;
let goal_index = index(size, goal) as u32;
let rule = options.neighborhood.corner_rule();
loop {
if budget == Some(0) {
return Ok(PathStatus::Pending);
}
let Some(open) = self.open.pop() else {
self.search = None;
return Ok(PathStatus::Unreachable);
};
let node = &mut self.nodes[open.index as usize];
if node.closed || open.g > node.g {
continue;
}
node.closed = true;
self.expanded += 1;
if let Some(remaining) = &mut budget {
*remaining -= 1;
}
if open.index == goal_index {
self.reconstruct(size, search.start, goal_index, path);
self.search = None;
return Ok(PathStatus::Found { cost: open.g });
}
let cell = cell_of(size, open.index as usize);
for &step in options.neighborhood.steps() {
let Some(next) = offset(size, cell, step) else {
continue;
};
let next_index = index(size, next);
let known = self.nodes[next_index];
let current = known.stamp == self.stamp;
if current && known.closed {
continue;
}
if !corner_open(grid, cell, step, rule) {
continue;
}
let cost = match checked_cost(grid, next, options.min_cost) {
Ok(Some(cost)) => cost,
Ok(None) => continue,
Err(error) => {
self.search = None;
return Err(error);
}
};
let g = open.g + cost * step_length(step);
if !current || g < known.g {
self.nodes[next_index] = Node {
stamp: self.stamp,
closed: false,
g,
parent: open.index,
};
self.push(next_index as u32, g, heuristic(next, goal, &options));
}
}
}
}
fn reconstruct(&self, size: UVec2, start: u32, goal: u32, path: &mut Vec<UVec2>) {
let mut current = goal;
loop {
path.push(cell_of(size, current as usize));
if current == start {
break;
}
current = self.nodes[current as usize].parent;
}
path.reverse();
}
}
fn heuristic(cell: UVec2, goal: UVec2, options: &PathOptions) -> f32 {
let delta = (cell.as_ivec2() - goal.as_ivec2()).abs().as_vec2();
let distance = match options.neighborhood {
Neighborhood::Four => delta.x + delta.y,
Neighborhood::Eight(_) => {
delta.max_element() + (std::f32::consts::SQRT_2 - 1.0) * delta.min_element()
}
};
distance * options.min_cost
}