diff --git a/azalea/src/pathfinder/astar.rs b/azalea/src/pathfinder/astar.rs index 8069ae6a..d4812e9b 100644 --- a/azalea/src/pathfinder/astar.rs +++ b/azalea/src/pathfinder/astar.rs @@ -6,7 +6,7 @@ use std::{ time::{Duration, Instant}, }; -use log::{info, warn}; +use log::{trace, warn}; use priority_queue::PriorityQueue; pub struct Path @@ -74,7 +74,7 @@ where .get(&neighbor.movement.target) .map(|n| n.g_score) .unwrap_or(f32::MAX); - if tentative_g_score < neighbor_g_score { + if tentative_g_score - neighbor_g_score < MIN_IMPROVEMENT { let heuristic = heuristic(neighbor.movement.target); let f_score = tentative_g_score + heuristic; nodes.insert( @@ -101,20 +101,22 @@ where if start_time.elapsed() > timeout { // timeout, just return the best path we have so far - info!("Pathfinder timeout"); + trace!("A* couldn't find a path in time, returning best path"); break; } } + let best_path = determine_best_path(&best_paths, &heuristic); + Path { - movements: reconstruct_path(nodes, determine_best_path(&best_paths, heuristic)), + movements: reconstruct_path(nodes, best_path), partial: true, } } const MIN_DISTANCE_PATH: f32 = 5.; -fn determine_best_path(best_node: &[P; 7], heuristic: HeuristicFn) -> P +fn determine_best_path(best_node: &[P; 7], heuristic: &HeuristicFn) -> P where HeuristicFn: Fn(P) -> f32, P: Eq + Hash + Copy + Debug, @@ -128,7 +130,7 @@ where } } warn!("No best node found, returning first node"); - return best_node[0]; + best_node[0] } fn reconstruct_path(mut nodes: HashMap>, current: P) -> Vec> diff --git a/azalea/src/pathfinder/mod.rs b/azalea/src/pathfinder/mod.rs index e2be4da1..890ef9df 100644 --- a/azalea/src/pathfinder/mod.rs +++ b/azalea/src/pathfinder/mod.rs @@ -30,10 +30,10 @@ use bevy_ecs::query::Changed; use bevy_ecs::schedule::IntoSystemConfigs; use bevy_tasks::{AsyncComputeTaskPool, Task}; use futures_lite::future; -use log::{debug, error, trace, warn}; +use log::{debug, error, info, trace, warn}; use std::collections::VecDeque; use std::sync::Arc; -use std::time::Duration; +use std::time::{Duration, Instant}; use self::moves::ExecuteCtx; @@ -69,8 +69,27 @@ impl Plugin for PathfinderPlugin { #[derive(Component, Default)] pub struct Pathfinder { pub path: VecDeque>, + pub queued_path: Option>>, + pub is_path_partial: bool, + pub last_reached_node: Option, + pub last_node_reached_at: Option, + pub goal: Option>, + pub is_calculating: bool, } +#[derive(Event)] +pub struct GotoEvent { + pub entity: Entity, + pub goal: Arc, +} +#[derive(Event)] +pub struct PathFoundEvent { + pub entity: Entity, + pub start: BlockPos, + pub path: Option>>, + pub is_partial: bool, +} + #[allow(clippy::type_complexity)] fn add_default_pathfinder( mut commands: Commands, @@ -100,17 +119,6 @@ impl PathfinderClientExt for azalea_client::Client { }); } } -#[derive(Event)] -pub struct GotoEvent { - pub entity: Entity, - pub goal: Arc, -} -#[derive(Event)] -pub struct PathFoundEvent { - pub entity: Entity, - pub start: BlockPos, - pub path: Option>>, -} #[derive(Component)] pub struct ComputePath(Task>); @@ -118,7 +126,7 @@ pub struct ComputePath(Task>); fn goto_listener( mut commands: Commands, mut events: EventReader, - mut query: Query<(&Position, &InstanceName)>, + mut query: Query<(&mut Pathfinder, &Position, &InstanceName)>, instance_container: Res, ) { let successors_fn = moves::basic::basic_move; @@ -126,9 +134,14 @@ fn goto_listener( let thread_pool = AsyncComputeTaskPool::get(); for event in events.iter() { - let (position, instance_name) = query + let (mut pathfinder, position, instance_name) = query .get_mut(event.entity) .expect("Called goto on an entity that's not in the world"); + + // we store the goal so it can be recalculated later if necessary + pathfinder.goal = Some(event.goal.clone()); + pathfinder.is_calculating = true; + let start = BlockPos::from(position); let world_lock = instance_container @@ -147,28 +160,50 @@ fn goto_listener( successors_fn(&world, pos) }; - let start_time = std::time::Instant::now(); - let astar::Path { movements, partial } = a_star( - start, - |n| goal.heuristic(n), - successors, - |n| goal.success(n), - Duration::from_secs(1), - ); - let end_time = std::time::Instant::now(); - debug!("partial: {partial:?}"); - debug!("time: {:?}", end_time - start_time); + let mut attempt_number = 0; - println!("Path:"); - for movement in &movements { - println!(" {:?}", movement.target); + let mut path; + let mut is_partial: bool; + + 'calculate: loop { + let start_time = std::time::Instant::now(); + let astar::Path { movements, partial } = a_star( + start, + |n| goal.heuristic(n), + successors, + |n| goal.success(n), + Duration::from_secs(if attempt_number == 0 { 1 } else { 5 }), + ); + let end_time = std::time::Instant::now(); + debug!("partial: {partial:?}"); + debug!("time: {:?}", end_time - start_time); + + info!("Path:"); + for movement in &movements { + info!(" {:?}", movement.target); + } + + path = movements.into_iter().collect::>(); + is_partial = partial; + + if path.is_empty() && partial { + if attempt_number == 0 { + debug!("this path is empty, retrying with a higher timeout"); + attempt_number += 1; + continue 'calculate; + } else { + debug!("this path is empty, giving up"); + break 'calculate; + } + } + break; } - let path = movements.into_iter().collect::>(); Some(PathFoundEvent { entity, start, path: Some(path), + is_partial, }) }); @@ -201,12 +236,20 @@ fn path_found_listener(mut events: EventReader, mut query: Query .get_mut(event.entity) .expect("Path found for an entity that doesn't have a pathfinder"); if let Some(path) = &event.path { - pathfinder.path = path.to_owned(); + if pathfinder.path.is_empty() { + pathfinder.path = path.to_owned(); + } else { + pathfinder.queued_path = Some(path.to_owned()); + } pathfinder.last_reached_node = Some(event.start); + pathfinder.last_node_reached_at = Some(Instant::now()); } else { error!("No path found"); pathfinder.path.clear(); + pathfinder.queued_path = None; } + pathfinder.is_calculating = false; + pathfinder.is_path_partial = event.is_partial; } } @@ -216,56 +259,88 @@ fn tick_execute_path( mut sprint_events: EventWriter, mut walk_events: EventWriter, mut jump_events: EventWriter, + mut goto_events: EventWriter, instance_container: Res, ) { let successors_fn = moves::basic::basic_move; for (entity, mut pathfinder, position, physics, instance_name) in &mut query { + if pathfinder.goal.is_none() { + // no goal, no pathfinding + continue; + } + let world_lock = instance_container .get(instance_name) .expect("Entity tried to pathfind but the entity isn't in a valid world"); - loop { - let Some(movement) = pathfinder.path.front() else { - break; - }; + if !pathfinder.is_calculating { + // timeout check + if let Some(last_node_reached_at) = pathfinder.last_node_reached_at { + if last_node_reached_at.elapsed() > Duration::from_secs(2) { + warn!("pathfinder timeout"); + pathfinder.path.clear(); + } + } + } + + 'skip: loop { // we check if the goal was reached *before* actually executing the movement so // we don't unnecessarily execute a movement when it wasn't necessary - if is_goal_reached(movement.target, position, physics) { - // println!("reached target"); - pathfinder.last_reached_node = Some(movement.target); - pathfinder.path.pop_front(); - if pathfinder.path.is_empty() { - // println!("reached goal"); - walk_events.send(StartWalkEvent { - entity, - direction: WalkDirection::None, - }); - } - // tick again, maybe we already reached the next node! - continue; - } + // see if we already reached any future nodes and can skip ahead + for (i, movement) in pathfinder + .path + .clone() + .into_iter() + .enumerate() + .take(10) + .rev() { - // obstruction check - let successors = |pos: BlockPos| { - let world = world_lock.read(); - successors_fn(&world, pos) - }; + if is_goal_reached(movement.target, position, physics) { + pathfinder.path = pathfinder.path.split_off(i + 1); + pathfinder.last_reached_node = Some(movement.target); + pathfinder.last_node_reached_at = Some(Instant::now()); - if let Some(obstructed_index) = - check_path_obstructed( - pathfinder.last_reached_node.expect("last_reached_node is set when we start pathfinding, so it should always be Some here"), - &pathfinder.path, - successors - ) - { - warn!("path obstructed at index {obstructed_index}"); - pathfinder.path.truncate(obstructed_index); - continue; + if let Some(new_path) = pathfinder.queued_path.take() { + debug!( + "swapped path to {:?}", + new_path.iter().take(10).collect::>() + ); + pathfinder.path = new_path; + + if pathfinder.path.is_empty() { + info!("the path we just swapped to was empty, so reached end of path"); + walk_events.send(StartWalkEvent { + entity, + direction: WalkDirection::None, + }); + break; + } + + // run the function again since we just swapped + continue 'skip; + } + + if pathfinder.path.is_empty() { + debug!("pathfinder path is now empty"); + walk_events.send(StartWalkEvent { + entity, + direction: WalkDirection::None, + }); + if let Some(goal) = pathfinder.goal.clone() { + if goal.success(movement.target) { + info!("goal was reached!"); + pathfinder.goal = None; + } + } + } } } + break; + } + if let Some(movement) = pathfinder.path.front() { let ctx = ExecuteCtx { entity, target: movement.target, @@ -277,7 +352,55 @@ fn tick_execute_path( }; trace!("executing move"); (movement.data.execute)(ctx); - break; + } + + { + // obstruction check + let successors = |pos: BlockPos| { + let world = world_lock.read(); + successors_fn(&world, pos) + }; + + if let Some(last_reached_node) = pathfinder.last_reached_node { + if let Some(obstructed_index) = + check_path_obstructed(last_reached_node, &pathfinder.path, successors) + { + warn!("path obstructed at index {obstructed_index} (starting at {last_reached_node:?}, path: {:?})", pathfinder.path); + pathfinder.path.truncate(obstructed_index); + } + } + } + + { + // start recalculating if the path ends soon + if pathfinder.path.len() < 5 && !pathfinder.is_calculating && pathfinder.is_path_partial + { + if let Some(goal) = pathfinder.goal.as_ref().cloned() { + debug!("Recalculating path because it ends soon"); + goto_events.send(GotoEvent { entity, goal }); + + if pathfinder.path.is_empty() { + if let Some(new_path) = pathfinder.queued_path.take() { + pathfinder.path = new_path; + if pathfinder.path.is_empty() { + info!( + "the path we just swapped to was empty, so reached end of path" + ); + walk_events.send(StartWalkEvent { + entity, + direction: WalkDirection::None, + }); + break; + } + } else { + walk_events.send(StartWalkEvent { + entity, + direction: WalkDirection::None, + }); + } + } + } + } } } } @@ -348,7 +471,6 @@ mod tests { use azalea_core::{BlockPos, ChunkPos, Vec3}; use azalea_world::{Chunk, ChunkStorage, PartialChunkStorage}; - use bevy_log::LogPlugin; use log::info; use super::{ @@ -381,10 +503,10 @@ mod tests { start_pos.z as f64 + 0.5, )); let mut simulation = Simulation::new(chunks, player); - simulation.app.add_plugins(LogPlugin { - level: bevy_log::Level::TRACE, - filter: "".to_string(), - }); + // simulation.app.add_plugins(bevy_log::LogPlugin { + // level: bevy_log::Level::TRACE, + // filter: "".to_string(), + // }); simulation.app.world.send_event(GotoEvent { entity: simulation.entity, diff --git a/azalea/src/pathfinder/moves/basic.rs b/azalea/src/pathfinder/moves/basic.rs index b8886c9e..a1175717 100644 --- a/azalea/src/pathfinder/moves/basic.rs +++ b/azalea/src/pathfinder/moves/basic.rs @@ -1,3 +1,5 @@ +use std::f32::consts::SQRT_2; + use azalea_client::{SprintDirection, StartSprintEvent}; use azalea_core::{BlockPos, CardinalDirection}; use azalea_world::Instance; @@ -183,7 +185,7 @@ fn diagonal_move(world: &Instance, pos: BlockPos) -> Vec { if !is_standable(&(pos + offset), world) { continue; } - let cost = SPRINT_ONE_BLOCK_COST * 1.4; + let cost = SPRINT_ONE_BLOCK_COST * SQRT_2; edges.push(Edge { movement: astar::Movement { diff --git a/azalea/src/pathfinder/moves/mod.rs b/azalea/src/pathfinder/moves/mod.rs index 47589264..3fad823c 100644 --- a/azalea/src/pathfinder/moves/mod.rs +++ b/azalea/src/pathfinder/moves/mod.rs @@ -1,5 +1,7 @@ pub mod basic; +use std::fmt::Debug; + use crate::{JumpEvent, LookAtEvent}; use super::astar; @@ -16,6 +18,13 @@ pub struct MoveData { // pub move_kind: BasicMoves, pub execute: &'static (dyn Fn(ExecuteCtx) + Send + Sync), } +impl Debug for MoveData { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.debug_struct("MoveData") + // .field("move_kind", &self.move_kind) + .finish() + } +} /// whether this block is passable fn is_block_passable(pos: &BlockPos, world: &Instance) -> bool { @@ -29,6 +38,12 @@ fn is_block_passable(pos: &BlockPos, world: &Instance) -> bool { if block.waterlogged() { return false; } + // block.waterlogged currently doesn't account for seagrass and some other water + // blocks + if block == azalea_registry::Block::Seagrass.into() { + return false; + } + block.shape() == &collision::empty_shape() } else { false