azalea/pathfinder/astar/
heap.rs

1use std::{
2    cmp::{self, Reverse},
3    collections::BinaryHeap,
4};
5
6use radix_heap::RadixHeapMap;
7
8#[derive(Default)]
9pub struct PathfinderHeap {
10    /// Key is f_score.to_bits(), value is (g_score, index)
11    ///
12    /// As long as the f_score is positive, comparing it as bits is fine. Also,
13    /// it has to be `Reverse`d to make it a min-heap.
14    radix_heap: RadixHeapMap<Reverse<u32>, (f32, u32)>,
15    // fallback
16    binary_heap: BinaryHeap<WeightedNode>,
17}
18impl PathfinderHeap {
19    pub fn new() -> Self {
20        Self::default()
21    }
22
23    pub fn push(&mut self, item: WeightedNode) {
24        if let Some(top) = self.radix_heap.top() {
25            // this can happen when the heuristic wasn't an underestimate, so just fall back
26            // to a binary heap in those cases
27            if item.f_score < f32::from_bits(top.0) {
28                self.binary_heap.push(item);
29                return;
30            }
31        }
32        self.radix_heap
33            .push(Reverse(item.f_score.to_bits()), (item.g_score, item.index))
34    }
35    pub fn pop(&mut self) -> Option<WeightedNode> {
36        self.binary_heap.pop().or_else(|| {
37            self.radix_heap
38                .pop()
39                .map(|(f_score, (g_score, index))| WeightedNode {
40                    f_score: f32::from_bits(f_score.0),
41                    g_score,
42                    index,
43                })
44        })
45    }
46}
47
48#[derive(PartialEq, Debug)]
49#[repr(C)]
50pub struct WeightedNode {
51    /// Sum of the g_score and heuristic
52    pub f_score: f32,
53    /// The actual cost to get to this node
54    pub g_score: f32,
55    pub index: u32,
56}
57
58impl Ord for WeightedNode {
59    #[inline]
60    fn cmp(&self, other: &Self) -> cmp::Ordering {
61        // intentionally inverted to make the BinaryHeap a min-heap
62        match other.f_score.total_cmp(&self.f_score) {
63            cmp::Ordering::Equal => self.g_score.total_cmp(&other.g_score),
64            s => s,
65        }
66    }
67}
68impl Eq for WeightedNode {}
69impl PartialOrd for WeightedNode {
70    #[inline]
71    fn partial_cmp(&self, other: &Self) -> Option<cmp::Ordering> {
72        Some(self.cmp(other))
73    }
74}
75
76#[cfg(test)]
77mod tests {
78    use super::*;
79
80    fn weighted_node(f: f32, g: f32) -> WeightedNode {
81        WeightedNode {
82            f_score: f,
83            g_score: g,
84            index: 0,
85        }
86    }
87
88    #[test]
89    fn test_weighted_node_eq() {
90        let a = weighted_node(0., 0.);
91        let b = weighted_node(0., 0.);
92        assert!(a == b);
93    }
94    #[test]
95    fn test_weighted_node_le() {
96        let a = weighted_node(1., 0.);
97        let b = weighted_node(0., 0.);
98        assert_eq!(a.cmp(&b), cmp::Ordering::Less);
99        assert!(a.le(&b));
100    }
101    #[test]
102    fn test_weighted_node_le_g() {
103        let a = weighted_node(0., 1.);
104        let b = weighted_node(0., 0.);
105        assert_eq!(a.cmp(&b), cmp::Ordering::Greater);
106        assert!(!a.le(&b));
107    }
108}
azalea/pathfinder/astar/heap.rs

azalea/pathfinder/astar/
heap.rs
