azalea/pathfinder/astar/
mod.rs

1pub mod heap;
2pub mod nodemap;
3
4use std::{
5    fmt::{self, Debug},
6    hash::Hash,
7    time::{Duration, Instant},
8};
9
10use num_format::ToFormattedString;
11use tracing::{debug, trace, warn};
12
13use crate::pathfinder::astar::{
14    heap::{PathfinderHeap, WeightedNode},
15    nodemap::NodeMap,
16};
17
18pub struct Path<P, M>
19where
20    P: Eq + Hash + Copy + Debug,
21{
22    pub movements: Vec<Movement<P, M>>,
23    pub is_partial: bool,
24    /// The A* cost for executing the path.
25    ///
26    /// For Azalea's pathfinder, this is generally the estimated amount of time
27    /// that it takes to complete the path, in ticks.
28    pub cost: f32,
29}
30
31// used for better results when timing out
32// see https://github.com/cabaletta/baritone/blob/1.19.4/src/main/java/baritone/pathing/calc/AbstractNodeCostSearch.java#L68
33const COEFFICIENTS: [f32; 7] = [1.5, 2., 2.5, 3., 4., 5., 10.];
34
35const MIN_IMPROVEMENT: f32 = 0.01;
36
37// Sources:
38// - https://en.wikipedia.org/wiki/A*_search_algorithm
39// - https://github.com/evenfurther/pathfinding/blob/main/src/directed/astar.rs
40// - https://github.com/cabaletta/baritone/blob/1.19.4/src/main/java/baritone/pathing/calc/AbstractNodeCostSearch.java
41pub fn a_star<P, M, HeuristicFn, SuccessorsFn, SuccessFn>(
42    start: P,
43    heuristic: HeuristicFn,
44    mut successors: SuccessorsFn,
45    success: SuccessFn,
46    min_timeout: PathfinderTimeout,
47    max_timeout: PathfinderTimeout,
48) -> Path<P, M>
49where
50    P: Eq + Hash + Copy + Debug,
51    HeuristicFn: Fn(P) -> f32,
52    SuccessorsFn: FnMut(P) -> Vec<Edge<P, M>>,
53    SuccessFn: Fn(P) -> bool,
54{
55    let start_time = Instant::now();
56
57    let mut open_set = PathfinderHeap::new();
58    open_set.push(WeightedNode {
59        g_score: 0.,
60        f_score: 0.,
61        index: 0,
62    });
63    let mut nodes: NodeMap<P> = NodeMap::default();
64    nodes.insert(
65        start,
66        Node {
67            came_from: u32::MAX,
68            g_score: 0.,
69        },
70    );
71
72    let mut best_paths: [u32; 7] = [0; 7];
73    let mut best_path_scores: [f32; 7] = [heuristic(start); 7];
74
75    let mut num_nodes = 0_usize;
76    let mut num_movements = 0;
77
78    while let Some(WeightedNode { index, g_score, .. }) = open_set.pop() {
79        let (&node, node_data) = nodes.get_index(index).unwrap();
80        if g_score > node_data.g_score {
81            continue;
82        }
83
84        num_nodes += 1;
85
86        if success(node) {
87            let best_path = index;
88            log_perf_info(start_time, num_nodes, num_movements);
89
90            return Path {
91                movements: reconstruct_path(nodes, best_path, successors),
92                is_partial: false,
93                cost: g_score,
94            };
95        }
96
97        for neighbor in successors(node) {
98            let tentative_g_score = g_score + neighbor.cost;
99            // let neighbor_heuristic = heuristic(neighbor.movement.target);
100            let neighbor_heuristic;
101            let neighbor_index;
102
103            num_movements += 1;
104
105            match nodes.entry(neighbor.movement.target) {
106                indexmap::map::Entry::Occupied(mut e) => {
107                    if e.get().g_score > tentative_g_score {
108                        neighbor_heuristic = heuristic(*e.key());
109                        neighbor_index = e.index() as u32;
110                        e.insert(Node {
111                            came_from: index,
112                            g_score: tentative_g_score,
113                        });
114                    } else {
115                        continue;
116                    }
117                }
118                indexmap::map::Entry::Vacant(e) => {
119                    neighbor_heuristic = heuristic(*e.key());
120                    neighbor_index = e.index() as u32;
121                    e.insert(Node {
122                        came_from: index,
123                        g_score: tentative_g_score,
124                    });
125                }
126            }
127
128            // we don't update the existing node, which means that the same node might be
129            // present in the open_set multiple times. this is fine because at the start of
130            // the loop we check `g_score > node_data.g_score`.
131            open_set.push(WeightedNode {
132                index: neighbor_index,
133                g_score: tentative_g_score,
134                f_score: tentative_g_score + neighbor_heuristic,
135            });
136
137            for (coefficient_i, &coefficient) in COEFFICIENTS.iter().enumerate() {
138                let node_score = neighbor_heuristic + tentative_g_score / coefficient;
139                if best_path_scores[coefficient_i] - node_score > MIN_IMPROVEMENT {
140                    best_paths[coefficient_i] = neighbor_index;
141                    best_path_scores[coefficient_i] = node_score;
142                }
143            }
144        }
145
146        // check for timeout every ~10ms
147        if num_nodes.is_multiple_of(10_000) {
148            let min_timeout_reached = match min_timeout {
149                PathfinderTimeout::Time(max_duration) => start_time.elapsed() >= max_duration,
150                PathfinderTimeout::Nodes(max_nodes) => num_nodes >= max_nodes,
151            };
152
153            if min_timeout_reached {
154                // means we have a non-empty path
155                if best_paths[6] != 0 {
156                    break;
157                }
158
159                if min_timeout_reached {
160                    let max_timeout_reached = match max_timeout {
161                        PathfinderTimeout::Time(max_duration) => {
162                            start_time.elapsed() >= max_duration
163                        }
164                        PathfinderTimeout::Nodes(max_nodes) => num_nodes >= max_nodes,
165                    };
166
167                    if max_timeout_reached {
168                        // timeout, we're gonna be returning an empty path :(
169                        trace!("A* couldn't find a path in time, returning best path");
170                        break;
171                    }
172                }
173            }
174        }
175    }
176
177    let best_path_idx = determine_best_path_idx(best_paths, 0);
178    log_perf_info(start_time, num_nodes, num_movements);
179    Path {
180        movements: reconstruct_path(nodes, best_paths[best_path_idx], successors),
181        is_partial: true,
182        cost: best_path_scores[best_path_idx],
183    }
184}
185
186fn log_perf_info(start_time: Instant, num_nodes: usize, num_movements: usize) {
187    let elapsed = start_time.elapsed();
188    let elapsed_seconds = elapsed.as_secs_f64();
189    let nodes_per_second = (num_nodes as f64 / elapsed_seconds) as u64;
190    let num_movements_per_second = (num_movements as f64 / elapsed_seconds) as u64;
191    debug!(
192        "Considered {} nodes in {elapsed:?}",
193        num_nodes.to_formatted_string(&num_format::Locale::en)
194    );
195    debug!(
196        "A* ran at {} nodes per second and {} movements per second",
197        nodes_per_second.to_formatted_string(&num_format::Locale::en),
198        num_movements_per_second.to_formatted_string(&num_format::Locale::en),
199    );
200}
201
202fn determine_best_path_idx(best_paths: [u32; 7], start: u32) -> usize {
203    // this basically makes sure we don't create a path that's really short
204
205    for (i, &node) in best_paths.iter().enumerate() {
206        if node != start {
207            return i;
208        }
209    }
210    warn!("No best node found, returning first node");
211    0
212}
213
214fn reconstruct_path<P, M, SuccessorsFn>(
215    nodes: NodeMap<P>,
216    mut current_index: u32,
217    mut successors: SuccessorsFn,
218) -> Vec<Movement<P, M>>
219where
220    P: Eq + Hash + Copy + Debug,
221    SuccessorsFn: FnMut(P) -> Vec<Edge<P, M>>,
222{
223    let mut path = Vec::new();
224    while let Some((&node_position, node)) = nodes.get_index(current_index) {
225        if node.came_from == u32::MAX {
226            break;
227        }
228        let came_from_position = *nodes.get_index(node.came_from).unwrap().0;
229
230        // find the movement data for this successor, we have to do this again because
231        // we don't include the movement data in the Node (as an optimization)
232        let mut best_successor = None;
233        let mut best_successor_cost = f32::INFINITY;
234        for successor in successors(came_from_position) {
235            if successor.movement.target == node_position && successor.cost < best_successor_cost {
236                best_successor_cost = successor.cost;
237                best_successor = Some(successor);
238            }
239        }
240        let Some(found_successor) = best_successor else {
241            warn!(
242                "a successor stopped being possible while reconstructing the path, returning empty path"
243            );
244            return vec![];
245        };
246
247        path.push(Movement {
248            target: node_position,
249            data: found_successor.movement.data,
250        });
251
252        current_index = node.came_from;
253    }
254    path.reverse();
255    path
256}
257
258pub struct Node {
259    pub came_from: u32,
260    pub g_score: f32,
261}
262
263#[derive(Clone, Debug)]
264pub struct Edge<P: Hash + Copy, M> {
265    pub movement: Movement<P, M>,
266    pub cost: f32,
267}
268
269pub struct Movement<P: Hash + Copy, M> {
270    pub target: P,
271    pub data: M,
272}
273
274impl<P: Hash + Copy + Debug, M: Debug> Debug for Movement<P, M> {
275    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
276        f.debug_struct("Movement")
277            .field("target", &self.target)
278            .field("data", &self.data)
279            .finish()
280    }
281}
282impl<P: Hash + Copy + Clone, M: Clone> Clone for Movement<P, M> {
283    fn clone(&self) -> Self {
284        Self {
285            target: self.target,
286            data: self.data.clone(),
287        }
288    }
289}
290
291/// A timeout that the pathfinder will consider when calculating a path.
292///
293/// See [`PathfinderOpts::min_timeout`] and [`PathfinderOpts::max_timeout`] if
294/// you want to modify this.
295///
296/// [`PathfinderOpts::min_timeout`]: super::goto_event::PathfinderOpts::min_timeout
297/// [`PathfinderOpts::max_timeout`]: super::goto_event::PathfinderOpts::max_timeout
298#[derive(Clone, Copy, Debug, PartialEq)]
299pub enum PathfinderTimeout {
300    /// Time out after a certain duration has passed.
301    ///
302    /// This is a good default so you don't waste too much time calculating a
303    /// path if you're on a slow computer.
304    Time(Duration),
305    /// Time out after this many nodes have been considered.
306    ///
307    /// This is useful as an alternative to a time limit if you're doing
308    /// something like running tests where you want consistent results.
309    Nodes(usize),
310}
311impl Default for PathfinderTimeout {
312    fn default() -> Self {
313        Self::Time(Duration::from_secs(1))
314    }
315}
azalea/pathfinder/astar/mod.rs

azalea/pathfinder/astar/
mod.rs
