1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
|
use std::{
collections::{BinaryHeap, HashMap},
hash::Hash,
};
use crate::{map::Floor, pos::Pos};
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
struct Node {
estimated_cost: u16,
cost: u16,
pos: Pos,
}
impl PartialOrd for Node {
fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
Some(self.cmp(other))
}
}
impl Ord for Node {
fn cmp(&self, other: &Self) -> std::cmp::Ordering {
self.estimated_cost
.cmp(&other.estimated_cost)
.then(self.cost.cmp(&other.cost))
.reverse()
}
}
/// A* pathfinding implementation
///
/// # Arguments
/// * `start` - Starting node (owned)
/// * `goal` - Goal node (borrowed reference)
/// * `floor` - Dungeon floor contaning all tiles
///
/// # Returns
/// * `Some((path, total_cost))` if path found
/// * `None` if no path exists
pub fn astar(start: Pos, goal: Pos, floor: &Floor) -> Option<(Vec<Pos>, u16)> {
// list of nodes we need to look at next (BinaryHeap for priority queue behavior)
let mut open_set = BinaryHeap::new();
// The best known source node to get to a
// destination node with minimum weight cost (HashMap for O(1) access)
let mut came_from: HashMap<Pos, Pos> = HashMap::new();
// The current known cost to get to a given node from the start node
// (HashMap for O(1) access)
let mut known_cost_map: HashMap<Pos, u16> = HashMap::new();
// Initialize with start node (add to known cost map and nodes to explore)
known_cost_map.insert(start, 0);
open_set.push(Node {
cost: 0,
estimated_cost: 0,
pos: start,
});
// Explore the nodes until we run out of nodes
while let Some(Node { cost, pos, .. }) = open_set.pop() {
// Goal reached! Reconstruct the path
if pos == goal {
return Some((reconstruct_path(&came_from, pos), cost));
}
// Explore neighbors
for neighbor in floor.neighbors(&pos) {
let tentative_cost = cost + 1;
// If the path we are about to take is worse then the best known so far
// for out neighbor, then we should not take it!
if let Some(known_cost) = known_cost_map.get(&neighbor)
&& *known_cost <= tentative_cost
{
continue;
}
// We have found a better path, we should update, so
// our structures to reflect this
came_from.insert(neighbor, pos);
known_cost_map.insert(neighbor, tentative_cost);
open_set.push(Node {
cost: tentative_cost,
estimated_cost: tentative_cost + goal.manhattan(neighbor),
pos: neighbor,
});
}
}
None // No path found
}
/// Reconstruct path from goal to start using came_from map
fn reconstruct_path(came_from: &HashMap<Pos, Pos>, mut current: Pos) -> Vec<Pos> {
let mut path = vec![current];
// Backtrack from goal to start
while let Some(prev) = came_from.get(¤t) {
current = *prev;
path.push(current);
}
path.reverse();
path
}
|
