azalea/pathfinder/
mod.rs

1//! A pathfinding plugin to make bots able to traverse the world.
2//!
3//! For the new functions on `Client` that the pathfinder adds, see
4//! [`PathfinderClientExt`].
5//!
6//! Much of this code is based on [Baritone](https://github.com/cabaletta/baritone).
7
8pub mod astar;
9pub mod costs;
10pub mod custom_state;
11pub mod debug;
12pub mod goals;
13mod goto_event;
14pub mod mining;
15pub mod moves;
16pub mod rel_block_pos;
17pub mod simulation;
18#[cfg(test)]
19mod tests;
20pub mod world;
21
22use std::{
23    cmp,
24    collections::VecDeque,
25    ops::RangeInclusive,
26    sync::{
27        Arc,
28        atomic::{self, AtomicUsize},
29    },
30    thread,
31    time::{Duration, Instant},
32};
33
34use astar::{Edge, PathfinderTimeout};
35use azalea_block::{BlockState, BlockTrait};
36use azalea_client::{
37    StartSprintEvent, StartWalkEvent,
38    inventory::{Inventory, InventorySystems},
39    local_player::InstanceHolder,
40    mining::{Mining, MiningSystems, StartMiningBlockEvent},
41    movement::MoveEventsSystems,
42};
43use azalea_core::{
44    position::{BlockPos, Vec3},
45    tick::GameTick,
46};
47use azalea_entity::{LocalEntity, Physics, Position, metadata::Player};
48use azalea_physics::{PhysicsSystems, get_block_pos_below_that_affects_movement};
49use azalea_world::{InstanceContainer, InstanceName};
50use bevy_app::{PreUpdate, Update};
51use bevy_ecs::prelude::*;
52use bevy_tasks::{AsyncComputeTaskPool, Task};
53use custom_state::{CustomPathfinderState, CustomPathfinderStateRef};
54use futures_lite::future;
55use goals::BlockPosGoal;
56pub use goto_event::{GotoEvent, PathfinderOpts};
57use parking_lot::RwLock;
58use rel_block_pos::RelBlockPos;
59use tokio::sync::broadcast::error::RecvError;
60use tracing::{debug, error, info, trace, warn};
61
62use self::{
63    debug::debug_render_path_with_particles,
64    goals::Goal,
65    mining::MiningCache,
66    moves::{ExecuteCtx, IsReachedCtx, SuccessorsFn},
67};
68use crate::{
69    WalkDirection,
70    app::{App, Plugin},
71    bot::{BotClientExt, JumpEvent, LookAtEvent},
72    ecs::{
73        component::Component,
74        entity::Entity,
75        query::{With, Without},
76        system::{Commands, Query, Res},
77    },
78    pathfinder::{astar::a_star, moves::PathfinderCtx, world::CachedWorld},
79};
80
81#[derive(Clone, Default)]
82pub struct PathfinderPlugin;
83impl Plugin for PathfinderPlugin {
84    fn build(&self, app: &mut App) {
85        app.add_message::<GotoEvent>()
86            .add_message::<PathFoundEvent>()
87            .add_message::<StopPathfindingEvent>()
88            .add_systems(
89                // putting systems in the GameTick schedule makes them run every Minecraft tick
90                // (every 50 milliseconds).
91                GameTick,
92                (
93                    timeout_movement,
94                    check_for_path_obstruction,
95                    check_node_reached,
96                    tick_execute_path,
97                    debug_render_path_with_particles,
98                    recalculate_near_end_of_path,
99                    recalculate_if_has_goal_but_no_path,
100                )
101                    .chain()
102                    .after(PhysicsSystems)
103                    .after(azalea_client::movement::send_position)
104                    .after(MiningSystems),
105            )
106            .add_systems(PreUpdate, add_default_pathfinder)
107            .add_systems(
108                Update,
109                (
110                    goto_listener,
111                    handle_tasks,
112                    stop_pathfinding_on_instance_change,
113                    path_found_listener,
114                    handle_stop_pathfinding_event,
115                )
116                    .chain()
117                    .before(MoveEventsSystems)
118                    .before(InventorySystems),
119            );
120    }
121}
122
123/// A component that makes this client able to pathfind.
124#[derive(Component, Default, Clone)]
125#[non_exhaustive]
126pub struct Pathfinder {
127    pub goal: Option<Arc<dyn Goal>>,
128    pub opts: Option<PathfinderOpts>,
129    pub is_calculating: bool,
130    pub goto_id: Arc<AtomicUsize>,
131}
132
133/// A component that's present on clients that are actively following a
134/// pathfinder path.
135#[derive(Component, Clone)]
136pub struct ExecutingPath {
137    pub path: VecDeque<astar::Edge<BlockPos, moves::MoveData>>,
138    pub queued_path: Option<VecDeque<astar::Edge<BlockPos, moves::MoveData>>>,
139    pub last_reached_node: BlockPos,
140    pub last_node_reached_at: Instant,
141    pub is_path_partial: bool,
142}
143
144#[derive(Message, Clone, Debug)]
145#[non_exhaustive]
146pub struct PathFoundEvent {
147    pub entity: Entity,
148    pub start: BlockPos,
149    pub path: Option<VecDeque<astar::Edge<BlockPos, moves::MoveData>>>,
150    pub is_partial: bool,
151    pub successors_fn: SuccessorsFn,
152    pub allow_mining: bool,
153}
154
155#[allow(clippy::type_complexity)]
156pub fn add_default_pathfinder(
157    mut commands: Commands,
158    mut query: Query<Entity, (Without<Pathfinder>, With<LocalEntity>, With<Player>)>,
159) {
160    for entity in &mut query {
161        commands.entity(entity).insert(Pathfinder::default());
162    }
163}
164
165pub trait PathfinderClientExt {
166    /// Pathfind to the given goal and wait until either the target is reached
167    /// or the pathfinding is canceled.
168    ///
169    /// You can use [`Self::start_goto`] instead if you don't want to wait.
170    ///
171    /// ```
172    /// # use azalea::prelude::*;
173    /// # use azalea::{BlockPos, pathfinder::goals::BlockPosGoal};
174    /// # async fn example(bot: &Client) {
175    /// bot.goto(BlockPosGoal(BlockPos::new(0, 70, 0))).await;
176    /// # }
177    /// ```
178    fn goto(&self, goal: impl Goal + 'static) -> impl Future<Output = ()>;
179    /// Same as [`Self::goto`], but allows you to set custom options for
180    /// pathfinding, including disabling mining and setting custom moves.
181    ///
182    /// ```
183    /// # use azalea::prelude::*;
184    /// # use azalea::{BlockPos, pathfinder::{goals::BlockPosGoal, PathfinderOpts}};
185    /// # async fn example(bot: &Client) {
186    /// bot.goto_with_opts(
187    ///     BlockPosGoal(BlockPos::new(0, 70, 0)),
188    ///     PathfinderOpts::new().allow_mining(false),
189    /// )
190    /// .await;
191    /// # }
192    /// ```
193    fn goto_with_opts(
194        &self,
195        goal: impl Goal + 'static,
196        opts: PathfinderOpts,
197    ) -> impl Future<Output = ()>;
198    /// Start pathfinding to a given goal.
199    ///
200    /// ```
201    /// # use azalea::prelude::*;
202    /// # use azalea::{BlockPos, pathfinder::goals::BlockPosGoal};
203    /// # fn example(bot: &Client) {
204    /// bot.start_goto(BlockPosGoal(BlockPos::new(0, 70, 0)));
205    /// # }
206    /// ```
207    fn start_goto(&self, goal: impl Goal + 'static);
208    /// Same as [`Self::start_goto`], but allows you to set custom
209    /// options for pathfinding, including disabling mining and setting custom
210    /// moves.
211    ///
212    /// Also see [`Self::goto_with_opts`].
213    fn start_goto_with_opts(&self, goal: impl Goal + 'static, opts: PathfinderOpts);
214    /// Stop calculating a path, and stop moving once the current movement is
215    /// finished.
216    ///
217    /// This behavior exists to prevent the bot from taking damage if
218    /// `stop_pathfinding` was called while executing a parkour jump, but if
219    /// it's undesirable then you may want to consider using
220    /// [`Self::force_stop_pathfinding`] instead.
221    fn stop_pathfinding(&self);
222    /// Stop calculating a path and stop executing the current movement
223    /// immediately.
224    fn force_stop_pathfinding(&self);
225    /// Waits forever until the bot no longer has a pathfinder goal.
226    fn wait_until_goto_target_reached(&self) -> impl Future<Output = ()>;
227    /// Returns true if the pathfinder has no active goal and isn't calculating
228    /// a path.
229    fn is_goto_target_reached(&self) -> bool;
230}
231
232impl PathfinderClientExt for azalea_client::Client {
233    async fn goto(&self, goal: impl Goal + 'static) {
234        self.goto_with_opts(goal, PathfinderOpts::new()).await;
235    }
236    async fn goto_with_opts(&self, goal: impl Goal + 'static, opts: PathfinderOpts) {
237        self.start_goto_with_opts(goal, opts);
238        self.wait_until_goto_target_reached().await;
239    }
240    fn start_goto(&self, goal: impl Goal + 'static) {
241        self.start_goto_with_opts(goal, PathfinderOpts::new());
242    }
243    fn start_goto_with_opts(&self, goal: impl Goal + 'static, opts: PathfinderOpts) {
244        self.ecs
245            .lock()
246            .write_message(GotoEvent::new(self.entity, goal, opts));
247    }
248    fn stop_pathfinding(&self) {
249        self.ecs.lock().write_message(StopPathfindingEvent {
250            entity: self.entity,
251            force: false,
252        });
253    }
254    fn force_stop_pathfinding(&self) {
255        self.ecs.lock().write_message(StopPathfindingEvent {
256            entity: self.entity,
257            force: true,
258        });
259    }
260    async fn wait_until_goto_target_reached(&self) {
261        // we do this to make sure the event got handled before we start checking
262        // is_goto_target_reached
263        self.wait_updates(1).await;
264
265        let mut tick_broadcaster = self.get_tick_broadcaster();
266        while !self.is_goto_target_reached() {
267            // check every tick
268            match tick_broadcaster.recv().await {
269                Ok(_) => (),
270                Err(RecvError::Closed) => return,
271                Err(err) => warn!("{err}"),
272            };
273        }
274    }
275    fn is_goto_target_reached(&self) -> bool {
276        self.query_self::<Option<&Pathfinder>, _>(|p| {
277            p.map(|p| p.goal.is_none() && !p.is_calculating)
278                .unwrap_or(true)
279        })
280    }
281}
282
283#[derive(Component)]
284pub struct ComputePath(Task<Option<PathFoundEvent>>);
285
286#[allow(clippy::type_complexity)]
287pub fn goto_listener(
288    mut commands: Commands,
289    mut events: MessageReader<GotoEvent>,
290    mut query: Query<(
291        &mut Pathfinder,
292        Option<&mut ExecutingPath>,
293        &Position,
294        &InstanceName,
295        &Inventory,
296        Option<&CustomPathfinderState>,
297    )>,
298    instance_container: Res<InstanceContainer>,
299) {
300    let thread_pool = AsyncComputeTaskPool::get();
301
302    for event in events.read() {
303        let Ok((mut pathfinder, executing_path, position, instance_name, inventory, custom_state)) =
304            query.get_mut(event.entity)
305        else {
306            warn!("got goto event for an entity that can't pathfind");
307            continue;
308        };
309
310        let cur_pos = player_pos_to_block_pos(**position);
311
312        if event.goal.success(cur_pos) {
313            // we're already at the goal, nothing to do
314            pathfinder.goal = None;
315            pathfinder.opts = None;
316            pathfinder.is_calculating = false;
317            debug!("already at goal, not pathfinding");
318            continue;
319        }
320
321        // we store the goal so it can be recalculated later if necessary
322        pathfinder.goal = Some(event.goal.clone());
323        pathfinder.opts = Some(event.opts.clone());
324        pathfinder.is_calculating = true;
325
326        let start = if let Some(mut executing_path) = executing_path
327            && { !executing_path.path.is_empty() }
328        {
329            // if we're currently pathfinding and got a goto event, start a little ahead
330
331            let executing_path_limit = 50;
332            // truncate the executing path so we can cleanly combine the two paths later
333            executing_path.path.truncate(executing_path_limit);
334
335            executing_path
336                .path
337                .back()
338                .expect("path was just checked to not be empty")
339                .movement
340                .target
341        } else {
342            cur_pos
343        };
344
345        if start == cur_pos {
346            info!("got goto {:?}, starting from {start:?}", event.goal);
347        } else {
348            info!(
349                "got goto {:?}, starting from {start:?} (currently at {cur_pos:?})",
350                event.goal,
351            );
352        }
353
354        let world_lock = instance_container
355            .get(instance_name)
356            .expect("Entity tried to pathfind but the entity isn't in a valid world");
357
358        let goal = event.goal.clone();
359        let entity = event.entity;
360
361        let goto_id_atomic = pathfinder.goto_id.clone();
362
363        let allow_mining = event.opts.allow_mining;
364        let mining_cache = MiningCache::new(if allow_mining {
365            Some(inventory.inventory_menu.clone())
366        } else {
367            None
368        });
369
370        let custom_state = custom_state.cloned().unwrap_or_default();
371        let opts = event.opts.clone();
372        let task = thread_pool.spawn(async move {
373            calculate_path(CalculatePathCtx {
374                entity,
375                start,
376                goal,
377                world_lock,
378                goto_id_atomic,
379                mining_cache,
380                custom_state,
381                opts,
382            })
383        });
384
385        commands.entity(event.entity).insert(ComputePath(task));
386    }
387}
388
389/// Convert a player position to a block position, used internally in the
390/// pathfinder.
391///
392/// This is almost the same as `BlockPos::from(position)`, except that non-full
393/// blocks are handled correctly.
394#[inline]
395pub fn player_pos_to_block_pos(position: Vec3) -> BlockPos {
396    // 0.5 to account for non-full blocks
397    BlockPos::from(position.up(0.5))
398}
399
400pub struct CalculatePathCtx {
401    pub entity: Entity,
402    pub start: BlockPos,
403    pub goal: Arc<dyn Goal>,
404    pub world_lock: Arc<RwLock<azalea_world::Instance>>,
405    pub goto_id_atomic: Arc<AtomicUsize>,
406    pub mining_cache: MiningCache,
407    pub custom_state: CustomPathfinderState,
408
409    pub opts: PathfinderOpts,
410}
411
412/// Calculate the [`PathFoundEvent`] for the given pathfinder options.
413///
414/// You usually want to just use [`PathfinderClientExt::goto`] or send a
415/// [`GotoEvent`] instead of calling this directly.
416///
417/// You are expected to immediately send the `PathFoundEvent` you received after
418/// calling this function. `None` will be returned if the pathfinding was
419/// interrupted by another path calculation.
420pub fn calculate_path(ctx: CalculatePathCtx) -> Option<PathFoundEvent> {
421    debug!("start: {:?}", ctx.start);
422
423    let goto_id = ctx.goto_id_atomic.fetch_add(1, atomic::Ordering::SeqCst) + 1;
424
425    let origin = ctx.start;
426    let cached_world = CachedWorld::new(ctx.world_lock, origin);
427    let successors = |pos: RelBlockPos| {
428        call_successors_fn(
429            &cached_world,
430            &ctx.mining_cache,
431            &ctx.custom_state.0.read(),
432            ctx.opts.successors_fn,
433            pos,
434        )
435    };
436
437    let start_time = Instant::now();
438
439    let astar::Path {
440        movements,
441        is_partial,
442    } = a_star(
443        RelBlockPos::get_origin(origin),
444        |n| ctx.goal.heuristic(n.apply(origin)),
445        successors,
446        |n| ctx.goal.success(n.apply(origin)),
447        ctx.opts.min_timeout,
448        ctx.opts.max_timeout,
449    );
450    let end_time = Instant::now();
451    debug!("partial: {is_partial:?}");
452    let duration = end_time - start_time;
453    if is_partial {
454        if movements.is_empty() {
455            info!("Pathfinder took {duration:?} (empty path)");
456        } else {
457            info!("Pathfinder took {duration:?} (incomplete path)");
458        }
459        // wait a bit so it's not a busy loop
460        thread::sleep(Duration::from_millis(100));
461    } else {
462        info!("Pathfinder took {duration:?}");
463    }
464
465    debug!("Path:");
466    for movement in &movements {
467        debug!("  {}", movement.target.apply(origin));
468    }
469
470    let path = movements.into_iter().collect::<VecDeque<_>>();
471
472    let goto_id_now = ctx.goto_id_atomic.load(atomic::Ordering::SeqCst);
473    if goto_id != goto_id_now {
474        // we must've done another goto while calculating this path, so throw it away
475        warn!("finished calculating a path, but it's outdated");
476        return None;
477    }
478
479    if path.is_empty() && is_partial {
480        debug!("this path is empty, we might be stuck :(");
481    }
482
483    let mut mapped_path = VecDeque::with_capacity(path.len());
484    let mut current_position = RelBlockPos::get_origin(origin);
485    for movement in path {
486        let mut found_edge = None;
487        for edge in successors(current_position) {
488            if edge.movement.target == movement.target {
489                found_edge = Some(edge);
490                break;
491            }
492        }
493
494        let found_edge = found_edge.expect(
495            "path should always still be possible because we're using the same world cache",
496        );
497        current_position = found_edge.movement.target;
498
499        // we don't just clone the found_edge because we're using BlockPos instead of
500        // RelBlockPos as the target type
501        mapped_path.push_back(Edge {
502            movement: astar::Movement {
503                target: movement.target.apply(origin),
504                data: movement.data,
505            },
506            cost: found_edge.cost,
507        });
508    }
509
510    Some(PathFoundEvent {
511        entity: ctx.entity,
512        start: ctx.start,
513        path: Some(mapped_path),
514        is_partial,
515        successors_fn: ctx.opts.successors_fn,
516        allow_mining: ctx.opts.allow_mining,
517    })
518}
519
520// poll the tasks and send the PathFoundEvent if they're done
521pub fn handle_tasks(
522    mut commands: Commands,
523    mut transform_tasks: Query<(Entity, &mut ComputePath)>,
524    mut path_found_events: MessageWriter<PathFoundEvent>,
525) {
526    for (entity, mut task) in &mut transform_tasks {
527        if let Some(optional_path_found_event) = future::block_on(future::poll_once(&mut task.0)) {
528            if let Some(path_found_event) = optional_path_found_event {
529                path_found_events.write(path_found_event);
530            }
531
532            // Task is complete, so remove task component from entity
533            commands.entity(entity).remove::<ComputePath>();
534        }
535    }
536}
537
538// set the path for the target entity when we get the PathFoundEvent
539#[allow(clippy::type_complexity)]
540pub fn path_found_listener(
541    mut events: MessageReader<PathFoundEvent>,
542    mut query: Query<(
543        &mut Pathfinder,
544        Option<&mut ExecutingPath>,
545        &InstanceName,
546        &Inventory,
547        Option<&CustomPathfinderState>,
548    )>,
549    instance_container: Res<InstanceContainer>,
550    mut commands: Commands,
551) {
552    for event in events.read() {
553        let Ok((mut pathfinder, executing_path, instance_name, inventory, custom_state)) =
554            query.get_mut(event.entity)
555        else {
556            debug!("got path found event for an entity that can't pathfind");
557            continue;
558        };
559        if let Some(path) = &event.path {
560            if let Some(mut executing_path) = executing_path {
561                let mut new_path = VecDeque::new();
562
563                // combine the old and new paths if the first node of the new path is a
564                // successor of the last node of the old path
565                if let Some(last_node_of_current_path) = executing_path.path.back() {
566                    let world_lock = instance_container
567                        .get(instance_name)
568                        .expect("Entity tried to pathfind but the entity isn't in a valid world");
569                    let origin = event.start;
570                    let successors_fn: moves::SuccessorsFn = event.successors_fn;
571                    let cached_world = CachedWorld::new(world_lock, origin);
572                    let mining_cache = MiningCache::new(if event.allow_mining {
573                        Some(inventory.inventory_menu.clone())
574                    } else {
575                        None
576                    });
577                    let custom_state = custom_state.cloned().unwrap_or_default();
578                    let custom_state_ref = custom_state.0.read();
579                    let successors = |pos: RelBlockPos| {
580                        call_successors_fn(
581                            &cached_world,
582                            &mining_cache,
583                            &custom_state_ref,
584                            successors_fn,
585                            pos,
586                        )
587                    };
588
589                    if let Some(first_node_of_new_path) = path.front() {
590                        let last_target_of_current_path = RelBlockPos::from_origin(
591                            origin,
592                            last_node_of_current_path.movement.target,
593                        );
594                        let first_target_of_new_path = RelBlockPos::from_origin(
595                            origin,
596                            first_node_of_new_path.movement.target,
597                        );
598
599                        if successors(last_target_of_current_path)
600                            .iter()
601                            .any(|edge| edge.movement.target == first_target_of_new_path)
602                        {
603                            debug!("combining old and new paths");
604                            debug!(
605                                "old path: {:?}",
606                                executing_path.path.iter().collect::<Vec<_>>()
607                            );
608                            debug!("new path: {:?}", path.iter().take(10).collect::<Vec<_>>());
609                            new_path.extend(executing_path.path.iter().cloned());
610                        }
611                    } else {
612                        new_path.extend(executing_path.path.iter().cloned());
613                    }
614                }
615
616                new_path.extend(path.to_owned());
617
618                debug!(
619                    "set queued path to {:?}",
620                    new_path.iter().take(10).collect::<Vec<_>>()
621                );
622                executing_path.queued_path = Some(new_path);
623                executing_path.is_path_partial = event.is_partial;
624            } else if path.is_empty() {
625                debug!("calculated path is empty, so didn't add ExecutingPath");
626                if !pathfinder.opts.as_ref().is_some_and(|o| o.retry_on_no_path) {
627                    debug!("retry_on_no_path is set to false, removing goal");
628                    pathfinder.goal = None;
629                }
630            } else {
631                commands.entity(event.entity).insert(ExecutingPath {
632                    path: path.to_owned(),
633                    queued_path: None,
634                    last_reached_node: event.start,
635                    last_node_reached_at: Instant::now(),
636                    is_path_partial: event.is_partial,
637                });
638                debug!("set path to {:?}", path.iter().take(10).collect::<Vec<_>>());
639                debug!("partial: {}", event.is_partial);
640            }
641        } else {
642            error!("No path found");
643            if let Some(mut executing_path) = executing_path {
644                // set the queued path so we don't stop in the middle of a move
645                executing_path.queued_path = Some(VecDeque::new());
646            } else {
647                // wasn't executing a path, don't need to do anything
648            }
649        }
650        pathfinder.is_calculating = false;
651    }
652}
653
654#[allow(clippy::type_complexity)]
655pub fn timeout_movement(
656    mut query: Query<(
657        Entity,
658        &mut Pathfinder,
659        &mut ExecutingPath,
660        &Position,
661        Option<&Mining>,
662        &InstanceName,
663        &Inventory,
664        Option<&CustomPathfinderState>,
665    )>,
666    instance_container: Res<InstanceContainer>,
667) {
668    for (
669        entity,
670        mut pathfinder,
671        mut executing_path,
672        position,
673        mining,
674        instance_name,
675        inventory,
676        custom_state,
677    ) in &mut query
678    {
679        // don't timeout if we're mining
680        if let Some(mining) = mining {
681            // also make sure we're close enough to the block that's being mined
682            if mining.pos.distance_squared_to(position.into()) < 6_i32.pow(2) {
683                // also reset the last_node_reached_at so we don't timeout after we finish
684                // mining
685                executing_path.last_node_reached_at = Instant::now();
686                continue;
687            }
688        }
689
690        if executing_path.last_node_reached_at.elapsed() > Duration::from_secs(2)
691            && !pathfinder.is_calculating
692            && !executing_path.path.is_empty()
693        {
694            warn!("pathfinder timeout, trying to patch path");
695            executing_path.queued_path = None;
696            let cur_pos = player_pos_to_block_pos(**position);
697            executing_path.last_reached_node = cur_pos;
698
699            let world_lock = instance_container
700                .get(instance_name)
701                .expect("Entity tried to pathfind but the entity isn't in a valid world");
702            let Some(opts) = pathfinder.opts.clone() else {
703                warn!(
704                    "pathfinder was going to patch path because of timeout, but pathfinder.opts was None"
705                );
706                return;
707            };
708
709            let custom_state = custom_state.cloned().unwrap_or_default();
710
711            // try to fix the path without recalculating everything.
712            // (though, it'll still get fully recalculated by `recalculate_near_end_of_path`
713            // if the new path is too short)
714            patch_path(
715                0..=cmp::min(20, executing_path.path.len() - 1),
716                &mut executing_path,
717                &mut pathfinder,
718                inventory,
719                entity,
720                world_lock,
721                custom_state,
722                opts,
723            );
724            // reset last_node_reached_at so we don't immediately try to patch again
725            executing_path.last_node_reached_at = Instant::now();
726        }
727    }
728}
729
730pub fn check_node_reached(
731    mut query: Query<(
732        Entity,
733        &mut Pathfinder,
734        &mut ExecutingPath,
735        &Position,
736        &Physics,
737        &InstanceName,
738    )>,
739    mut walk_events: MessageWriter<StartWalkEvent>,
740    mut commands: Commands,
741    instance_container: Res<InstanceContainer>,
742) {
743    for (entity, mut pathfinder, mut executing_path, position, physics, instance_name) in &mut query
744    {
745        let Some(instance) = instance_container.get(instance_name) else {
746            warn!("entity is pathfinding but not in a valid world");
747            continue;
748        };
749
750        'skip: loop {
751            // we check if the goal was reached *before* actually executing the movement so
752            // we don't unnecessarily execute a movement when it wasn't necessary
753
754            // see if we already reached any future nodes and can skip ahead
755            for (i, edge) in executing_path
756                .path
757                .clone()
758                .into_iter()
759                .enumerate()
760                .take(20)
761                .rev()
762            {
763                let movement = edge.movement;
764                let is_reached_ctx = IsReachedCtx {
765                    target: movement.target,
766                    start: executing_path.last_reached_node,
767                    position: **position,
768                    physics,
769                };
770                let extra_check = if i == executing_path.path.len() - 1 {
771                    // be extra strict about the velocity and centering if we're on the last node so
772                    // we don't fall off
773
774                    let x_difference_from_center = position.x - (movement.target.x as f64 + 0.5);
775                    let z_difference_from_center = position.z - (movement.target.z as f64 + 0.5);
776
777                    let block_pos_below = get_block_pos_below_that_affects_movement(*position);
778
779                    let block_state_below = {
780                        let instance = instance.read();
781                        instance
782                            .chunks
783                            .get_block_state(block_pos_below)
784                            .unwrap_or(BlockState::AIR)
785                    };
786                    let block_below: Box<dyn BlockTrait> = block_state_below.into();
787                    // friction for normal blocks is 0.6, for ice it's 0.98
788                    let block_friction = block_below.behavior().friction as f64;
789
790                    // if the block has the default friction, this will multiply by 1
791                    // for blocks like ice, it'll multiply by a higher number
792                    let scaled_velocity = physics.velocity * (0.4 / (1. - block_friction));
793
794                    let x_predicted_offset = (x_difference_from_center + scaled_velocity.x).abs();
795                    let z_predicted_offset = (z_difference_from_center + scaled_velocity.z).abs();
796
797                    // this is to make sure we don't fall off immediately after finishing the path
798                    physics.on_ground()
799                        && player_pos_to_block_pos(**position) == movement.target
800                        // adding the delta like this isn't a perfect solution but it helps to make
801                        // sure we don't keep going if our delta is high
802                        && x_predicted_offset < 0.2
803                        && z_predicted_offset < 0.2
804                } else {
805                    true
806                };
807
808                if (movement.data.is_reached)(is_reached_ctx) && extra_check {
809                    executing_path.path = executing_path.path.split_off(i + 1);
810                    executing_path.last_reached_node = movement.target;
811                    executing_path.last_node_reached_at = Instant::now();
812                    trace!("reached node {}", movement.target);
813
814                    if let Some(new_path) = executing_path.queued_path.take() {
815                        debug!(
816                            "swapped path to {:?}",
817                            new_path.iter().take(10).collect::<Vec<_>>()
818                        );
819                        executing_path.path = new_path;
820
821                        if executing_path.path.is_empty() {
822                            info!("the path we just swapped to was empty, so reached end of path");
823                            walk_events.write(StartWalkEvent {
824                                entity,
825                                direction: WalkDirection::None,
826                            });
827                            commands.entity(entity).remove::<ExecutingPath>();
828                            break;
829                        }
830
831                        // run the function again since we just swapped
832                        continue 'skip;
833                    }
834
835                    if executing_path.path.is_empty() {
836                        debug!("pathfinder path is now empty");
837                        walk_events.write(StartWalkEvent {
838                            entity,
839                            direction: WalkDirection::None,
840                        });
841                        commands.entity(entity).remove::<ExecutingPath>();
842                        if let Some(goal) = pathfinder.goal.clone()
843                            && goal.success(movement.target)
844                        {
845                            info!("goal was reached!");
846                            pathfinder.goal = None;
847                            pathfinder.opts = None;
848                        }
849                    }
850
851                    break;
852                }
853            }
854            break;
855        }
856    }
857}
858
859#[allow(clippy::type_complexity)]
860pub fn check_for_path_obstruction(
861    mut query: Query<(
862        Entity,
863        &mut Pathfinder,
864        &mut ExecutingPath,
865        &InstanceName,
866        &Inventory,
867        Option<&CustomPathfinderState>,
868    )>,
869    instance_container: Res<InstanceContainer>,
870) {
871    for (entity, mut pathfinder, mut executing_path, instance_name, inventory, custom_state) in
872        &mut query
873    {
874        let Some(opts) = pathfinder.opts.clone() else {
875            continue;
876        };
877
878        let world_lock = instance_container
879            .get(instance_name)
880            .expect("Entity tried to pathfind but the entity isn't in a valid world");
881
882        // obstruction check (the path we're executing isn't possible anymore)
883        let origin = executing_path.last_reached_node;
884        let cached_world = CachedWorld::new(world_lock, origin);
885        let mining_cache = MiningCache::new(if opts.allow_mining {
886            Some(inventory.inventory_menu.clone())
887        } else {
888            None
889        });
890        let custom_state = custom_state.cloned().unwrap_or_default();
891        let custom_state_ref = custom_state.0.read();
892        let successors = |pos: RelBlockPos| {
893            call_successors_fn(
894                &cached_world,
895                &mining_cache,
896                &custom_state_ref,
897                opts.successors_fn,
898                pos,
899            )
900        };
901
902        let Some(obstructed_index) = check_path_obstructed(
903            origin,
904            RelBlockPos::from_origin(origin, executing_path.last_reached_node),
905            &executing_path.path,
906            successors,
907        ) else {
908            continue;
909        };
910
911        drop(custom_state_ref);
912
913        warn!(
914            "path obstructed at index {obstructed_index} (starting at {:?})",
915            executing_path.last_reached_node,
916        );
917        debug!("obstructed path: {:?}", executing_path.path);
918        // if it's near the end, don't bother recalculating a patch, just truncate and
919        // mark it as partial
920        if obstructed_index + 5 > executing_path.path.len() {
921            debug!(
922                "obstruction is near the end of the path, truncating and marking path as partial"
923            );
924            executing_path.path.truncate(obstructed_index);
925            executing_path.is_path_partial = true;
926            continue;
927        }
928
929        let Some(opts) = pathfinder.opts.clone() else {
930            error!("got PatchExecutingPathEvent but the bot has no pathfinder opts");
931            continue;
932        };
933
934        let world_lock = instance_container
935            .get(instance_name)
936            .expect("Entity tried to pathfind but the entity isn't in a valid world");
937
938        // patch up to 20 nodes
939        let patch_end_index = cmp::min(obstructed_index + 20, executing_path.path.len() - 1);
940
941        patch_path(
942            obstructed_index..=patch_end_index,
943            &mut executing_path,
944            &mut pathfinder,
945            inventory,
946            entity,
947            world_lock,
948            custom_state.clone(),
949            opts,
950        );
951    }
952}
953
954/// Update the given [`ExecutingPath`] to recalculate the path of the nodes in
955/// the given index range.
956///
957/// You should avoid making the range too large, since the timeout for the A*
958/// calculation is very low. About 20 nodes is a good amount.
959#[allow(clippy::too_many_arguments)]
960fn patch_path(
961    patch_nodes: RangeInclusive<usize>,
962    executing_path: &mut ExecutingPath,
963    pathfinder: &mut Pathfinder,
964    inventory: &Inventory,
965    entity: Entity,
966    world_lock: Arc<RwLock<azalea_world::Instance>>,
967    custom_state: CustomPathfinderState,
968    opts: PathfinderOpts,
969) {
970    let patch_start = if *patch_nodes.start() == 0 {
971        executing_path.last_reached_node
972    } else {
973        executing_path.path[*patch_nodes.start() - 1]
974            .movement
975            .target
976    };
977
978    let patch_end = executing_path.path[*patch_nodes.end()].movement.target;
979
980    // this doesn't override the main goal, it's just the goal for this A*
981    // calculation
982    let goal = Arc::new(BlockPosGoal(patch_end));
983
984    let goto_id_atomic = pathfinder.goto_id.clone();
985    let allow_mining = opts.allow_mining;
986
987    let mining_cache = MiningCache::new(if allow_mining {
988        Some(inventory.inventory_menu.clone())
989    } else {
990        None
991    });
992
993    // the timeout is small enough that this doesn't need to be async
994    let path_found_event = calculate_path(CalculatePathCtx {
995        entity,
996        start: patch_start,
997        goal,
998        world_lock,
999        goto_id_atomic,
1000        mining_cache,
1001        custom_state,
1002        opts: PathfinderOpts {
1003            min_timeout: PathfinderTimeout::Nodes(10_000),
1004            max_timeout: PathfinderTimeout::Nodes(10_000),
1005            ..opts
1006        },
1007    });
1008
1009    // this is necessary in case we interrupted another ongoing path calculation
1010    pathfinder.is_calculating = false;
1011
1012    debug!("obstruction patch: {path_found_event:?}");
1013
1014    let mut new_path = VecDeque::new();
1015    if *patch_nodes.start() > 0 {
1016        new_path.extend(
1017            executing_path
1018                .path
1019                .iter()
1020                .take(*patch_nodes.start())
1021                .cloned(),
1022        );
1023    }
1024
1025    let mut is_patch_complete = false;
1026    if let Some(path_found_event) = path_found_event {
1027        if let Some(found_path_patch) = path_found_event.path
1028            && !found_path_patch.is_empty()
1029        {
1030            new_path.extend(found_path_patch);
1031
1032            if !path_found_event.is_partial {
1033                new_path.extend(executing_path.path.iter().skip(*patch_nodes.end()).cloned());
1034                is_patch_complete = true;
1035                debug!("the patch is not partial :)");
1036            } else {
1037                debug!("the patch is partial, throwing away rest of path :(");
1038            }
1039        }
1040    } else {
1041        // no path found, rip
1042    }
1043
1044    executing_path.path = new_path;
1045    if !is_patch_complete {
1046        executing_path.is_path_partial = true;
1047    }
1048}
1049
1050pub fn recalculate_near_end_of_path(
1051    mut query: Query<(Entity, &mut Pathfinder, &mut ExecutingPath)>,
1052    mut walk_events: MessageWriter<StartWalkEvent>,
1053    mut goto_events: MessageWriter<GotoEvent>,
1054    mut commands: Commands,
1055) {
1056    for (entity, mut pathfinder, mut executing_path) in &mut query {
1057        let Some(mut opts) = pathfinder.opts.clone() else {
1058            continue;
1059        };
1060
1061        // start recalculating if the path ends soon
1062        if (executing_path.path.len() == 50 || executing_path.path.len() < 5)
1063            && !pathfinder.is_calculating
1064            && executing_path.is_path_partial
1065        {
1066            match pathfinder.goal.as_ref().cloned() {
1067                Some(goal) => {
1068                    debug!("Recalculating path because it's empty or ends soon");
1069                    debug!(
1070                        "recalculate_near_end_of_path executing_path.is_path_partial: {}",
1071                        executing_path.is_path_partial
1072                    );
1073
1074                    opts.min_timeout = if executing_path.path.len() == 50 {
1075                        // we have quite some time until the node is reached, soooo we might as
1076                        // well burn some cpu cycles to get a good path
1077                        PathfinderTimeout::Time(Duration::from_secs(5))
1078                    } else {
1079                        PathfinderTimeout::Time(Duration::from_secs(1))
1080                    };
1081
1082                    goto_events.write(GotoEvent { entity, goal, opts });
1083                    pathfinder.is_calculating = true;
1084
1085                    if executing_path.path.is_empty() {
1086                        if let Some(new_path) = executing_path.queued_path.take() {
1087                            executing_path.path = new_path;
1088                            if executing_path.path.is_empty() {
1089                                info!(
1090                                    "the path we just swapped to was empty, so reached end of path"
1091                                );
1092                                walk_events.write(StartWalkEvent {
1093                                    entity,
1094                                    direction: WalkDirection::None,
1095                                });
1096                                commands.entity(entity).remove::<ExecutingPath>();
1097                                break;
1098                            }
1099                        } else {
1100                            walk_events.write(StartWalkEvent {
1101                                entity,
1102                                direction: WalkDirection::None,
1103                            });
1104                            commands.entity(entity).remove::<ExecutingPath>();
1105                        }
1106                    }
1107                }
1108                _ => {
1109                    if executing_path.path.is_empty() {
1110                        // idk when this can happen but stop moving just in case
1111                        walk_events.write(StartWalkEvent {
1112                            entity,
1113                            direction: WalkDirection::None,
1114                        });
1115                    }
1116                }
1117            }
1118        }
1119    }
1120}
1121
1122#[allow(clippy::type_complexity)]
1123pub fn tick_execute_path(
1124    mut commands: Commands,
1125    mut query: Query<(
1126        Entity,
1127        &mut ExecutingPath,
1128        &Position,
1129        &Physics,
1130        Option<&Mining>,
1131        &InstanceHolder,
1132        &Inventory,
1133    )>,
1134    mut look_at_events: MessageWriter<LookAtEvent>,
1135    mut sprint_events: MessageWriter<StartSprintEvent>,
1136    mut walk_events: MessageWriter<StartWalkEvent>,
1137    mut jump_events: MessageWriter<JumpEvent>,
1138    mut start_mining_events: MessageWriter<StartMiningBlockEvent>,
1139) {
1140    for (entity, executing_path, position, physics, mining, instance_holder, inventory_component) in
1141        &mut query
1142    {
1143        if let Some(edge) = executing_path.path.front() {
1144            let ctx = ExecuteCtx {
1145                entity,
1146                target: edge.movement.target,
1147                position: **position,
1148                start: executing_path.last_reached_node,
1149                physics,
1150                is_currently_mining: mining.is_some(),
1151                instance: instance_holder.instance.clone(),
1152                menu: inventory_component.inventory_menu.clone(),
1153
1154                commands: &mut commands,
1155                look_at_events: &mut look_at_events,
1156                sprint_events: &mut sprint_events,
1157                walk_events: &mut walk_events,
1158                jump_events: &mut jump_events,
1159                start_mining_events: &mut start_mining_events,
1160            };
1161            trace!(
1162                "executing move, position: {}, last_reached_node: {}",
1163                **position, executing_path.last_reached_node
1164            );
1165            (edge.movement.data.execute)(ctx);
1166        }
1167    }
1168}
1169
1170pub fn recalculate_if_has_goal_but_no_path(
1171    mut query: Query<(Entity, &mut Pathfinder), Without<ExecutingPath>>,
1172    mut goto_events: MessageWriter<GotoEvent>,
1173) {
1174    for (entity, mut pathfinder) in &mut query {
1175        if pathfinder.goal.is_some()
1176            && !pathfinder.is_calculating
1177            && let Some(goal) = pathfinder.goal.as_ref().cloned()
1178            && let Some(opts) = pathfinder.opts.clone()
1179        {
1180            debug!("Recalculating path because it has a goal but no ExecutingPath");
1181            goto_events.write(GotoEvent { entity, goal, opts });
1182            pathfinder.is_calculating = true;
1183        }
1184    }
1185}
1186
1187#[derive(Message)]
1188pub struct StopPathfindingEvent {
1189    pub entity: Entity,
1190    /// Whether we should stop moving immediately without waiting for the
1191    /// current movement to finish.
1192    ///
1193    /// This is usually set to false, since it might cause the bot to fall if it
1194    /// was in the middle of parkouring.
1195    pub force: bool,
1196}
1197
1198pub fn handle_stop_pathfinding_event(
1199    mut events: MessageReader<StopPathfindingEvent>,
1200    mut query: Query<(&mut Pathfinder, &mut ExecutingPath)>,
1201    mut walk_events: MessageWriter<StartWalkEvent>,
1202    mut commands: Commands,
1203) {
1204    for event in events.read() {
1205        // stop computing any path that's being computed
1206        commands.entity(event.entity).remove::<ComputePath>();
1207
1208        let Ok((mut pathfinder, mut executing_path)) = query.get_mut(event.entity) else {
1209            continue;
1210        };
1211        pathfinder.goal = None;
1212        if event.force {
1213            executing_path.path.clear();
1214            executing_path.queued_path = None;
1215        } else {
1216            // switch to an empty path as soon as it can
1217            executing_path.queued_path = Some(VecDeque::new());
1218            // make sure it doesn't recalculate
1219            executing_path.is_path_partial = false;
1220        }
1221
1222        if executing_path.path.is_empty() {
1223            walk_events.write(StartWalkEvent {
1224                entity: event.entity,
1225                direction: WalkDirection::None,
1226            });
1227            commands.entity(event.entity).remove::<ExecutingPath>();
1228        }
1229    }
1230}
1231
1232pub fn stop_pathfinding_on_instance_change(
1233    mut query: Query<(Entity, &mut ExecutingPath), Changed<InstanceName>>,
1234    mut stop_pathfinding_events: MessageWriter<StopPathfindingEvent>,
1235) {
1236    for (entity, mut executing_path) in &mut query {
1237        if !executing_path.path.is_empty() {
1238            debug!("instance changed, clearing path");
1239            executing_path.path.clear();
1240            stop_pathfinding_events.write(StopPathfindingEvent {
1241                entity,
1242                force: true,
1243            });
1244        }
1245    }
1246}
1247
1248/// Checks whether the path has been obstructed, and returns Some(index) if it
1249/// has been.
1250///
1251/// The index is of the first obstructed node.
1252pub fn check_path_obstructed<SuccessorsFn>(
1253    origin: BlockPos,
1254    mut current_position: RelBlockPos,
1255    path: &VecDeque<astar::Edge<BlockPos, moves::MoveData>>,
1256    successors_fn: SuccessorsFn,
1257) -> Option<usize>
1258where
1259    SuccessorsFn: Fn(RelBlockPos) -> Vec<astar::Edge<RelBlockPos, moves::MoveData>>,
1260{
1261    for (i, edge) in path.iter().enumerate() {
1262        let movement_target = RelBlockPos::from_origin(origin, edge.movement.target);
1263
1264        let mut found_edge = None;
1265        for candidate_edge in successors_fn(current_position) {
1266            if candidate_edge.movement.target == movement_target {
1267                found_edge = Some(candidate_edge);
1268                break;
1269            }
1270        }
1271
1272        current_position = movement_target;
1273        // if found_edge is None or the cost increased, then return the index
1274        if found_edge
1275            .map(|found_edge| found_edge.cost > edge.cost)
1276            .unwrap_or(true)
1277        {
1278            // if the node that we're currently executing was obstructed then it's often too
1279            // late to change the path, so it's usually better to just ignore this case :/
1280            if i == 0 {
1281                warn!("path obstructed at index 0 ({edge:?}), ignoring");
1282                continue;
1283            }
1284
1285            return Some(i);
1286        }
1287    }
1288
1289    None
1290}
1291
1292pub fn call_successors_fn(
1293    cached_world: &CachedWorld,
1294    mining_cache: &MiningCache,
1295    custom_state: &CustomPathfinderStateRef,
1296    successors_fn: SuccessorsFn,
1297    pos: RelBlockPos,
1298) -> Vec<astar::Edge<RelBlockPos, moves::MoveData>> {
1299    let mut edges = Vec::with_capacity(16);
1300    let mut ctx = PathfinderCtx {
1301        edges: &mut edges,
1302        world: cached_world,
1303        mining_cache,
1304        custom_state,
1305    };
1306    successors_fn(&mut ctx, pos);
1307    edges
1308}