use super::{
NavGrid, Neighborhood, PathError, 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)]
struct Open {
distance: f32,
index: u32,
}
impl Ord for Open {
fn cmp(&self, other: &Self) -> Ordering {
other
.distance
.total_cmp(&self.distance)
.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, Debug, Default)]
pub struct DistanceField {
size: UVec2,
neighborhood: Neighborhood,
distances: Vec<f32>,
open: BinaryHeap<Open>,
}
impl DistanceField {
pub fn new() -> Self {
Self::default()
}
pub fn compute<G: NavGrid + ?Sized>(
&mut self,
grid: &G,
goals: impl IntoIterator<Item = UVec2>,
neighborhood: Neighborhood,
) -> Result<(), PathError> {
let result = self.fill(grid, goals, neighborhood);
if result.is_err() {
self.size = UVec2::ZERO;
self.distances.clear();
}
self.open.clear();
result
}
fn fill<G: NavGrid + ?Sized>(
&mut self,
grid: &G,
goals: impl IntoIterator<Item = UVec2>,
neighborhood: Neighborhood,
) -> Result<(), PathError> {
let size = grid.size();
let count = cell_count(size)?;
self.size = size;
self.neighborhood = neighborhood;
self.distances.clear();
self.distances.resize(count, f32::INFINITY);
self.open.clear();
for goal in goals {
check_bounds(size, goal)?;
let goal_index = index(size, goal);
if checked_cost(grid, goal, 0.0)?.is_some() && self.distances[goal_index] != 0.0 {
self.distances[goal_index] = 0.0;
self.open.push(Open {
distance: 0.0,
index: goal_index as u32,
});
}
}
let rule = neighborhood.corner_rule();
while let Some(open) = self.open.pop() {
let current = open.index as usize;
if open.distance > self.distances[current] {
continue;
}
let cell = cell_of(size, current);
let Some(cost) = checked_cost(grid, cell, 0.0)? else {
continue;
};
for &step in neighborhood.steps() {
let Some(from) = offset(size, cell, step) else {
continue;
};
if !corner_open(grid, from, -step, rule) || !grid.walkable(from) {
continue;
}
let from_index = index(size, from);
let distance = open.distance + cost * step_length(step);
if distance < self.distances[from_index] {
self.distances[from_index] = distance;
self.open.push(Open {
distance,
index: from_index as u32,
});
}
}
}
Ok(())
}
pub fn size(&self) -> UVec2 {
self.size
}
pub fn distance(&self, cell: UVec2) -> Option<f32> {
if !cell.cmplt(self.size).all() {
return None;
}
let distance = self.distances[index(self.size, cell)];
distance.is_finite().then_some(distance)
}
pub fn next_step<G: NavGrid + ?Sized>(&self, grid: &G, cell: UVec2) -> Option<UVec2> {
if grid.size() != self.size {
return None;
}
let here = self.distance(cell)?;
let rule = self.neighborhood.corner_rule();
let mut best: Option<(f32, UVec2)> = None;
for &step in self.neighborhood.steps() {
let Some(next) = offset(self.size, cell, step) else {
continue;
};
let (Some(distance), Some(cost)) = (self.distance(next), grid.cost(next)) else {
continue;
};
if !corner_open(grid, cell, step, rule) {
continue;
}
let total = distance + cost * step_length(step);
if best.is_none_or(|(best, _)| total < best) {
best = Some((total, next));
}
}
best.filter(|&(_, next)| self.distances[index(self.size, next)] < here)
.map(|(_, next)| next)
}
}