use alloc::boxed::Box;
use alloc::collections::BinaryHeap;
use alloc::vec;
use alloc::vec::Vec;
use core::cmp::Ordering;
use super::Corridor;
use crate::decomp::{CellId, Decomposition, PortalId};
use crate::error::PathPlanError;
use crate::geom::point::Point2D;
#[derive(Clone, Copy, Debug, PartialEq)]
struct Open {
f: f64,
counter: u32,
cell: CellId,
}
impl Eq for Open {}
impl Ord for Open {
fn cmp(&self, other: &Self) -> Ordering {
other
.f
.total_cmp(&self.f)
.then_with(|| other.counter.cmp(&self.counter))
}
}
impl PartialOrd for Open {
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
Some(self.cmp(other))
}
}
#[derive(Clone, Debug, Default)]
pub struct SearchScratch {
g_score: Vec<f64>,
came_from: Vec<u32>,
closed: Vec<bool>,
open: BinaryHeap<Open>,
counter: u32,
}
impl SearchScratch {
pub(crate) fn reset(&mut self, cell_count: usize) {
self.g_score.clear();
self.g_score.resize(cell_count, f64::INFINITY);
self.came_from.clear();
self.came_from.resize(cell_count, u32::MAX);
self.closed.clear();
self.closed.resize(cell_count, false);
self.open.clear();
self.counter = 0;
}
}
pub fn search(
decomp: &Decomposition,
start: Point2D,
goal: Point2D,
scratch: &mut SearchScratch,
) -> Result<Corridor, PathPlanError> {
let start_cell = decomp.locate(start).ok_or(PathPlanError::NoPathFound)?;
let goal_cell = decomp.locate(goal).ok_or(PathPlanError::NoPathFound)?;
if start_cell == goal_cell {
return Ok(Corridor { cells: vec![start_cell], portals: Vec::new() });
}
if let Some(found) = run(decomp, start_cell, goal_cell, goal, true, scratch) {
return Ok(found);
}
run(decomp, start_cell, goal_cell, goal, false, scratch).ok_or(PathPlanError::NoPathFound)
}
fn run(
decomp: &Decomposition,
start_cell: CellId,
goal_cell: CellId,
goal_point: Point2D,
monotone: bool,
scratch: &mut SearchScratch,
) -> Option<Corridor> {
scratch.reset(decomp.cell_count());
let travel = if goal_point.x > decomp.cells()[start_cell as usize].br.x { 1.0 } else { -1.0 };
scratch.g_score[start_cell as usize] = 0.0;
let start_f = decomp.cells()[start_cell as usize].centroid().distance(goal_point);
let counter = scratch.next_counter();
scratch.open.push(Open { f: start_f, counter, cell: start_cell });
while let Some(Open { cell, .. }) = scratch.open.pop() {
if scratch.closed[cell as usize] {
continue;
}
scratch.closed[cell as usize] = true;
if cell == goal_cell {
return Some(reconstruct(decomp, scratch, start_cell, goal_cell));
}
let expand: Box<dyn Iterator<Item = (PortalId, CellId)>> = if !monotone {
Box::new(decomp.right_neighbours(cell).chain(decomp.left_neighbours(cell)))
} else if travel > 0.0 {
Box::new(decomp.right_neighbours(cell))
} else {
Box::new(decomp.left_neighbours(cell))
};
for (portal, next) in expand {
if scratch.closed[next as usize] {
continue;
}
let step = decomp.portals()[portal as usize].length();
let tentative = scratch.g_score[cell as usize] + step;
if tentative < scratch.g_score[next as usize] {
scratch.g_score[next as usize] = tentative;
scratch.came_from[next as usize] = cell;
let h = decomp.cells()[next as usize].centroid().distance(goal_point);
let counter = scratch.next_counter();
scratch.open.push(Open { f: tentative + h, counter, cell: next });
}
}
}
None
}
impl SearchScratch {
fn next_counter(&mut self) -> u32 {
self.counter += 1;
self.counter
}
}
fn reconstruct(
decomp: &Decomposition,
scratch: &SearchScratch,
start: CellId,
goal: CellId,
) -> Corridor {
let mut cells: Vec<CellId> = vec![goal];
let mut current = goal;
while current != start {
current = scratch.came_from[current as usize] as CellId;
cells.push(current);
}
cells.reverse();
let mut portals: Vec<PortalId> = Vec::with_capacity(cells.len().saturating_sub(1));
for pair in cells.windows(2) {
let (a, b) = (pair[0], pair[1]);
let found = decomp
.right_portals(a)
.iter()
.chain(decomp.left_portals(a).iter())
.find(|p| {
let portal = decomp.portals()[**p as usize];
(portal.left == b && portal.right == a) || (portal.right == b && portal.left == a)
})
.copied()
.expect("came_from is only ever set across a portal");
portals.push(found);
}
Corridor { cells, portals }
}