use std::cmp::Ordering;
use std::collections::{BinaryHeap, HashMap};
struct Node {
position: (i32, i32),
cost: i32,
estimated_total: i32,
}
impl Eq for Node {}
impl PartialEq for Node {
fn eq(&self, other: &Self) -> bool {
self.estimated_total == other.estimated_total
}
}
impl Ord for Node {
fn cmp(&self, other: &Self) -> Ordering {
other.estimated_total.cmp(&self.estimated_total)
}
}
impl PartialOrd for Node {
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
Some(self.cmp(other))
}
}
fn manhattan(a: (i32, i32), b: (i32, i32)) -> i32 {
(a.0 - b.0).abs() + (a.1 - b.1).abs()
}
fn chebyshev(a: (i32, i32), b: (i32, i32)) -> i32 {
(a.0 - b.0).abs().max((a.1 - b.1).abs())
}
const CARDINAL_NEIGHBORS: [(i32, i32); 4] = [(0, -1), (1, 0), (0, 1), (-1, 0)];
const ALL_NEIGHBORS: [(i32, i32); 8] = [
(0, -1),
(1, 0),
(0, 1),
(-1, 0),
(-1, -1),
(1, -1),
(-1, 1),
(1, 1),
];
pub fn astar(
start: (i32, i32),
goal: (i32, i32),
is_walkable: impl Fn(i32, i32) -> bool,
allow_diagonal: bool,
) -> Option<Vec<(i32, i32)>> {
if start == goal {
return Some(vec![start]);
}
if !is_walkable(start.0, start.1) || !is_walkable(goal.0, goal.1) {
return None;
}
let heuristic: fn((i32, i32), (i32, i32)) -> i32 =
if allow_diagonal { chebyshev } else { manhattan };
let mut open = BinaryHeap::new();
let mut came_from: HashMap<(i32, i32), (i32, i32)> = HashMap::new();
let mut cost_so_far: HashMap<(i32, i32), i32> = HashMap::new();
open.push(Node {
position: start,
cost: 0,
estimated_total: heuristic(start, goal),
});
cost_so_far.insert(start, 0);
let neighbors: &[(i32, i32)] = if allow_diagonal {
&ALL_NEIGHBORS
} else {
&CARDINAL_NEIGHBORS
};
while let Some(current) = open.pop() {
if current.position == goal {
let mut path = Vec::new();
let mut position = goal;
while position != start {
path.push(position);
position = came_from[&position];
}
path.push(start);
path.reverse();
return Some(path);
}
let current_cost = cost_so_far[¤t.position];
if current.cost > current_cost {
continue;
}
for &(delta_column, delta_row) in neighbors {
let neighbor = (
current.position.0 + delta_column,
current.position.1 + delta_row,
);
if !is_walkable(neighbor.0, neighbor.1) {
continue;
}
let new_cost = current_cost + 1;
if !cost_so_far.contains_key(&neighbor) || new_cost < cost_so_far[&neighbor] {
cost_so_far.insert(neighbor, new_cost);
came_from.insert(neighbor, current.position);
open.push(Node {
position: neighbor,
cost: new_cost,
estimated_total: new_cost + heuristic(neighbor, goal),
});
}
}
}
None
}