optimizations

src/Roboto.Navigation/PathFinder.cs

@@ -1,5 +1,6 @@
 using System.Numerics;
 using Roboto.Core;
+using Serilog;
 
 namespace Roboto.Navigation;
 
@@ -39,12 +40,17 @@ public static class PathFinder
         var openSet = new PriorityQueue<(int x, int y), float>();
         var cameFrom = new Dictionary<(int, int), (int, int)>();
         var gScore = new Dictionary<(int, int), float>();
+        var closedSet = new HashSet<(int, int)>();
+
+        // Track closest-to-goal node for fallback when goal is unreachable
+        (int x, int y) bestNode = startNode;
+        var bestDist = Heuristic(startNode, goalNode);
 
         gScore[startNode] = 0;
         openSet.Enqueue(startNode, Heuristic(startNode, goalNode));
 
         var iterations = 0;
-        const int maxIterations = 50_000;
+        const int maxIterations = 200_000;
 
         while (openSet.Count > 0 && iterations++ < maxIterations)
         {
@@ -53,6 +59,18 @@ public static class PathFinder
             if (current == goalNode)
                 return ReconstructAndSimplify(cameFrom, current, gridToWorld);
 
+            // Skip already-expanded nodes (duplicate PQ entries)
+            if (!closedSet.Add(current))
+                continue;
+
+            // Track nearest reachable cell to goal
+            var distToGoal = Heuristic(current, goalNode);
+            if (distToGoal < bestDist)
+            {
+                bestDist = distToGoal;
+                bestNode = current;
+            }
+
             var currentG = gScore.GetValueOrDefault(current, float.MaxValue);
 
             for (var i = 0; i < 8; i++)
@@ -63,6 +81,9 @@ public static class PathFinder
                 if (nx < 0 || nx >= w || ny < 0 || ny >= h) continue;
                 if (!terrain.IsWalkable(nx, ny)) continue;
 
+                var neighbor = (nx, ny);
+                if (closedSet.Contains(neighbor)) continue;
+
                 // Diagonal corner-cut check
                 if (i >= 4)
                 {
@@ -71,10 +92,9 @@ public static class PathFinder
                         continue;
                 }
 
-                var neighbor = (nx, ny);
                 var stepCost = Cost[i];
                 if (exploredGrid is not null && nx < exploredWidth && ny < exploredHeight && exploredGrid[ny * exploredWidth + nx])
-                    stepCost *= 3f;
+                    stepCost *= 1.5f;
                 var tentativeG = currentG + stepCost;
 
                 if (tentativeG < gScore.GetValueOrDefault(neighbor, float.MaxValue))
@@ -86,6 +106,19 @@ public static class PathFinder
             }
         }
 
+        // No exact path — check if we exhausted the connected region or hit iteration limit
+        var exhausted = openSet.Count == 0;
+        Log.Warning("PATH FAIL detail: expanded={Expanded}, {Reason}, bestDist={Best:F0} from goal",
+            closedSet.Count, exhausted ? "disconnected regions" : "iteration limit", bestDist);
+
+        // Fallback: path to closest reachable cell (only if meaningfully closer than start)
+        if (bestNode != startNode && bestDist < Heuristic(startNode, goalNode) * 0.8f)
+        {
+            Log.Information("PATH FALLBACK: pathing to nearest reachable cell ({X},{Y}), dist={D:F0} from goal",
+                bestNode.x, bestNode.y, bestDist);
+            return ReconstructAndSimplify(cameFrom, bestNode, gridToWorld);
+        }
+
         return null;
     }