instant path updates for simple paths, and add follow command to testbot

2 files changed, 32 insertions, 7 deletions

diff --git a/azalea/src/pathfinder/execute/mod.rs b/azalea/src/pathfinder/execute/mod.rs
index f4214054..a06d1c0f 100644
--- a/azalea/src/pathfinder/execute/mod.rs
+++ b/azalea/src/pathfinder/execute/mod.rs
@@ -157,7 +157,10 @@ pub fn check_node_reached(
                     position: **position,
                     physics,
                 };
-                let extra_check = if i == executing_path.path.len() - 1 {
+                let extra_check = if i == executing_path.path.len() - 1
+                    // only do the extra check if we don't have a new path immediately queued up
+                    && executing_path.is_empty_queued_path()
+                {
                     // be extra strict about the velocity and centering if we're on the last node so
                     // we don't fall off
 
@@ -257,7 +260,7 @@ pub fn timeout_movement(
         &WorldName,
         &Inventory,
         Option<&CustomPathfinderState>,
-        Option<&SimulatingPathState>,
+        Option<&mut SimulatingPathState>,
     )>,
     worlds: Res<Worlds>,
 ) {
@@ -274,8 +277,8 @@ pub fn timeout_movement(
     ) in &mut query
     {
         if !executing_path.path.is_empty() {
-            let (start, end) = if let Some(SimulatingPathState::Simulated(simulating_path_state)) =
-                simulating_path_state
+            let (start, end) = if let Some(s) = &simulating_path_state
+                && let SimulatingPathState::Simulated(simulating_path_state) = &**s
             {
                 (simulating_path_state.start, simulating_path_state.target)
             } else {
@@ -302,6 +305,12 @@ pub fn timeout_movement(
                     "pathfinder went too far from path (xz_distance={xz_distance}/{xz_tolerance}, y_distance={y_distance}/{y_tolerance}, line is {start} to {end}, point at {}), trying to patch!",
                     **position
                 );
+
+                if let Some(mut simulating_path_state) = simulating_path_state {
+                    // don't keep executing the simulation
+                    *simulating_path_state = SimulatingPathState::Fail;
+                }
+
                 patch_path_from_timeout(
                     entity,
                     &mut executing_path,
@@ -410,7 +419,11 @@ pub fn recalculate_near_end_of_path(
             continue;
         };
 
-        // start recalculating if the path ends soon
+        // start recalculating if the path ends soon. 50 is arbitrary, that's just to
+        // make us recalculate once when we start nearing the end. this doesn't account
+        // for skipping nodes, though...
+        // TODO: have a variable to store whether we've recalculated, and then check
+        // that `&& path.len() <= 50` to see if we should recalculate.
         if (executing_path.path.len() == 50 || executing_path.path.len() < 5)
             && !pathfinder.is_calculating
             && executing_path.is_path_partial
@@ -513,7 +526,9 @@ pub fn point_line_distance_1d(point: f64, (start, end): (f64, f64)) -> f64 {
     let max = start.max(end);
     if point < min {
         min - point
-    } else {
+    } else if point > max {
         point - max
+    } else {
+        0.
     }
 }
diff --git a/azalea/src/pathfinder/execute/simulation.rs b/azalea/src/pathfinder/execute/simulation.rs
index 7287587b..1b010cca 100644
--- a/azalea/src/pathfinder/execute/simulation.rs
+++ b/azalea/src/pathfinder/execute/simulation.rs
@@ -79,7 +79,7 @@ pub enum SimulatingPathState {
     Fail,
     Simulated(SimulatingPathOpts),
 }
-#[derive(Clone, Component, Debug)]
+#[derive(Clone, Debug)]
 pub struct SimulatingPathOpts {
     pub start: BlockPos,
     pub target: BlockPos,
@@ -89,6 +89,14 @@ pub struct SimulatingPathOpts {
     pub sprinting: bool,
     pub y_rot: f32,
 }
+impl SimulatingPathState {
+    pub fn as_simulated(&self) -> Option<&SimulatingPathOpts> {
+        match self {
+            Self::Fail => None,
+            Self::Simulated(s) => Some(s),
+        }
+    }
+}
 
 #[allow(clippy::type_complexity)]
 pub fn tick_execute_path(
@@ -249,6 +257,8 @@ fn run_simulations(
 
     let mut sim = Simulation::new(world_holder.shared.read().chunks.clone(), player.clone());
 
+    // note that we can't skip more than 50 nodes without causing issues with the
+    // executing_path_limit in goto_listener
     for nodes_ahead in [20, 15, 10, 5, 4, 3, 2, 1, 0] {
         if nodes_ahead + 1 >= executing_path.path.len() {
             // don't simulate to the last node since it has stricter checks