use super::{
DiagonalRule, NavGrid, Neighborhood, PathError, STEPS, check_bounds, checked_cost, corner_open,
offset,
};
use glam::{DVec2, IVec2, UVec2, Vec2};
pub fn line_of_sight<G: NavGrid + ?Sized>(
grid: &G,
from: UVec2,
to: UVec2,
neighborhood: Neighborhood,
clearance: Vec2,
) -> Result<bool, PathError> {
check_bounds(grid.size(), from)?;
check_bounds(grid.size(), to)?;
check_clearance(clearance)?;
trace(
grid,
from,
to,
neighborhood.corner_rule(),
clearance,
|cell, _| Ok(grid.walkable(cell)),
)
}
pub fn smooth_path<G: NavGrid + ?Sized>(
grid: &G,
path: &mut Vec<UVec2>,
neighborhood: Neighborhood,
clearance: Vec2,
) -> Result<(), PathError> {
for &cell in path.iter() {
check_bounds(grid.size(), cell)?;
}
check_clearance(clearance)?;
if path.len() < 3 {
return Ok(());
}
let rule = neighborhood.corner_rule();
let along = |from, to| segment_cost(grid, from, to, DiagonalRule::Always, Vec2::ZERO);
let mut kept = 1;
let mut anchor = path[0];
let mut original = 0.0;
for next in 1..path.len() {
original += along(path[next - 1], path[next])?.unwrap_or(f32::INFINITY);
if next == 1 {
continue;
}
let direct = segment_cost(grid, anchor, path[next], rule, clearance)?;
if direct.is_some_and(|direct| direct <= original * (1.0 + 1e-5)) {
continue;
}
anchor = path[next - 1];
path[kept] = anchor;
kept += 1;
original = along(anchor, path[next])?.unwrap_or(f32::INFINITY);
}
path[kept] = path[path.len() - 1];
path.truncate(kept + 1);
Ok(())
}
fn check_clearance(clearance: Vec2) -> Result<(), PathError> {
if clearance
.to_array()
.iter()
.all(|axis| (0.0..0.5).contains(axis))
{
Ok(())
} else {
Err(PathError::InvalidOptions)
}
}
fn segment_cost<G: NavGrid + ?Sized>(
grid: &G,
from: UVec2,
to: UVec2,
rule: DiagonalRule,
clearance: Vec2,
) -> Result<Option<f32>, PathError> {
let mut total = 0.0;
let clear = trace(grid, from, to, rule, clearance, |cell, length| {
Ok(match checked_cost(grid, cell, 0.0)? {
Some(cost) => {
total += cost * length;
true
}
None => false,
})
})?;
Ok(clear.then_some(total))
}
fn trace<G: NavGrid + ?Sized>(
grid: &G,
from: UVec2,
to: UVec2,
rule: DiagonalRule,
clearance: Vec2,
mut visit: impl FnMut(UVec2, f32) -> Result<bool, PathError>,
) -> Result<bool, PathError> {
let (start, end) = (from.as_dvec2() + 0.5, to.as_dvec2() + 0.5);
let crowded = |cell: UVec2| {
clearance.cmpgt(Vec2::ZERO).any()
&& STEPS.iter().any(|&step| {
offset(grid.size(), cell, step).is_some_and(|side| {
!grid.walkable(side)
&& sweeps(start, end, side.as_dvec2(), clearance.as_dvec2())
})
})
};
let delta = to.as_ivec2() - from.as_ivec2();
let (dx, dy) = (delta.x.unsigned_abs(), delta.y.unsigned_abs());
let step = delta.signum();
let length = delta.as_vec2().length();
let (mut crossed_x, mut crossed_y) = (0_u64, 0_u64);
let mut cell = from;
let mut entered = 0.0_f32;
loop {
let next_x =
(dx > 0 && crossed_x < u64::from(dx)).then(|| (2 * crossed_x + 1) * u64::from(dy));
let next_y =
(dy > 0 && crossed_y < u64::from(dy)).then(|| (2 * crossed_y + 1) * u64::from(dx));
let (move_x, move_y) = match (next_x, next_y) {
(None, None) => return Ok(visit(cell, (1.0 - entered) * length)? && !crowded(cell)),
(Some(_), None) => (true, false),
(None, Some(_)) => (false, true),
(Some(x), Some(y)) => (x <= y, y <= x),
};
let exit = if move_x {
(2 * crossed_x + 1) as f32 / (2 * dx) as f32
} else {
(2 * crossed_y + 1) as f32 / (2 * dy) as f32
};
if !visit(cell, (exit - entered) * length)? || crowded(cell) {
return Ok(false);
}
entered = exit;
let advance = IVec2::new(
if move_x { step.x } else { 0 },
if move_y { step.y } else { 0 },
);
if move_x && move_y && !corner_open(grid, cell, advance, rule) {
return Ok(false);
}
crossed_x += u64::from(move_x);
crossed_y += u64::from(move_y);
cell = (cell.as_ivec2() + advance).as_uvec2();
}
}
fn sweeps(start: DVec2, end: DVec2, min: DVec2, clearance: DVec2) -> bool {
let (low, high) = (min - clearance, min + 1.0 + clearance);
let delta = end - start;
let (mut enter, mut exit) = (0.0_f64, 1.0_f64);
for axis in 0..2 {
if delta[axis] == 0.0 {
if start[axis] <= low[axis] || start[axis] >= high[axis] {
return false;
}
} else {
let a = (low[axis] - start[axis]) / delta[axis];
let b = (high[axis] - start[axis]) / delta[axis];
enter = enter.max(a.min(b));
exit = exit.min(a.max(b));
}
}
enter < exit
}