quantized_pathfinding/directed/
quantized_astar.rs

1//use std::ops::Add;
2//use std::hash::Hash;
3use std::fmt::Debug;
4use pathfinding::directed::astar::astar;
5use pathfinding::num_traits::Zero;
6
7use crate::utils::quantizer_3d::Quantizer3d;
8use crate::traits::FiniteFloat;
9
10// convenient but less performant than quantized_astar
11pub fn quantized_astar_auto<T: FiniteFloat, Cost, FN, IN, FH, FS>(
12    quantizer: &Quantizer3d<T>,
13    start: (T, T, T),
14    mut successors: FN,
15    mut heuristic: FH,
16    mut success: FS,
17) -> Option<(Vec<(T,T,T)>, Cost)>
18where
19    Cost: Zero + Ord + Copy + Debug,
20    FN: FnMut(&(T, T, T)) -> IN,
21    IN: IntoIterator<Item = ((T, T, T), Cost)>,
22    FH: FnMut(&(T, T, T)) -> Cost,
23    FS: FnMut(&(T, T, T)) -> bool,    
24{
25    let start_n: (usize, usize, usize)  = quantizer.quantize(start);
26    let result = astar(
27        &start_n,
28        |n| {
29            let dequantized = quantizer.dequantize(*n);
30            successors(&dequantized)
31                .into_iter()
32                .map(|(pos, cost)| (quantizer.quantize(pos), cost))
33                .collect::<Vec<_>>()
34        },
35        |n| heuristic(&quantizer.dequantize(*n)),
36        |n| success(&quantizer.dequantize(*n)),
37    );
38    result.map(|(paths, cost)| {
39        let paths_in_float = paths.into_iter()
40            .map(|q| quantizer.dequantize(q))
41            .collect();
42        (paths_in_float, cost)
43    })
44}
45
46pub fn quantized_astar<T: FiniteFloat, Cost, FN, IN, FH, FS>(
47    quantizer: &Quantizer3d<T>,
48    start: (T, T, T),
49    mut successors: FN,
50    mut heuristic: FH,
51    mut success: FS,
52) -> Option<(Vec<(T,T,T)>, Cost)>
53where
54    Cost: Zero + Ord + Copy + Debug,
55    FN: FnMut(&(usize, usize, usize)) -> IN,
56    IN: IntoIterator<Item = ((usize, usize, usize), Cost)>,
57    FH: FnMut(&(usize, usize, usize)) -> Cost,
58    FS: FnMut(&(usize, usize, usize)) -> bool,    
59{
60    let start_n: (usize, usize, usize)  = quantizer.quantize(start);
61    let result = astar(
62        &start_n,
63        |n| {
64            successors(n)
65        },
66        |n| {
67            heuristic(n)
68        },
69        |n| {
70            success(n)
71        },
72    );
73    result.map(|(paths, cost)| {
74        let paths_in_float = paths.into_iter()
75            .map(|q| quantizer.dequantize(q))
76            .collect();
77        (paths_in_float, cost)
78    })
79}