use std::collections::BinaryHeap;
use movingai::Coords2D;
use movingai::Map2D;
use crate::node::Node;
use crate::utils::{direction, distance, rewind};
use crate::Route;
#[derive(Copy, Clone)]
enum Direction {
Vertical(i32),
Horizontal(i32),
Diagonal(i32, i32),
}
#[inline]
pub fn jps_path<U, T: Map2D<U>>(map: &T, start: Coords2D, goal: Coords2D) -> Option<Route> {
if start == goal {
return Some(Route::from((0.0, vec![])));
}
let start_node = Node::new(0.0, distance(start, goal), start, start);
let prev_x = start_node.position.0 - 1;
let next_x = start_node.position.0 + 1;
let prev_y = start_node.position.1 - 1;
let next_y = start_node.position.1 + 1;
let capacity = (1 + next_x - prev_x) * (1 + next_y - prev_y);
let mut open = BinaryHeap::with_capacity(capacity);
let mut closed = Vec::with_capacity(capacity);
for x in prev_x..=next_x {
for y in prev_y..=next_y {
open.push(Node::from_parent(&start_node, (x, y), goal));
}
}
closed.push(start_node);
while let Some(node_current) = open.pop() {
if node_current.position == goal {
closed.append(&mut open.into_vec());
let path = rewind(&node_current, &closed);
let route = Route::from((node_current.g, path));
return Some(route);
}
if closed.contains(&node_current) {
continue;
}
let direction = direction(node_current.position, node_current.parent);
if let Some(nodes) = check_jump(&node_current, map, direction, goal) {
for node in nodes {
open.push(node);
}
}
closed.push(node_current);
}
None
}
#[inline]
fn check_jump<U, T: Map2D<U>>(
parent: &Node,
map: &T,
(dx, dy): (i32, i32),
goal: Coords2D,
) -> Option<Vec<Node>> {
if dx != 0 {
if dy != 0 {
expand(map, &parent, Direction::Diagonal(dx, dy), goal)
} else {
expand(map, &parent, Direction::Horizontal(dx), goal)
}
} else if dy != 0 {
expand(map, &parent, Direction::Vertical(dy), goal)
} else {
None
}
}
#[inline]
fn forced_horizontal<U, T: Map2D<U>>(
nodes: &mut Vec<Node>,
map: &T,
check_node: &Node,
direction: i32,
goal: Coords2D,
) {
let (check_x, check_y) = check_node.position;
let next_x = (check_x as i32 + direction) as usize;
let up_y = (check_y as i32 - 1) as usize;
let down_y = (check_y as i32 + 1) as usize;
if !map.is_traversable((check_x, up_y)) && map.is_traversable((next_x, up_y)) {
nodes.push(Node::from_parent(&check_node, (next_x, up_y), goal));
}
if !map.is_traversable((check_x, down_y)) && map.is_traversable((next_x, down_y)) {
nodes.push(Node::from_parent(&check_node, (next_x, down_y), goal));
}
}
#[inline]
fn forced_vertical<U, T: Map2D<U>>(
nodes: &mut Vec<Node>,
map: &T,
check_node: &Node,
direction: i32,
goal: Coords2D,
) {
let (check_x, check_y) = check_node.position;
let left_x = (check_x as i32 - 1) as usize;
let right_x = (check_x as i32 + 1) as usize;
let next_y = (check_y as i32 + direction) as usize;
if !map.is_traversable((left_x, check_y)) && map.is_traversable((left_x, next_y)) {
nodes.push(Node::from_parent(&check_node, (left_x, next_y), goal));
}
if !map.is_traversable((right_x, check_y)) && map.is_traversable((right_x, next_y)) {
nodes.push(Node::from_parent(&check_node, (right_x, next_y), goal));
}
}
#[inline]
fn expand<U, T: Map2D<U>>(
map: &T,
start_node: &Node,
direction: Direction,
goal: Coords2D,
) -> Option<Vec<Node>> {
let mut current = *start_node;
let mut nodes = Vec::new();
loop {
if current.position == goal {
nodes.push(current);
return Some(nodes);
}
if !map.is_traversable(current.position) {
return None;
}
let dir = match direction {
Direction::Vertical(vert) => {
forced_vertical(&mut nodes, map, ¤t, vert, goal);
(0, vert)
}
Direction::Horizontal(hor) => {
forced_horizontal(&mut nodes, map, ¤t, hor, goal);
(hor, 0)
}
Direction::Diagonal(hor, vert) => {
if let Some(mut hor_nodes) = expand(map, ¤t, Direction::Horizontal(hor), goal)
{
nodes.append(&mut hor_nodes);
}
if let Some(mut vert_nodes) = expand(map, ¤t, Direction::Vertical(vert), goal)
{
nodes.append(&mut vert_nodes);
}
(hor, vert)
}
};
let next_x = (current.position.0 as i32 + dir.0) as usize;
let next_y = (current.position.1 as i32 + dir.1) as usize;
let next_position = (next_x, next_y);
if !nodes.is_empty() {
let next_node = Node::from_parent(¤t, next_position, goal);
nodes.push(current);
nodes.push(next_node);
return Some(nodes);
}
current = Node::from_parent(start_node, next_position, goal);
}
}