From 89dac16560c8617e42c13e40983134d37a89ea1b Mon Sep 17 00:00:00 2001
From: Rutherther <fandabohacek@gmail.com>
Date: Fri, 30 Dec 2022 23:51:17 +0100
Subject: [PATCH] fix(pathfinding): update pathfinding to work with same
 position, small distances...

---
 Extensions/NosSmooth.Extensions.Pathfinding/Path.cs   | 11 ++++++++---
 .../NosSmooth.Extensions.Pathfinding/Pathfinder.cs    |  7 ++++++-
 .../NosSmooth.Extensions.Pathfinding/WalkManager.cs   | 10 ++++++++++
 3 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/Extensions/NosSmooth.Extensions.Pathfinding/Path.cs b/Extensions/NosSmooth.Extensions.Pathfinding/Path.cs
index 0918f8ad5546113bbf48875e7a13d565828aaddf..422989c7f10f2d536a446388c7d16da2fc6d42ea 100644
--- a/Extensions/NosSmooth.Extensions.Pathfinding/Path.cs
+++ b/Extensions/NosSmooth.Extensions.Pathfinding/Path.cs
@@ -85,10 +85,15 @@ public class Path
     /// <returns>A position to walk to.</returns>
     public (short X, short Y) TakeForwardPath()
     {
-        if (ReachedEnd || CurrentPartIndex + 2 >= Parts.Count)
+        if (ReachedEnd)
         {
             return (TargetX, TargetY);
         }
+        if (CurrentPartIndex + 2 >= Parts.Count)
+        {
+            CurrentPartIndex++;
+            return (TargetX, TargetY);
+        }
 
         var zeroTile = (CurrentX, CurrentY);
         var firstTile = Parts[++CurrentPartIndex];
@@ -129,11 +134,11 @@ public class Path
         if (xFirstDiff != 0)
         {
             var xRatio = xCurrentDiff / (float)xFirstDiff;
-            return (yFirstDiff * xRatio) - yCurrentDiff < float.Epsilon * 10;
+            return Math.Abs((yFirstDiff * xRatio) - yCurrentDiff) < float.Epsilon;
         }
 
         var yRatio = yCurrentDiff / (float)yFirstDiff;
-        return (xFirstDiff * yRatio) - xCurrentDiff < float.Epsilon * 10;
+        return Math.Abs((xFirstDiff * yRatio) - xCurrentDiff) < float.Epsilon;
     }
 
     /// <summary>
diff --git a/Extensions/NosSmooth.Extensions.Pathfinding/Pathfinder.cs b/Extensions/NosSmooth.Extensions.Pathfinding/Pathfinder.cs
index 1235a0580bb24c37a7a3546cb994609fc9945d14..d5ee45d57f86372c7ff6c6b19e9bbd73f38460f7 100644
--- a/Extensions/NosSmooth.Extensions.Pathfinding/Pathfinder.cs
+++ b/Extensions/NosSmooth.Extensions.Pathfinding/Pathfinder.cs
@@ -90,11 +90,16 @@ public class Pathfinder
         short targetY
     )
     {
-        if (!mapInfo.IsWalkable((short)targetX, (short)targetY))
+        if (!mapInfo.IsWalkable(targetX, targetY))
         {
             return new NotFoundError("The requested target is not walkable, path cannot be found.");
         }
 
+        if (x == targetX && y == targetY)
+        {
+            return new Path(mapInfo.Id, x, y, targetX, targetY, Array.Empty<(short X, short Y)>());
+        }
+
         var target = (targetX, targetY);
         var visited = new HashSet<(short X, short Y)>();
         var offsets = new (short X, short Y)[] { (0, 1), (1, 0), (1, 1), (0, -1), (-1, 0), (-1, -1), (1, -1), (-1, 1) };
diff --git a/Extensions/NosSmooth.Extensions.Pathfinding/WalkManager.cs b/Extensions/NosSmooth.Extensions.Pathfinding/WalkManager.cs
index 64e3e201d8e62f316618ac5376a00e7631b764ff..517ce52171415c609327d83f8d369f51c15406a8 100644
--- a/Extensions/NosSmooth.Extensions.Pathfinding/WalkManager.cs
+++ b/Extensions/NosSmooth.Extensions.Pathfinding/WalkManager.cs
@@ -53,6 +53,11 @@ public class WalkManager
             return Result.FromError(pathResult);
         }
 
+        if (pathResult.Entity.Parts.Count == 0)
+        {
+            return Result.FromSuccess();
+        }
+
         var path = pathResult.Entity;
         while (!path.ReachedEnd)
         {
@@ -71,6 +76,11 @@ public class WalkManager
                     return Result.FromSuccess();
                 }
 
+                if (_state.X == x && _state.Y == y)
+                {
+                    return Result.FromSuccess();
+                }
+
                 return walkResult;
             }
         }