Improve pathfinding capabilities (#1999)

* Add `ClientIsMoving()` API to determine if currently walking/falling
* Improve `MoveToLocation()` performance and allow approaching location
Co-authored-by: ORelio <[email protected]>

Author: Daenges

MinecraftClient/ChatBot.cs

@@ -983,13 +983,26 @@ protected Mapping.Location GetCurrentLocation()
         /// </summary>
         /// <param name="location">Location to reach</param>
         /// <param name="allowUnsafe">Allow possible but unsafe locations thay may hurt the player: lava, cactus...</param>
-        /// <param name="allowDirectTeleport">Allow non-vanilla teleport instead of computing path, but may cause invalid moves and/or trigger anti-cheat plugins</param>
+        /// <param name="allowDirectTeleport">Allow non-vanilla direct teleport instead of computing path, but may cause invalid moves and/or trigger anti-cheat plugins</param>
+        /// <param name="maxOffset">If no valid path can be found, also allow locations within specified distance of destination</param>
+        /// <param name="minOffset">Do not get closer of destination than specified distance</param>
+        /// <param name="timeout">How long to wait before stopping computation (default: 5 seconds)</param>
+        /// <remarks>When location is unreachable, computation will reach timeout, then optionally fallback to a close location within maxOffset</remarks>
         /// <returns>True if a path has been found</returns>
-        protected bool MoveToLocation(Mapping.Location location, bool allowUnsafe = false, bool allowDirectTeleport = false)
+        protected bool MoveToLocation(Mapping.Location location, bool allowUnsafe = false, bool allowDirectTeleport = false, int maxOffset = 0, int minOffset = 0, TimeSpan? timeout = null)
         {
-            return Handler.MoveTo(location, allowUnsafe, allowDirectTeleport);
+            return Handler.MoveTo(location, allowUnsafe, allowDirectTeleport, maxOffset, minOffset, timeout);
         }
 
+        /// <summary>
+        /// Check if the client is currently processing a Movement.
+        /// </summary>
+        /// <returns>true if a movement is currently handled</returns>
+        protected bool ClientIsMoving()
+        {
+            return Handler.ClientIsMoving();
+        }
+        
         /// <summary>
         /// Look at the specified location
         /// </summary>

MinecraftClient/Mapping/Movement.cs

@@ -1,7 +1,8 @@
 using System;
 using System.Collections.Generic;
 using System.Linq;
-using System.Text;
+using System.Threading;
+using System.Threading.Tasks;
 
 namespace MinecraftClient.Mapping
 {
@@ -129,62 +130,120 @@ public static Queue<Location> Move2Steps(Location start, Location goal, ref doub
         /// <param name="start">Start location</param>
         /// <param name="goal">Destination location</param>
         /// <param name="allowUnsafe">Allow possible but unsafe locations</param>
+        /// <param name="maxOffset">If no valid path can be found, also allow locations within specified distance of destination</param>
+        /// <param name="minOffset">Do not get closer of destination than specified distance</param>
+        /// <param name="timeout">How long to wait before stopping computation</param>
+        /// <remarks>When location is unreachable, computation will reach timeout, then optionally fallback to a close location within maxOffset</remarks>
         /// <returns>A list of locations, or null if calculation failed</returns>
-        public static Queue<Location> CalculatePath(World world, Location start, Location goal, bool allowUnsafe = false)
+        public static Queue<Location> CalculatePath(World world, Location start, Location goal, bool allowUnsafe, int maxOffset, int minOffset, TimeSpan timeout)
         {
-            Queue<Location> result = null;
+            CancellationTokenSource cts = new CancellationTokenSource();
+            Task<Queue<Location>> pathfindingTask = Task.Factory.StartNew(() => Movement.CalculatePath(world, start, goal, allowUnsafe, maxOffset, minOffset, cts.Token));
+            pathfindingTask.Wait(timeout);
+            if (!pathfindingTask.IsCompleted)
+            {
+                cts.Cancel();
+                pathfindingTask.Wait();
+            }
+            return pathfindingTask.Result;
+        }
+
+        /// <summary>
+        /// Calculate a path from the start location to the destination location
+        /// </summary>
+        /// <remarks>
+        /// Based on the A* pathfinding algorithm described on Wikipedia
+        /// </remarks>
+        /// <see href="https://en.wikipedia.org/wiki/A*_search_algorithm#Pseudocode"/>
+        /// <param name="start">Start location</param>
+        /// <param name="goal">Destination location</param>
+        /// <param name="allowUnsafe">Allow possible but unsafe locations</param>
+        /// <param name="maxOffset">If no valid path can be found, also allow locations within specified distance of destination</param>
+        /// <param name="minOffset">Do not get closer of destination than specified distance</param>
+        /// <param name="ct">Token for stopping computation after a certain time</param>
+        /// <returns>A list of locations, or null if calculation failed</returns>
+        public static Queue<Location> CalculatePath(World world, Location start, Location goal, bool allowUnsafe, int maxOffset, int minOffset, CancellationToken ct)
+        {
+
+            if (minOffset > maxOffset)
+                throw new ArgumentException("minOffset must be lower or equal to maxOffset", "minOffset");
+
+            Location current = new Location(); // Location that is currently processed
+            Location closestGoal = new Location(); // Closest Location to the goal. Used for approaching if goal can not be reached or was not found.
+            HashSet<Location> ClosedSet = new HashSet<Location>(); // The set of locations already evaluated.
+            HashSet<Location> OpenSet = new HashSet<Location>(new[] { start });  // The set of tentative nodes to be evaluated, initially containing the start node
+            Dictionary<Location, Location> Came_From = new Dictionary<Location, Location>(); // The map of navigated nodes.
+
+            Dictionary<Location, int> g_score = new Dictionary<Location, int>(); //:= map with default value of Infinity
+            g_score[start] = 0; // Cost from start along best known path.
+            // Estimated total cost from start to goal through y.
+            Dictionary<Location, int> f_score = new Dictionary<Location, int>(); //:= map with default value of Infinity
+            f_score[start] = (int)start.DistanceSquared(goal); //heuristic_cost_estimate(start, goal)
 
-            AutoTimeout.Perform(() =>
+            while (OpenSet.Count > 0)
             {
-                HashSet<Location> ClosedSet = new HashSet<Location>(); // The set of locations already evaluated.
-                HashSet<Location> OpenSet = new HashSet<Location>(new[] { start });  // The set of tentative nodes to be evaluated, initially containing the start node
-                Dictionary<Location, Location> Came_From = new Dictionary<Location, Location>(); // The map of navigated nodes.
+                current = //the node in OpenSet having the lowest f_score[] value
+                    OpenSet.Select(location => f_score.ContainsKey(location)
+                    ? new KeyValuePair<Location, int>(location, f_score[location])
+                    : new KeyValuePair<Location, int>(location, int.MaxValue))
+                    .OrderBy(pair => pair.Value).First().Key;
 
-                Dictionary<Location, int> g_score = new Dictionary<Location, int>(); //:= map with default value of Infinity
-                g_score[start] = 0; // Cost from start along best known path.
-                // Estimated total cost from start to goal through y.
-                Dictionary<Location, int> f_score = new Dictionary<Location, int>(); //:= map with default value of Infinity
-                f_score[start] = (int)start.DistanceSquared(goal); //heuristic_cost_estimate(start, goal)
+                // Only assert a value if it is of actual use later
+                if (maxOffset > 0 && ClosedSet.Count > 0)
+                    // Get the block that currently is closest to the goal
+                    closestGoal = ClosedSet.OrderBy(checkedLocation => checkedLocation.DistanceSquared(goal)).First();
 
-                while (OpenSet.Count > 0)
+                // Stop when goal is reached or we are close enough
+                if (current == goal || (minOffset > 0 && current.DistanceSquared(goal) <= minOffset))
+                    return ReconstructPath(Came_From, current);
+                else if (ct.IsCancellationRequested)
+                    break;              // Return if we are cancelled
+
+                OpenSet.Remove(current);
+                ClosedSet.Add(current);
+
+                foreach (Location neighbor in GetAvailableMoves(world, current, allowUnsafe))
                 {
-                    Location current = //the node in OpenSet having the lowest f_score[] value
-                        OpenSet.Select(location => f_score.ContainsKey(location)
-                        ? new KeyValuePair<Location, int>(location, f_score[location])
-                        : new KeyValuePair<Location, int>(location, int.MaxValue))
-                        .OrderBy(pair => pair.Value).First().Key;
-                    if (current == goal)
-                    { //reconstruct_path(Came_From, goal)
-                        List<Location> total_path = new List<Location>(new[] { current });
-                        while (Came_From.ContainsKey(current))
-                        {
-                            current = Came_From[current];
-                            total_path.Add(current);
-                        }
-                        total_path.Reverse();
-                        result = new Queue<Location>(total_path);
-                    }
-                    OpenSet.Remove(current);
-                    ClosedSet.Add(current);
-                    foreach (Location neighbor in GetAvailableMoves(world, current, allowUnsafe))
-                    {
-                        if (ClosedSet.Contains(neighbor))
-                            continue;		// Ignore the neighbor which is already evaluated.
-                        int tentative_g_score = g_score[current] + (int)current.DistanceSquared(neighbor); //dist_between(current,neighbor) // length of this path.
-                        if (!OpenSet.Contains(neighbor))	// Discover a new node
-                            OpenSet.Add(neighbor);
-                        else if (tentative_g_score >= g_score[neighbor])
-                            continue;		// This is not a better path.
+                    if (ct.IsCancellationRequested)
+                        break;          // Stop searching for blocks if we are cancelled.
+                    if (ClosedSet.Contains(neighbor))
+                        continue;       // Ignore the neighbor which is already evaluated.
+                    int tentative_g_score = g_score[current] + (int)current.DistanceSquared(neighbor); //dist_between(current,neighbor) // length of this path.
+                    if (!OpenSet.Contains(neighbor))    // Discover a new node
+                        OpenSet.Add(neighbor);
+                    else if (tentative_g_score >= g_score[neighbor])
+                        continue;       // This is not a better path.
 
-                        // This path is the best until now. Record it!
-                        Came_From[neighbor] = current;
-                        g_score[neighbor] = tentative_g_score;
-                        f_score[neighbor] = g_score[neighbor] + (int)neighbor.DistanceSquared(goal); //heuristic_cost_estimate(neighbor, goal)
-                    }
+                    // This path is the best until now. Record it!
+                    Came_From[neighbor] = current;
+                    g_score[neighbor] = tentative_g_score;
+                    f_score[neighbor] = g_score[neighbor] + (int)neighbor.DistanceSquared(goal); //heuristic_cost_estimate(neighbor, goal)
                 }
-            }, TimeSpan.FromSeconds(5));
+            }
 
-            return result;
+            // Goal could not be reached. Set the path to the closest location if close enough
+            if (maxOffset == int.MaxValue || goal.DistanceSquared(closestGoal) <= maxOffset)            
+                return ReconstructPath(Came_From, closestGoal);
+            else
+                return null;
+        }
+
+        /// <summary>
+        /// Helper function for CalculatePath(). Backtrack from goal to start to reconstruct a step-by-step path.
+        /// </summary>
+        /// <param name="Came_From">The collection of Locations that leads back to the start</param>
+        /// <param name="current">Endpoint of our later walk</param>
+        /// <returns>the path that leads to current from the start position</returns>
+        private static Queue<Location> ReconstructPath(Dictionary<Location, Location> Came_From, Location current)
+        {
+            List<Location> total_path = new List<Location>(new[] { current });
+            while (Came_From.ContainsKey(current))
+            {
+                current = Came_From[current];
+                total_path.Add(current);
+            }
+            total_path.Reverse();
+            return new Queue<Location>(total_path);
         }
 
         /* ========= LOCATION PROPERTIES ========= */

MinecraftClient/McClient.cs

@@ -1061,8 +1061,12 @@ public Dictionary<string, string> GetOnlinePlayersWithUUID()
         /// <param name="location">Location to reach</param>
         /// <param name="allowUnsafe">Allow possible but unsafe locations thay may hurt the player: lava, cactus...</param>
         /// <param name="allowDirectTeleport">Allow non-vanilla direct teleport instead of computing path, but may cause invalid moves and/or trigger anti-cheat plugins</param>
+        /// <param name="maxOffset">If no valid path can be found, also allow locations within specified distance of destination</param>
+        /// <param name="minOffset">Do not get closer of destination than specified distance</param>
+        /// <param name="timeout">How long to wait until the path is evaluated (default: 5 seconds)</param>
+        /// <remarks>When location is unreachable, computation will reach timeout, then optionally fallback to a close location within maxOffset</remarks>
         /// <returns>True if a path has been found</returns>
-        public bool MoveTo(Location location, bool allowUnsafe = false, bool allowDirectTeleport = false)
+        public bool MoveTo(Location location, bool allowUnsafe = false, bool allowDirectTeleport = false, int maxOffset = 0, int minOffset = 0, TimeSpan? timeout=null)
         {
             lock (locationLock)
             {
@@ -1078,7 +1082,7 @@ public bool MoveTo(Location location, bool allowUnsafe = false, bool allowDirect
                     // Calculate path through pathfinding. Path contains a list of 1-block movement that will be divided into steps
                     if (Movement.GetAvailableMoves(world, this.location, allowUnsafe).Contains(location))
                         path = new Queue<Location>(new[] { location });
-                    else path = Movement.CalculatePath(world, this.location, location, allowUnsafe);
+                    else path = Movement.CalculatePath(world, this.location, location, allowUnsafe, maxOffset, minOffset, timeout ?? TimeSpan.FromSeconds(5));
                     return path != null;
                 }
             }
@@ -1847,6 +1851,15 @@ public void OnRespawn()
             DispatchBotEvent(bot => bot.OnRespawn());
         }
 
+        /// <summary>
+        /// Check if the client is currently processing a Movement.
+        /// </summary>
+        /// <returns>true if a movement is currently handled</returns>
+        public bool ClientIsMoving() 
+        {
+            return terrainAndMovementsEnabled && locationReceived && ((steps != null && steps.Count > 0) || (path != null && path.Count > 0));
+        }
+
         /// <summary>
         /// Called when the server sends a new player location,
         /// or if a ChatBot whishes to update the player's location.