use config::*;
use direction::*;
use error::*;
use grid::*;
use metadata::*;
use num_traits::*;
use path;
use search::*;
use std::ops::*;
fn octile_distance<C>(a: Coord, b: Coord) -> C
where
C: Add<C, Output = C> + Mul<C, Output = C> + FloatConst + NumCast,
{
let dx = (a.x - b.x).abs();
let dy = (a.y - b.y).abs();
let (cardinal, ordinal) = if dx > dy {
(dx - dy, dy)
} else {
(dy - dx, dx)
};
let sqrt_2: C = FloatConst::SQRT_2();
let cardinal: C = NumCast::from(cardinal).expect("Failed to cast to Cost");
let ordinal: C = NumCast::from(ordinal).expect("Failed to cast to Cost");
cardinal + ordinal * sqrt_2
}
fn has_forced_neighbour_cardinal<G>(grid: &G, coord: Coord, direction: CardinalDirection) -> bool
where
G: SolidGrid,
{
if grid.is_solid_or_outside(coord + direction.left90().coord()) {
return !grid.is_solid_or_outside(coord + direction.left45().coord());
}
if grid.is_solid_or_outside(coord + direction.right90().coord()) {
return !grid.is_solid_or_outside(coord + direction.right45().coord());
}
false
}
fn has_forced_neighbour_ordinal<G>(grid: &G, coord: Coord, direction: OrdinalDirection) -> bool
where
G: SolidGrid,
{
if grid.is_solid_or_outside(coord + direction.left135().coord()) {
return !grid.is_solid_or_outside(coord + direction.left90().coord());
}
if grid.is_solid_or_outside(coord + direction.right135().coord()) {
return !grid.is_solid_or_outside(coord + direction.right90().coord());
}
false
}
fn jump_cardinal<G, C>(
grid: &G,
coord: Coord,
direction: CardinalDirection,
goal: Coord,
) -> Option<(Coord, C)>
where
G: SolidGrid,
C: Add<C, Output = C> + One,
{
let neighbour_coord = coord + direction.coord();
if grid.is_solid_or_outside(neighbour_coord) {
return None;
}
if neighbour_coord == goal {
return Some((neighbour_coord, One::one()));
}
if has_forced_neighbour_cardinal(grid, neighbour_coord, direction) {
return Some((neighbour_coord, One::one()));
}
jump_cardinal(grid, neighbour_coord, direction, goal)
.map(|(coord, cost): (_, C)| (coord, cost + One::one()))
}
fn jump_ordinal<G, C>(
grid: &G,
coord: Coord,
direction: OrdinalDirection,
goal: Coord,
) -> Option<(Coord, C)>
where
G: SolidGrid,
C: Add<C, Output = C> + One + FloatConst,
{
let neighbour_coord = coord + direction.coord();
if grid.is_solid_or_outside(neighbour_coord) {
return None;
}
if neighbour_coord == goal {
return Some((neighbour_coord, FloatConst::SQRT_2()));
}
if has_forced_neighbour_ordinal(grid, neighbour_coord, direction) {
return Some((neighbour_coord, FloatConst::SQRT_2()));
}
let (card0, card1) = direction.to_cardinals();
if jump_cardinal::<_, C>(grid, neighbour_coord, card0, goal).is_some()
|| jump_cardinal::<_, C>(grid, neighbour_coord, card1, goal).is_some()
{
return Some((neighbour_coord, FloatConst::SQRT_2()));
}
jump_ordinal(grid, neighbour_coord, direction, goal)
.map(|(coord, cost): (_, C)| (coord, cost + FloatConst::SQRT_2()))
}
impl<C> SearchContext<C>
where
C: Copy
+ Zero
+ One
+ Add<C, Output = C>
+ Mul<C, Output = C>
+ FloatConst
+ NumCast
+ PartialOrd<C>,
{
fn expand_cardinal<G>(
&mut self,
grid: &G,
current_coord: Coord,
current_cost: C,
direction: CardinalDirection,
goal: Coord,
) -> Result<(), Error>
where
G: SolidGrid,
{
if let Some((successor_coord, successor_cost)) =
jump_cardinal::<_, C>(grid, current_coord, direction, goal)
{
self.see_successor(
current_cost + successor_cost,
successor_coord,
direction.direction(),
octile_distance,
goal,
)?;
}
Ok(())
}
fn expand_ordinal<G>(
&mut self,
grid: &G,
current_coord: Coord,
current_cost: C,
direction: OrdinalDirection,
goal: Coord,
) -> Result<(), Error>
where
G: SolidGrid,
{
if let Some((successor_coord, successor_cost)) =
jump_ordinal::<_, C>(grid, current_coord, direction, goal)
{
self.see_successor(
current_cost + successor_cost,
successor_coord,
direction.direction(),
octile_distance,
goal,
)?;
}
Ok(())
}
fn expand_general<G>(
&mut self,
grid: &G,
current_coord: Coord,
current_cost: C,
direction: Direction,
goal: Coord,
) -> Result<(), Error>
where
G: SolidGrid,
{
match direction.typ() {
DirectionType::Cardinal(direction) => {
self.expand_cardinal(grid, current_coord, current_cost, direction, goal)
}
DirectionType::Ordinal(direction) => {
self.expand_ordinal(grid, current_coord, current_cost, direction, goal)
}
}
}
pub fn jump_point_search_octile_distance_heuristic<G>(
&mut self,
grid: &G,
start: Coord,
goal: Coord,
config: SearchConfig,
path: &mut Vec<Direction>,
) -> Result<SearchMetadata<C>, Error>
where
G: SolidGrid,
{
let initial_entry = match self.init(start, |c| c == goal, grid, config, path) {
Ok(initial_entry) => initial_entry,
Err(result) => return result,
};
let goal_index = self
.node_grid
.index_of_coord(goal)
.ok_or(Error::VisitOutsideContext)?;
for direction in Directions {
self.expand_general(grid, start, initial_entry.cost, direction, goal)?;
}
let mut num_nodes_visited = 0;
while let Some(current_entry) = self.priority_queue.pop() {
num_nodes_visited += 1;
if current_entry.node_index == goal_index {
let node = &self.node_grid[goal_index];
path::make_path_jump_points(&self.node_grid, goal, self.seq, path);
return Ok(SearchMetadata {
num_nodes_visited,
cost: node.cost,
length: path.len(),
});
}
let (current_coord, current_cost, direction) = {
let node = &mut self.node_grid[current_entry.node_index];
if node.visited == self.seq {
continue;
}
node.visited = self.seq;
let direction = node.from_parent.expect("Open set node without direction");
(node.coord, node.cost, direction)
};
match direction.typ() {
DirectionType::Cardinal(direction) => {
self.expand_cardinal(grid, current_coord, current_cost, direction, goal)?;
let left = direction.left90();
if grid.is_solid_or_outside(current_coord + left.coord()) {
self.expand_ordinal(
grid,
current_coord,
current_cost,
direction.left45(),
goal,
)?;
}
let right = direction.right90();
if grid.is_solid_or_outside(current_coord + right.coord()) {
self.expand_ordinal(
grid,
current_coord,
current_cost,
direction.right45(),
goal,
)?;
}
}
DirectionType::Ordinal(direction) => {
self.expand_ordinal(grid, current_coord, current_cost, direction, goal)?;
let (left, right) = direction.to_cardinals();
self.expand_cardinal(grid, current_coord, current_cost, left, goal)?;
self.expand_cardinal(grid, current_coord, current_cost, right, goal)?;
let (check_right, check_left) = direction.opposite().to_cardinals();
if grid.is_solid_or_outside(current_coord + check_left.coord()) {
self.expand_ordinal(
grid,
current_coord,
current_cost,
direction.left90(),
goal,
)?;
}
if grid.is_solid_or_outside(current_coord + check_right.coord()) {
self.expand_ordinal(
grid,
current_coord,
current_cost,
direction.right90(),
goal,
)?;
}
}
}
}
Err(Error::NoPath)
}
}