Add maxNodes limit, path caching, and rectangle weight fixes to pathfinding

src/LargeGridPathfinding/LargeGridPathfindingGame.cs

@@ -257,6 +257,7 @@ public class LargeGridPathfindingGame : Game
 
                                 if (grid[newY, newX] < 0)
                                 {
+                                    agent.Position = currentPos;
                                     agent.Path = null;
                                 }
                             }
@@ -269,7 +270,7 @@ public class LargeGridPathfindingGame : Game
             int totalAgents = agents.Count;
             int offScreenBudget = Math.Max(1000, totalAgents / 1000);
             float catchUpStep = step * (totalAgents / (float)offScreenBudget);
-            int startIndex = (frameCount * offScreenBudget) % totalAgents;
+            int startIndex = frameCount * offScreenBudget % totalAgents;
 
             for (int i = 0; i < offScreenBudget; i++)
             {
@@ -827,10 +828,10 @@ public class LargeGridPathfindingGame : Game
             float viewTop = camera.BoundingRectangle.Top - 10;
             float viewBottom = camera.BoundingRectangle.Bottom + 10;
 
-            int pathMinSX = Math.Max(0, (int)((viewLeft / 10f) / SectorSize) - 1);
+            int pathMinSX = Math.Max(0, (int)(viewLeft / 10f / SectorSize) - 1);
-            int pathMaxSX = Math.Min(sectorsX - 1, (int)((viewRight / 10f) / SectorSize) + 1);
+            int pathMaxSX = Math.Min(sectorsX - 1, (int)(viewRight / 10f / SectorSize) + 1);
-            int pathMinSY = Math.Max(0, (int)((viewTop / 10f) / SectorSize) - 1);
+            int pathMinSY = Math.Max(0, (int)(viewTop / 10f / SectorSize) - 1);
-            int pathMaxSY = Math.Min(sectorsY - 1, (int)((viewBottom / 10f) / SectorSize) + 1);
+            int pathMaxSY = Math.Min(sectorsY - 1, (int)(viewBottom / 10f / SectorSize) + 1);
 
             for (int sx = pathMinSX; sx <= pathMaxSX; sx++)
             {
@@ -890,10 +891,10 @@ public class LargeGridPathfindingGame : Game
         // Draw agents (sector-culled)
         spriteBatch.Begin(SpriteSortMode.Deferred, samplerState: SamplerState.PointClamp, transformMatrix: transformMatrix);
 
-        int drawMinSX = Math.Max(0, (int)(xStart / SectorSize) - 1);
+        int drawMinSX = Math.Max(0, (xStart / SectorSize) - 1);
-        int drawMaxSX = Math.Min(sectorsX - 1, (int)(xEnd / SectorSize) + 1);
+        int drawMaxSX = Math.Min(sectorsX - 1, (xEnd / SectorSize) + 1);
-        int drawMinSY = Math.Max(0, (int)(yStart / SectorSize) - 1);
+        int drawMinSY = Math.Max(0, (yStart / SectorSize) - 1);
-        int drawMaxSY = Math.Min(sectorsY - 1, (int)(yEnd / SectorSize) + 1);
+        int drawMaxSY = Math.Min(sectorsY - 1, (yEnd / SectorSize) + 1);
 
         for (int sx = drawMinSX; sx <= drawMaxSX; sx++)
         {
@@ -1178,13 +1179,14 @@ public class LargeGridPathfindingGame : Game
                     if (agentsRequirePathList.Count != 0)
                     {
                         int agentCount = agentsRequirePathList.Count;
+
                         int batchSize = Math.Max(64, agentCount / (Environment.ProcessorCount * 4));
 
                         ProgressTracker.ProgressData progressDataPaths = progressTracker.AddProgress($"Calculating {agentCount} paths", out IProgress<float> progress);
 
                         ParallelOptions parallelOptions = new ParallelOptions
                         {
-                            MaxDegreeOfParallelism = Math.Max(1, Environment.ProcessorCount / 3 * 2)
+                            MaxDegreeOfParallelism = Environment.ProcessorCount
                         };
 
                         long lastProgressReport = Environment.TickCount64;
@@ -1196,8 +1198,7 @@ public class LargeGridPathfindingGame : Game
                                 {
                                     Agent agent = agentsRequirePathList[i];
 
-                                    List<Vector2>? path = CalculatePath(agent.GridPosition, agent.Destination)
+                                    List<Vector2>? path = CalculatePath(agent.GridPosition, agent.Destination, maxNodes: int.MaxValue) ?? CalculatePath(maxNodes: int.MaxValue);
-                                        ?? CalculatePath();
 
                                     pendingPathResults.Enqueue((agent, path));
                                 }
@@ -1215,8 +1216,11 @@ public class LargeGridPathfindingGame : Game
                         progressTracker.RemoveProgress(progressDataPaths);
                     }
 
+                    if (agentsRequirePathList.Count < 1000)
+                    {
                         await Task.Delay(100);
                     }
+                }
                 catch (Exception ex)
                 {
                     Trace.WriteLine(ex);
@@ -1244,19 +1248,31 @@ public class LargeGridPathfindingGame : Game
         agentSectors = newSectors;
     }
 
-    private List<Vector2>? CalculatePath(Point? start = null, Point? goal = null)
+    private List<Vector2>? CalculatePath(Point? start = null, Point? goal = null, int maxNodes = int.MaxValue)
     {
         int width = gridFiller.Width;
         int height = gridFiller.Height;
-        int startX = Random.Shared.Next(0, width - 1);
+        int[,] g = gridFiller.Grid;
-        int startY = Random.Shared.Next(0, height - 1);
-        int goalX = Random.Shared.Next(0, width - 1);
-        int goalY = Random.Shared.Next(0, height - 1);
 
-        start ??= new Point(startX, startY);
+        if (start is null)
-        goal ??= new Point(goalX, goalY);
+        {
+            do
+            {
+                start = new Point(Random.Shared.Next(0, width - 1), Random.Shared.Next(0, height - 1));
+            }
+            while (g[start.Value.Y, start.Value.X] <= 0);
+        }
 
-        return pathfinder.FindPath(start.Value, goal.Value);
+        if (goal is null)
+        {
+            do
+            {
+                goal = new Point(Random.Shared.Next(0, width - 1), Random.Shared.Next(0, height - 1));
+            }
+            while (g[goal.Value.Y, goal.Value.X] <= 0);
+        }
+
+        return pathfinder.FindPath(start.Value, goal.Value, maxNodes);
     }
 
     private void EnqueuePendingOperations(IEnumerable<Point> points, PendingOperation operation)

src/LargeGridPathfinding/Pathfinder.cs

@@ -19,9 +19,12 @@ public class Pathfinder
     private readonly int[,] weightGrid;
     private readonly bool pathRandomization;
     private readonly bool penalizeStretchedRectangles;
+    private readonly ConcurrentDictionary<(int start, int goal), int[]> pathCache = new();
     private Dictionary<int, Rectangle> rectangleMap;
     private Dictionary<int, List<int>> adjacencyList;
 
+    public bool EnablePathCache { get; set; } = false;
+
     /// <summary>
     /// Returns the current zone adjacency list.
     /// </summary>
@@ -60,7 +63,7 @@ public class Pathfinder
     /// <summary>
     /// Finds a path between two grid points as coordinate waypoints.
     /// </summary>
-    public List<Vector2>? FindPath(Point startPoint, Point goalPoint)
+    public List<Vector2>? FindPath(Point startPoint, Point goalPoint, int maxNodes = int.MaxValue)
     {
         if (!IsInBounds(startPoint.X, startPoint.Y) || !IsInBounds(goalPoint.X, goalPoint.Y))
         {
@@ -81,6 +84,11 @@ public class Pathfinder
             return null;
         }
 
+        if (EnablePathCache && pathCache.TryGetValue((start, goal), out int[]? cachedZonePath))
+        {
+            return BuildCoordinatePath(cachedZonePath, startPoint, goalPoint);
+        }
+
         PriorityQueue<(int node, int cost), int> openSet = new();
         Dictionary<int, int> gScore = [];
         Dictionary<int, int> cameFrom = [];
@@ -88,7 +96,14 @@ public class Pathfinder
         gScore[start] = 0;
         openSet.Enqueue((start, 0), 0);
 
-        while (openSet.Count > 0)
+        if (!rectangleMap.TryGetValue(goal, out Rectangle goalRect))
+        {
+            return null;
+        }
+
+        int nodesVisited = 0;
+
+        while (openSet.Count > 0 && nodesVisited < maxNodes)
         {
             (int current, int currentCost) = openSet.Dequeue();
 
@@ -108,22 +123,31 @@ public class Pathfinder
                 }
                 path.Add(start);
                 path.Reverse();
+
+                // Cache for other agents with same zone pair
+                if (EnablePathCache)
+                {
+                    int[] zonePathArray = [.. path];
+                    _ = pathCache.TryAdd((start, goal), zonePathArray);
+                }
+
                 return BuildCoordinatePath(path, startPoint, goalPoint);
             }
 
+            nodesVisited++;
+
             foreach (int neighbor in adjacencyList.TryGetValue(current, out List<int>? neighbors) ? neighbors : [])
             {
-                if (!rectangleMap.ContainsKey(neighbor))
+                if (!rectangleMap.TryGetValue(neighbor, out Rectangle neighborRect))
                 {
                     continue;
                 }
 
-                int tentativeG = currentCost + GetRectangleWeight(neighbor);
+                int tentativeG = currentCost + Math.Max(1, weightGrid[neighborRect.Y, neighborRect.X]);
 
                 if (penalizeStretchedRectangles)
                 {
-                    Rectangle rectangle = rectangleMap[neighbor];
+                    tentativeG += Math.Max(neighborRect.Width, neighborRect.Height) / Math.Min(neighborRect.Width, neighborRect.Height);
-                    tentativeG += Math.Max(rectangle.Width, rectangle.Height) / Math.Min(rectangle.Width, rectangle.Height);
                 }
 
                 if (gScore.TryGetValue(neighbor, out int existingG) && tentativeG >= existingG)
@@ -133,7 +157,9 @@ public class Pathfinder
 
                 cameFrom[neighbor] = current;
                 gScore[neighbor] = tentativeG;
-                int priority = tentativeG + GetHeuristicCost(neighbor, goal);
+
+                int priority = tentativeG + Math.Abs(neighborRect.Center.X - goalRect.Center.X)
+                             + Math.Abs(neighborRect.Center.Y - goalRect.Center.Y);
 
                 if (pathRandomization)
                 {
@@ -155,6 +181,10 @@ public class Pathfinder
         rectangleMap = new Dictionary<int, Rectangle>(rectanglesSource);
         adjacencyList.Clear();
         adjacencyList = BuildGraphInternal();
+        if (EnablePathCache)
+        {
+            pathCache.Clear();
+        }
     }
 
     private bool IsInBounds(int x, int y)
@@ -361,6 +391,11 @@ public class Pathfinder
             return;
         }
 
+        if (EnablePathCache)
+        {
+            pathCache.Clear();
+        }
+
         // Capture incoming connections to affected zones before mutating adjacency lists.
         Dictionary<int, List<int>> incomingByAffected = [];
         foreach (int zoneId in affectedZones)