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

azalea/pathfinder/astar/
mod.rs
