1pub mod astar;
17pub mod costs;
18pub mod custom_state;
19pub mod debug;
20pub mod execute;
21pub mod goals;
22mod goto_event;
23pub mod mining;
24pub mod moves;
25pub mod positions;
26pub mod simulation;
27#[cfg(test)]
28mod tests;
29pub mod world;
30
31use std::{
32 collections::VecDeque,
33 sync::{
34 Arc,
35 atomic::{self, AtomicBool, AtomicUsize},
36 },
37 thread,
38 time::{Duration, Instant},
39};
40
41use astar::Edge;
42use azalea_client::{StartWalkEvent, inventory::InventorySystems, movement::MoveEventsSystems};
43use azalea_core::{
44 position::{BlockPos, Vec3},
45 tick::GameTick,
46};
47use azalea_entity::{LocalEntity, Position, inventory::Inventory, metadata::Player};
48use azalea_world::{WorldName, Worlds};
49use bevy_app::{PreUpdate, Update};
50use bevy_ecs::prelude::*;
51use bevy_tasks::{AsyncComputeTaskPool, Task};
52use custom_state::{CustomPathfinderState, CustomPathfinderStateRef};
53use futures_lite::future;
54pub use goto_event::{GotoEvent, PathfinderOpts};
55use parking_lot::RwLock;
56use positions::RelBlockPos;
57use tokio::sync::broadcast::error::RecvError;
58use tracing::{debug, error, info, warn};
59
60use self::{
61 debug::debug_render_path_with_particles, goals::Goal, mining::MiningCache, moves::SuccessorsFn,
62};
63use crate::{
64 Client, WalkDirection,
65 app::{App, Plugin},
66 ecs::{
67 component::Component,
68 entity::Entity,
69 query::{With, Without},
70 system::{Commands, Query, Res},
71 },
72 pathfinder::{
73 astar::{PathfinderTimeout, a_star},
74 execute::{DefaultPathfinderExecutionPlugin, simulation::SimulatingPathState},
75 moves::MovesCtx,
76 world::CachedWorld,
77 },
78};
79
80#[derive(Clone, Default)]
81pub struct PathfinderPlugin;
82impl Plugin for PathfinderPlugin {
83 fn build(&self, app: &mut App) {
84 app.add_message::<GotoEvent>()
85 .add_message::<PathFoundEvent>()
86 .add_message::<StopPathfindingEvent>()
87 .add_systems(
88 GameTick,
89 debug_render_path_with_particles.in_set(PathfinderSystems),
90 )
91 .add_systems(PreUpdate, add_default_pathfinder.in_set(PathfinderSystems))
92 .add_systems(
93 Update,
94 (
95 goto_listener,
96 handle_tasks,
97 stop_pathfinding_on_world_change,
98 path_found_listener,
99 handle_stop_pathfinding_event,
100 )
101 .chain()
102 .before(MoveEventsSystems)
103 .before(InventorySystems)
104 .in_set(PathfinderSystems),
105 )
106 .add_plugins(DefaultPathfinderExecutionPlugin);
107 }
108}
109
110#[derive(Clone, Debug, Eq, Hash, PartialEq, SystemSet)]
111pub struct PathfinderSystems;
112
113#[derive(Clone, Component, Default)]
115#[non_exhaustive]
116pub struct Pathfinder {
117 pub goal: Option<Arc<dyn Goal>>,
118 pub opts: Option<PathfinderOpts>,
119 pub is_calculating: bool,
120 pub goto_id: Arc<AtomicUsize>,
121}
122
123#[derive(Clone, Component)]
126pub struct ExecutingPath {
127 pub path: VecDeque<astar::Edge<BlockPos, moves::MoveData>>,
128 pub queued_path: Option<VecDeque<astar::Edge<BlockPos, moves::MoveData>>>,
129 pub last_reached_node: BlockPos,
130 pub ticks_since_last_node_reached: usize,
133 pub is_path_partial: bool,
134}
135impl ExecutingPath {
136 pub fn is_empty_queued_path(&self) -> bool {
137 self.queued_path.is_none() || self.queued_path.as_ref().is_some_and(|p| p.is_empty())
138 }
139}
140
141#[derive(Clone, Debug, Message)]
142#[non_exhaustive]
143pub struct PathFoundEvent {
144 pub entity: Entity,
145 pub start: BlockPos,
146 pub path: Option<VecDeque<astar::Edge<BlockPos, moves::MoveData>>>,
147 pub is_partial: bool,
148 pub successors_fn: SuccessorsFn,
149 pub allow_mining: bool,
150}
151
152#[allow(clippy::type_complexity)]
153pub fn add_default_pathfinder(
154 mut commands: Commands,
155 mut query: Query<Entity, (Without<Pathfinder>, With<LocalEntity>, With<Player>)>,
156) {
157 for entity in &mut query {
158 commands.entity(entity).insert(Pathfinder::default());
159 }
160}
161
162pub trait PathfinderClientExt {
163 fn goto(&self, goal: impl Goal + 'static) -> impl Future<Output = ()>;
176 fn goto_with_opts(
191 &self,
192 goal: impl Goal + 'static,
193 opts: PathfinderOpts,
194 ) -> impl Future<Output = ()>;
195 fn start_goto(&self, goal: impl Goal + 'static);
205 fn start_goto_with_opts(&self, goal: impl Goal + 'static, opts: PathfinderOpts);
211 fn stop_pathfinding(&self);
219 fn force_stop_pathfinding(&self);
222 fn wait_until_goto_target_reached(&self) -> impl Future<Output = ()>;
224 fn is_goto_target_reached(&self) -> bool;
227 fn is_executing_path(&self) -> bool;
232 fn is_calculating_path(&self) -> bool;
237}
238
239impl PathfinderClientExt for Client {
240 async fn goto(&self, goal: impl Goal + 'static) {
241 self.goto_with_opts(goal, PathfinderOpts::new()).await;
242 }
243 async fn goto_with_opts(&self, goal: impl Goal + 'static, opts: PathfinderOpts) {
244 self.start_goto_with_opts(goal, opts);
245 self.wait_until_goto_target_reached().await;
246 }
247 fn start_goto(&self, goal: impl Goal + 'static) {
248 self.start_goto_with_opts(goal, PathfinderOpts::new());
249 }
250 fn start_goto_with_opts(&self, goal: impl Goal + 'static, opts: PathfinderOpts) {
251 self.ecs
252 .write()
253 .write_message(GotoEvent::new(self.entity, goal, opts));
254 }
255 fn stop_pathfinding(&self) {
256 self.ecs.write().write_message(StopPathfindingEvent {
257 entity: self.entity,
258 force: false,
259 });
260 }
261 fn force_stop_pathfinding(&self) {
262 self.ecs.write().write_message(StopPathfindingEvent {
263 entity: self.entity,
264 force: true,
265 });
266 }
267
268 async fn wait_until_goto_target_reached(&self) {
269 self.wait_updates(1).await;
272
273 let mut tick_broadcaster = self.get_tick_broadcaster();
274 while !self.is_goto_target_reached() {
275 match tick_broadcaster.recv().await {
277 Ok(_) => (),
278 Err(RecvError::Closed) => return,
279 Err(err) => warn!("{err}"),
280 };
281 }
282 }
283 fn is_goto_target_reached(&self) -> bool {
284 self.get_component::<Pathfinder>()
285 .is_none_or(|p| p.goal.is_none() && !p.is_calculating)
286 }
287 fn is_executing_path(&self) -> bool {
288 self.get_component::<ExecutingPath>().is_some()
289 }
290 fn is_calculating_path(&self) -> bool {
291 self.get_component::<Pathfinder>()
292 .is_some_and(|p| p.is_calculating)
293 }
294}
295
296#[derive(Component)]
297pub struct ComputePath(Task<Option<PathFoundEvent>>);
298
299#[allow(clippy::type_complexity)]
300pub fn goto_listener(
301 mut commands: Commands,
302 mut events: MessageReader<GotoEvent>,
303 mut path_found_events: MessageWriter<PathFoundEvent>,
304 mut query: Query<(
305 &mut Pathfinder,
306 Option<&mut ExecutingPath>,
307 Option<&SimulatingPathState>,
308 &Position,
309 &WorldName,
310 &Inventory,
311 Option<&CustomPathfinderState>,
312 )>,
313 worlds: Res<Worlds>,
314) {
315 let thread_pool = AsyncComputeTaskPool::get();
316
317 for event in events.read() {
318 let Ok((
319 mut pathfinder,
320 executing_path,
321 simulating_path_state,
322 position,
323 world_name,
324 inventory,
325 custom_state,
326 )) = query.get_mut(event.entity)
327 else {
328 warn!("got goto event for an entity that can't pathfind");
329 continue;
330 };
331
332 if env!("OPT_LEVEL") == "0" {
334 static WARNED: AtomicBool = AtomicBool::new(false);
335 if !WARNED.swap(true, atomic::Ordering::Relaxed) {
336 warn!(
337 "Azalea was compiled with no optimizations, which may result in significantly reduced pathfinding performance. Consider following the steps at https://azalea.matdoes.dev/azalea/#optimization for faster performance in debug mode."
338 )
339 }
340 }
341
342 let cur_pos = player_pos_to_block_pos(**position);
343
344 if event.goal.success(cur_pos) {
345 pathfinder.goal = None;
347 pathfinder.opts = None;
348 pathfinder.is_calculating = false;
349 debug!("already at goal, not pathfinding");
350 continue;
351 }
352
353 pathfinder.goal = Some(event.goal.clone());
355 pathfinder.opts = Some(event.opts.clone());
356 pathfinder.is_calculating = true;
357
358 let world_lock = worlds
359 .get(world_name)
360 .expect("Entity tried to pathfind but the entity isn't in a valid world");
361
362 let goal = event.goal.clone();
363 let entity = event.entity;
364
365 let goto_id_atomic = pathfinder.goto_id.clone();
366
367 let allow_mining = event.opts.allow_mining;
368 let inventory_menu = if allow_mining {
369 Some(inventory.inventory_menu.clone())
370 } else {
371 None
372 };
373
374 let custom_state = custom_state.cloned().unwrap_or_default();
375 let opts = event.opts.clone();
376
377 let mut start = cur_pos;
379
380 if let Some(mut executing_path) = executing_path {
381 let instant_path_start = simulating_path_state
386 .and_then(|s| s.as_simulated().map(|s| s.target))
387 .unwrap_or_else(|| {
388 executing_path
389 .path
390 .iter()
391 .next()
392 .map(|e| e.movement.target)
393 .unwrap_or(cur_pos)
394 });
395
396 let path_found_event = calculate_path(CalculatePathCtx {
397 entity,
398 start: instant_path_start,
399 goal: goal.clone(),
400 world_lock: world_lock.clone(),
401 goto_id_atomic: goto_id_atomic.clone(),
402 mining_cache: MiningCache::new(inventory_menu.clone()),
403 custom_state: custom_state.clone(),
404 opts: PathfinderOpts {
405 min_timeout: PathfinderTimeout::Nodes(2_000),
406 max_timeout: PathfinderTimeout::Nodes(2_000),
407 ..opts
408 },
409 });
410
411 if let Some(path_found_event) = path_found_event
412 && !path_found_event.is_partial
413 {
414 debug!("Found path instantly!");
415
416 let instant_path_start_index = executing_path
419 .path
420 .iter()
421 .position(|e| e.movement.target == instant_path_start);
422 if let Some(instant_path_start_index) = instant_path_start_index {
423 let truncate_to_len = instant_path_start_index + 1;
424 debug!("truncating to {truncate_to_len} for instant path");
425 executing_path.path.truncate(truncate_to_len);
426
427 path_found_events.write(path_found_event);
428
429 continue;
431 } else {
432 warn!(
433 "we just calculated an instant path, but the start of it isn't in the current path? instant_path_start: {instant_path_start:?}, simulating_path_state: {simulating_path_state:?}, executing_path.path: {:?}",
434 executing_path.path
435 )
436 }
437 }
438
439 if !executing_path.path.is_empty() {
440 let executing_path_limit = 50;
443
444 executing_path.path.truncate(executing_path_limit);
446
447 start = executing_path
448 .path
449 .back()
450 .expect("path was just checked to not be empty")
451 .movement
452 .target;
453 }
454 }
455
456 if start == cur_pos {
457 info!("got goto {:?}, starting from {start:?}", event.goal);
458 } else {
459 info!(
460 "got goto {:?}, starting from {start:?} (currently at {cur_pos:?})",
461 event.goal,
462 );
463 }
464
465 let mining_cache = MiningCache::new(inventory_menu);
466 let task = thread_pool.spawn(async move {
467 calculate_path(CalculatePathCtx {
468 entity,
469 start,
470 goal,
471 world_lock,
472 goto_id_atomic,
473 mining_cache,
474 custom_state,
475 opts,
476 })
477 });
478
479 commands.entity(event.entity).insert(ComputePath(task));
480 }
481}
482
483#[inline]
489pub fn player_pos_to_block_pos(position: Vec3) -> BlockPos {
490 BlockPos::from(position.up(0.5))
492}
493
494pub struct CalculatePathCtx {
495 pub entity: Entity,
496 pub start: BlockPos,
497 pub goal: Arc<dyn Goal>,
498 pub world_lock: Arc<RwLock<azalea_world::World>>,
499 pub goto_id_atomic: Arc<AtomicUsize>,
500 pub mining_cache: MiningCache,
501 pub custom_state: CustomPathfinderState,
502
503 pub opts: PathfinderOpts,
504}
505
506pub fn calculate_path(ctx: CalculatePathCtx) -> Option<PathFoundEvent> {
515 debug!("start: {}", ctx.start);
516
517 let goto_id = ctx.goto_id_atomic.fetch_add(1, atomic::Ordering::SeqCst) + 1;
518
519 let origin = ctx.start;
520 let cached_world = CachedWorld::new(ctx.world_lock, origin);
521 let successors = |pos: RelBlockPos| {
522 call_successors_fn(
523 &cached_world,
524 &ctx.mining_cache,
525 &ctx.custom_state.0.read(),
526 ctx.opts.successors_fn,
527 pos,
528 )
529 };
530
531 let start_time = Instant::now();
532
533 let astar::Path {
534 movements,
535 is_partial,
536 cost,
537 } = a_star(
538 RelBlockPos::get_origin(origin),
539 |n| ctx.goal.heuristic(n.apply(origin)),
540 successors,
541 |n| ctx.goal.success(n.apply(origin)),
542 ctx.opts.min_timeout,
543 ctx.opts.max_timeout,
544 );
545 let end_time = Instant::now();
546 debug!("partial: {is_partial:?}, cost: {cost}");
547 let duration = end_time - start_time;
548 if is_partial {
549 if movements.is_empty() {
550 info!("Pathfinder took {duration:?} (empty path)");
551 } else {
552 info!("Pathfinder took {duration:?} (incomplete path)");
553 }
554 thread::sleep(Duration::from_millis(100));
556 } else {
557 info!("Pathfinder took {duration:?}");
558 }
559
560 debug!("Path:");
561 for movement in &movements {
562 debug!(" {}", movement.target.apply(origin));
563 }
564
565 let path = movements.into_iter().collect::<VecDeque<_>>();
566
567 let goto_id_now = ctx.goto_id_atomic.load(atomic::Ordering::SeqCst);
568 if goto_id != goto_id_now {
569 warn!("finished calculating a path, but it's outdated");
571 return None;
572 }
573
574 if path.is_empty() && is_partial {
575 debug!("this path is empty, we might be stuck :(");
576 }
577
578 let mut mapped_path = VecDeque::with_capacity(path.len());
579 let mut current_position = RelBlockPos::get_origin(origin);
580 for movement in path {
581 let mut found_edge = None;
582 for edge in successors(current_position) {
583 if edge.movement.target == movement.target {
584 found_edge = Some(edge);
585 break;
586 }
587 }
588
589 let found_edge = found_edge.expect(
590 "path should always still be possible because we're using the same world cache",
591 );
592 current_position = found_edge.movement.target;
593
594 mapped_path.push_back(Edge {
597 movement: astar::Movement {
598 target: movement.target.apply(origin),
599 data: movement.data,
600 },
601 cost: found_edge.cost,
602 });
603 }
604
605 Some(PathFoundEvent {
606 entity: ctx.entity,
607 start: ctx.start,
608 path: Some(mapped_path),
609 is_partial,
610 successors_fn: ctx.opts.successors_fn,
611 allow_mining: ctx.opts.allow_mining,
612 })
613}
614
615pub fn handle_tasks(
617 mut commands: Commands,
618 mut transform_tasks: Query<(Entity, &mut ComputePath)>,
619 mut path_found_events: MessageWriter<PathFoundEvent>,
620) {
621 for (entity, mut task) in &mut transform_tasks {
622 if let Some(optional_path_found_event) = future::block_on(future::poll_once(&mut task.0)) {
623 if let Some(path_found_event) = optional_path_found_event {
624 path_found_events.write(path_found_event);
625 }
626
627 commands.entity(entity).remove::<ComputePath>();
629 }
630 }
631}
632
633#[allow(clippy::type_complexity)]
635pub fn path_found_listener(
636 mut events: MessageReader<PathFoundEvent>,
637 mut query: Query<(
638 &mut Pathfinder,
639 Option<&mut ExecutingPath>,
640 &WorldName,
641 &Inventory,
642 Option<&CustomPathfinderState>,
643 )>,
644 worlds: Res<Worlds>,
645 mut commands: Commands,
646) {
647 for event in events.read() {
648 let Ok((mut pathfinder, executing_path, world_name, inventory, custom_state)) =
649 query.get_mut(event.entity)
650 else {
651 debug!("got path found event for an entity that can't pathfind");
652 continue;
653 };
654 if let Some(found_path) = &event.path {
655 if let Some(mut executing_path) = executing_path {
656 let mut new_path = VecDeque::new();
657
658 if let Some(last_node_of_current_path) = executing_path.path.back() {
661 let world_lock = worlds
662 .get(world_name)
663 .expect("Entity tried to pathfind but the entity isn't in a valid world");
664 let origin = event.start;
665 let successors_fn: moves::SuccessorsFn = event.successors_fn;
666 let cached_world = CachedWorld::new(world_lock, origin);
667 let mining_cache = MiningCache::new(if event.allow_mining {
668 Some(inventory.inventory_menu.clone())
669 } else {
670 None
671 });
672 let custom_state = custom_state.cloned().unwrap_or_default();
673 let custom_state_ref = custom_state.0.read();
674 let successors = |pos: RelBlockPos| {
675 call_successors_fn(
676 &cached_world,
677 &mining_cache,
678 &custom_state_ref,
679 successors_fn,
680 pos,
681 )
682 };
683
684 if let Some(first_node_of_new_path) = found_path.front() {
685 let last_target_of_current_path = RelBlockPos::from_origin(
686 origin,
687 last_node_of_current_path.movement.target,
688 );
689 let first_target_of_new_path = RelBlockPos::from_origin(
690 origin,
691 first_node_of_new_path.movement.target,
692 );
693
694 if successors(last_target_of_current_path)
695 .iter()
696 .any(|edge| edge.movement.target == first_target_of_new_path)
697 {
698 debug!("combining old and new paths");
699 debug!(
700 "old path: {:?}",
701 executing_path.path.iter().collect::<Vec<_>>()
702 );
703 debug!(
704 "new path: {:?}",
705 found_path.iter().take(10).collect::<Vec<_>>()
706 );
707 new_path.extend(executing_path.path.iter().cloned());
708 }
709 } else {
710 new_path.extend(executing_path.path.iter().cloned());
711 }
712 }
713
714 new_path.extend(found_path.to_owned());
715
716 debug!(
717 "set queued path to {:?}",
718 new_path.iter().take(10).collect::<Vec<_>>()
719 );
720 executing_path.queued_path = Some(new_path);
721 executing_path.is_path_partial = event.is_partial;
722 } else if found_path.is_empty() {
723 debug!("calculated path is empty, so didn't add ExecutingPath");
724 if !pathfinder.opts.as_ref().is_some_and(|o| o.retry_on_no_path) {
725 debug!("retry_on_no_path is set to false, removing goal");
726 pathfinder.goal = None;
727 }
728 } else {
729 commands.entity(event.entity).insert(ExecutingPath {
730 path: found_path.to_owned(),
731 queued_path: None,
732 last_reached_node: event.start,
733 ticks_since_last_node_reached: 0,
734 is_path_partial: event.is_partial,
735 });
736 debug!(
737 "set path to {:?}",
738 found_path.iter().take(10).collect::<Vec<_>>()
739 );
740 debug!("partial: {}", event.is_partial);
741 }
742 } else {
743 error!("No path found");
744 if let Some(mut executing_path) = executing_path {
745 executing_path.queued_path = Some(VecDeque::new());
747 } else {
748 }
750 }
751 pathfinder.is_calculating = false;
752 }
753}
754
755#[derive(Message)]
756pub struct StopPathfindingEvent {
757 pub entity: Entity,
758 pub force: bool,
764}
765
766pub fn handle_stop_pathfinding_event(
767 mut events: MessageReader<StopPathfindingEvent>,
768 mut query: Query<(&mut Pathfinder, &mut ExecutingPath)>,
769 mut walk_events: MessageWriter<StartWalkEvent>,
770 mut commands: Commands,
771) {
772 for event in events.read() {
773 commands.entity(event.entity).remove::<ComputePath>();
775
776 let Ok((mut pathfinder, mut executing_path)) = query.get_mut(event.entity) else {
777 continue;
778 };
779 pathfinder.goal = None;
780 if event.force {
781 executing_path.path.clear();
782 executing_path.queued_path = None;
783 } else {
784 executing_path.queued_path = Some(VecDeque::new());
786 executing_path.is_path_partial = false;
788 }
789
790 if executing_path.path.is_empty() {
791 walk_events.write(StartWalkEvent {
792 entity: event.entity,
793 direction: WalkDirection::None,
794 });
795 commands.entity(event.entity).remove::<ExecutingPath>();
796 }
797 }
798}
799
800pub fn stop_pathfinding_on_world_change(
801 mut query: Query<(Entity, &mut ExecutingPath), Changed<WorldName>>,
802 mut stop_pathfinding_events: MessageWriter<StopPathfindingEvent>,
803) {
804 for (entity, mut executing_path) in &mut query {
805 if !executing_path.path.is_empty() {
806 debug!("world changed, clearing path");
807 executing_path.path.clear();
808 stop_pathfinding_events.write(StopPathfindingEvent {
809 entity,
810 force: true,
811 });
812 }
813 }
814}
815
816pub fn call_successors_fn(
817 cached_world: &CachedWorld,
818 mining_cache: &MiningCache,
819 custom_state: &CustomPathfinderStateRef,
820 successors_fn: SuccessorsFn,
821 pos: RelBlockPos,
822) -> Vec<astar::Edge<RelBlockPos, moves::MoveData>> {
823 let mut edges = Vec::with_capacity(16);
824 let mut ctx = MovesCtx {
825 edges: &mut edges,
826 world: cached_world,
827 mining_cache,
828 custom_state,
829 };
830 successors_fn(&mut ctx, pos);
831 edges
832}