quantized_pathfinding/directed/
quantized_astar.rs1use 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
10pub 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}