hierarchical_pathfinding/graph/
a_star.rs

1use super::*;
2use crate::neighbors::Neighborhood;
3
4use std::cmp::Ordering;
5use std::collections::BinaryHeap;
6
7pub fn a_star_search<N: Neighborhood>(
8    nodes: &NodeList,
9    start: NodeID,
10    goal: NodeID,
11    neighborhood: &N,
12    size_hint: usize,
13) -> Option<Path<NodeID>> {
14    if start == goal {
15        return Some(Path::from_slice(&[start, start], 0));
16    }
17    let mut visited = NodeIDMap::with_capacity(size_hint);
18    let mut next = BinaryHeap::with_capacity(size_hint / 2);
19    next.push(HeuristicElement(start, 0, 0));
20    visited.insert(start, (0, start));
21
22    while let Some(HeuristicElement(current_id, current_cost, _)) = next.pop() {
23        if current_id == goal {
24            break;
25        }
26        match current_cost.cmp(&visited[&current_id].0) {
27            Ordering::Greater => continue,
28            Ordering::Equal => {}
29            Ordering::Less => panic!("Binary Heap failed"),
30        }
31
32        let current = &nodes[current_id];
33
34        for (&other_id, path) in current.edges.iter() {
35            let other_cost = current_cost + path.cost();
36            let other = &nodes[other_id];
37
38            let mut needs_visit = true;
39            if let Some((prev_cost, prev_id)) = visited.get_mut(&other_id) {
40                if *prev_cost > other_cost {
41                    *prev_cost = other_cost;
42                    *prev_id = current_id;
43                } else {
44                    needs_visit = false;
45                }
46            } else {
47                visited.insert(other_id, (other_cost, current_id));
48            }
49
50            if needs_visit {
51                let heuristic = neighborhood.heuristic(current.pos, other.pos);
52                next.push(HeuristicElement(
53                    other_id,
54                    other_cost,
55                    other_cost + heuristic,
56                ));
57            }
58        }
59    }
60
61    if !visited.contains_key(&goal) {
62        return None;
63    }
64
65    let steps = {
66        let mut steps = vec![];
67        let mut current = goal;
68
69        while current != start {
70            steps.push(current);
71            let (_, prev) = visited[&current];
72            current = prev;
73        }
74        steps.push(start);
75        steps.reverse();
76        steps
77    };
78
79    Some(Path::new(steps, visited[&goal].0))
80}
hierarchical_pathfinding/graph/a_star.rs

hierarchical_pathfinding/graph/
a_star.rs
