improve Pathfinder

Author: LoneWandererProductions

Pathfinder/Node.cs

@@ -92,7 +92,7 @@ internal Node(int x, int y, int g, int h, int movementCost, Node parent = null)
         /// <value>
         ///     The F cost.
         /// </value>
-        private int F => G + H;
+        internal int F => G + H;
 
         /// <inheritdoc />
         /// <summary>

Pathfinder/NodeComparer.cs

@@ -28,12 +28,23 @@ public sealed class NodeComparer : IComparer<Node>
         /// </returns>
         public int Compare(Node? x, Node? y)
         {
-            var result = (x.G + x.H).CompareTo(y.G + y.H);
+            if (ReferenceEquals(x, y)) return 0;
+            if (x is null) return -1;
+            if (y is null) return 1;
+
+            // 1. Total Cost is always the king of A*
+            int result = x.F.CompareTo(y.F);
+
             if (result == 0)
             {
+                // 2. Revert to your logic: Compare G. 
+                // This ensures we respect the diagonal and movement costs 
+                // when the heuristic guess is identical.
                 result = x.G.CompareTo(y.G);
+
                 if (result == 0)
                 {
+                    // 3. Final unique identifier for the SortedSet
                     result = x.X.CompareTo(y.X);
                     if (result == 0)
                     {

Pathfinder/Pathfinding.cs

@@ -69,20 +69,25 @@ public Pathfinding(int[,] grid, int straightCost = 1, int diagonalCost = 2)
         public PathfinderResults FindPath(int startX, int startY, int goalX, int goalY, int rangeLimit)
         {
             var result = new PathfinderResults();
+
+            // We use SortedSet for picking the best node, BUT...
             var openList = new SortedSet<Node>(new NodeComparer());
+
+            // ...we use a Dictionary to find if a coordinate is already in the open list instantly!
+            var openLookUp = new Dictionary<(int, int), Node>();
+
             var closedList = new HashSet<(int, int)>();
-            var nodeIds = new Dictionary<Node, int>();
-            var nodeIdCounter = 0;
 
-            // Initialize start node with its movement cost
             var startNode = new Node(startX, startY, 0, Heuristic(startX, startY, goalX, goalY), _grid[startX, startY]);
+
             openList.Add(startNode);
-            nodeIds[startNode] = nodeIdCounter++;
+            openLookUp[(startX, startY)] = startNode;
 
             while (openList.Count > 0)
             {
                 var currentNode = openList.Min;
-                _ = openList.Remove(currentNode);
+                openList.Remove(currentNode);
+                openLookUp.Remove((currentNode.X, currentNode.Y));
 
                 if (currentNode.X == goalX && currentNode.Y == goalY)
                 {
@@ -95,26 +100,27 @@ public PathfinderResults FindPath(int startX, int startY, int goalX, int goalY,
 
                 foreach (var neighbor in GetNeighbors(currentNode, _grid, goalX, goalY))
                 {
-                    if (closedList.Contains((neighbor.X, neighbor.Y)))
+                    if (closedList.Contains((neighbor.X, neighbor.Y))) continue;
+
+                    // INSTANT LOOKUP - No more FirstOrDefault linear scans!
+                    if (openLookUp.TryGetValue((neighbor.X, neighbor.Y), out var existingOpenNode))
                     {
-                        continue;
-                    }
+                        if (neighbor.G < existingOpenNode.G)
+                        {
+                            // To update a value in a SortedSet, you MUST remove and re-add
+                            // because the sort order has changed.
+                            openList.Remove(existingOpenNode);
 
-                    neighbor.H = Heuristic(neighbor.X, neighbor.Y, goalX, goalY);
-                    neighbor.Parent = currentNode;
+                            existingOpenNode.G = neighbor.G;
+                            existingOpenNode.Parent = currentNode;
 
-                    var openNode = openList.FirstOrDefault(n => n.X == neighbor.X && n.Y == neighbor.Y);
-                    if (openNode == null)
-                    {
-                        openList.Add(neighbor);
-                        nodeIds[neighbor] = nodeIdCounter++;
+                            openList.Add(existingOpenNode);
+                        }
                     }
-                    else if (neighbor.G < openNode.G)
+                    else
                     {
-                        openList.Remove(openNode);
-                        openNode.G = neighbor.G;
-                        openNode.Parent = currentNode;
-                        openList.Add(openNode);
+                        openList.Add(neighbor);
+                        openLookUp[(neighbor.X, neighbor.Y)] = neighbor;
                     }
                 }
             }
@@ -151,11 +157,14 @@ private static List<Node> BuildPath(Node goalNode)
         /// <param name="x2">The x-coordinate of the second point.</param>
         /// <param name="y2">The y-coordinate of the second point.</param>
         /// <returns>The heuristic estimate of the cost.</returns>
-        private static int Heuristic(int x1, int y1, int x2, int y2)
+        private int Heuristic(int x1, int y1, int x2, int y2)
         {
-            return Math.Abs(x1 - x2) + Math.Abs(y1 - y2);
-        }
+            int dx = Math.Abs(x1 - x2);
+            int dy = Math.Abs(y1 - y2);
 
+            // Move diagonally as much as possible, then move straight for the remaining distance
+            return _straightCost * (dx + dy) + (_diagonalCost - 2 * _straightCost) * Math.Min(dx, dy);
+        }
         /// <summary>
         ///     Gets the neighboring nodes for a given node based on the grid and movement costs.
         /// </summary>