Pathfinder rework (#11452)

Content.Server/NPC/Pathfinding/PathfindingSystem.Common.cs

@@ -0,0 +1,151 @@
+using Content.Shared.NPC;
+
+namespace Content.Server.NPC.Pathfinding;
+
+public sealed partial class PathfindingSystem
+{
+    /*
+     * Code that is common to all pathfinding methods.
+     */
+
+    /// <summary>
+    /// Maximum amount of nodes we're allowed to expand.
+    /// </summary>
+    private const int NodeLimit = 200;
+
+    private sealed class PathComparer : IComparer<ValueTuple<float, PathPoly>>
+    {
+        public int Compare((float, PathPoly) x, (float, PathPoly) y)
+        {
+            return y.Item1.CompareTo(x.Item1);
+        }
+    }
+
+    private static readonly PathComparer PathPolyComparer = new();
+
+    private Queue<PathPoly> ReconstructPath(Dictionary<PathPoly, PathPoly> path, PathPoly currentNodeRef)
+    {
+        var running = new List<PathPoly> { currentNodeRef };
+        while (path.ContainsKey(currentNodeRef))
+        {
+            var previousCurrent = currentNodeRef;
+            currentNodeRef = path[currentNodeRef];
+            path.Remove(previousCurrent);
+            running.Add(currentNodeRef);
+        }
+
+        running = Simplify(running);
+        running.Reverse();
+        var result = new Queue<PathPoly>(running);
+        return result;
+    }
+
+    private float GetTileCost(PathRequest request, PathPoly start, PathPoly end)
+    {
+        var modifier = 1f;
+
+        // TODO
+        if ((end.Data.Flags & PathfindingBreadcrumbFlag.Space) != 0x0)
+        {
+            return 0f;
+        }
+
+        if ((request.CollisionLayer & end.Data.CollisionMask) != 0x0 ||
+            (request.CollisionMask & end.Data.CollisionLayer) != 0x0)
+        {
+            var isDoor = (end.Data.Flags & PathfindingBreadcrumbFlag.Door) != 0x0;
+            var isAccess = (end.Data.Flags & PathfindingBreadcrumbFlag.Access) != 0x0;
+
+            // TODO: Handling power + door prying
+            // Door we should be able to open
+            if (isDoor && !isAccess)
+            {
+                modifier += 0.5f;
+            }
+            // Door we can force open one way or another
+            else if (isDoor && isAccess && (request.Flags & PathFlags.Prying) != 0x0)
+            {
+                modifier += 10f;
+            }
+            else if ((request.Flags & PathFlags.Smashing) != 0x0 && end.Data.Damage > 0f)
+            {
+                modifier += 10f + end.Data.Damage / 100f;
+            }
+            else
+            {
+                return 0f;
+            }
+        }
+
+        return modifier * OctileDistance(end, start);
+    }
+
+    #region Simplifier
+
+    public List<PathPoly> Simplify(List<PathPoly> vertices, float tolerance = 0)
+    {
+        // TODO: Needs more work
+        if (vertices.Count <= 3)
+            return vertices;
+
+        var simplified = new List<PathPoly>();
+
+        for (var i = 0; i < vertices.Count; i++)
+        {
+            // No wraparound for negative sooooo
+            var prev = vertices[i == 0 ? vertices.Count - 1 : i - 1];
+            var current = vertices[i];
+            var next = vertices[(i + 1) % vertices.Count];
+
+            var prevData = prev.Data;
+            var currentData = current.Data;
+            var nextData = next.Data;
+
+            // If they collinear, continue
+            if (i != 0 && i != vertices.Count - 1 &&
+                prevData.Equals(currentData) &&
+                currentData.Equals(nextData) &&
+                IsCollinear(prev, current, next, tolerance))
+            {
+                continue;
+            }
+
+            simplified.Add(current);
+        }
+
+        // Farseer didn't seem to handle straight lines and nuked all points
+        if (simplified.Count == 0)
+        {
+            simplified.Add(vertices[0]);
+            simplified.Add(vertices[^1]);
+        }
+
+        // Check LOS and cut out more nodes
+        // TODO: Grid cast
+        // https://github.com/recastnavigation/recastnavigation/blob/c5cbd53024c8a9d8d097a4371215e3342d2fdc87/Detour/Source/DetourNavMeshQuery.cpp#L2455
+        // Essentially you just do a raycast but a specialised version.
+
+        return simplified;
+    }
+
+    private bool IsCollinear(PathPoly prev, PathPoly current, PathPoly next, float tolerance)
+    {
+        return FloatInRange(Area(prev, current, next), -tolerance, tolerance);
+    }
+
+    private float Area(PathPoly a, PathPoly b, PathPoly c)
+    {
+        var (ax, ay) = a.Box.Center;
+        var (bx, by) = b.Box.Center;
+        var (cx, cy) = c.Box.Center;
+
+        return ax * (by - cy) + bx * (cy - ay) + cx * (ay - by);
+    }
+
+    private bool FloatInRange(float value, float min, float max)
+    {
+        return (value >= min && value <= max);
+    }
+
+    #endregion
+}