Move path assignment to main thread and refactor pathfinder to avoid memory allocations

src/LargeGridPathfinding/LargeGridPathfindingGame.cs

@@ -73,6 +73,7 @@ public class LargeGridPathfindingGame : Game
     private readonly object pendingOperationsLock = new();
     private readonly ProgressTracker progressTracker = new ProgressTracker();
     private readonly List<Agent> agents = [];
+    private readonly ConcurrentQueue<(Agent agent, List<Vector2>? path)> pendingPathResults = new();
     private readonly HashSet<int> affectedZones = [];
     private readonly object affectedZonesLock = new();
     private SpriteBatch spriteBatch = null!;
@@ -147,6 +148,22 @@ public class LargeGridPathfindingGame : Game
 
         if (pathfinder is not null)
         {
+            Trace.WriteLine($"Pending path results: {pendingPathResults.Count}, Affected zones: {affectedZones.Count}, Pending operations: {pendingOperations.Count}");
+
+            // Drain pending path results — background thread enqueues, main thread applies
+            while (pendingPathResults.TryDequeue(out (Agent agent, List<Vector2>? path) result))
+            {
+                Agent agent = result.agent;
+                List<Vector2>? path = result.path;
+                agent.Path = path;
+                if (path?.Count > 0)
+                {
+                    agent.Position = path[0];
+                    agent.NextPosition = path.Count > 1 ? path[1] : path[0];
+                    agent.Destination = new Point((int)path[^1].X, (int)path[^1].Y);
+                }
+            }
+
             // Update agent positions
             foreach (Agent agent in agents)
             {
@@ -238,7 +255,7 @@ public class LargeGridPathfindingGame : Game
                         break;
 
                     case StartupMenuItem.AgentCount:
-                        startupAgentCount = Math.Clamp(startupAgentCount + (direction * 100), 100, 200000);
+                        startupAgentCount = Math.Clamp(startupAgentCount + (direction * 100), 100, 1000000);
                         break;
 
                     case StartupMenuItem.MapPreset:
@@ -1008,7 +1025,7 @@ public class LargeGridPathfindingGame : Game
 
                         ParallelOptions parallelOptions = new ParallelOptions
                         {
-                            MaxDegreeOfParallelism = Environment.ProcessorCount
+                            MaxDegreeOfParallelism = Math.Max(1, Environment.ProcessorCount / 3 * 2)
                         };
 
                         long lastProgressReport = Environment.TickCount64;
@@ -1023,14 +1040,7 @@ public class LargeGridPathfindingGame : Game
                                     List<Vector2>? path = CalculatePath(agent.GridPosition, agent.Destination)
                                         ?? CalculatePath();
 
-                                    agent.Path = path;
+                                    pendingPathResults.Enqueue((agent, path));
-
-                                    if (path?.Count > 0)
-                                    {
-                                        agent.Position = path[0];
-                                        agent.NextPosition = path.Count > 1 ? path[1] : path[0];
-                                        agent.Destination = new Point((int)path[^1].X, (int)path[^1].Y);
-                                    }
                                 }
 
                                 int local = Interlocked.Add(ref pathsCalculated, range.Item2 - range.Item1);
@@ -1050,7 +1060,7 @@ public class LargeGridPathfindingGame : Game
                 }
                 catch (Exception ex)
                 {
-                    Debug.WriteLine(ex);
+                    Trace.WriteLine(ex);
                 }
             }
         }, TaskCreationOptions.LongRunning);

src/LargeGridPathfinding/Pathfinder.cs

@@ -75,55 +75,72 @@ public class Pathfinder
             return null;
         }
 
-        PriorityQueue<(int node, List<int> path, int cost), int> queue = new();
-        HashSet<int> visited = [];
-        queue.Enqueue((start, new List<int> { start }, 0), 0);
-
         if (!rectangleMap.ContainsKey(start) || !rectangleMap.ContainsKey(goal))
         {
             Debug.WriteLine($"{(rectangleMap.ContainsKey(start) ? "Goal" : "Start")} rectangle not found.");
             return null;
         }
 
-        while (queue.Count > 0)
+        PriorityQueue<(int node, int cost), int> openSet = new();
-        {
+        Dictionary<int, int> gScore = [];
-            (int node, List<int> path, int cost) = queue.Dequeue();
+        Dictionary<int, int> cameFrom = [];
 
-            if (node == goal)
+        gScore[start] = 0;
-            {
+        openSet.Enqueue((start, 0), 0);
-                return BuildCoordinatePath(path, startPoint, goalPoint);
-            }
 
-            if (visited.Contains(node))
+        while (openSet.Count > 0)
+        {
+            (int current, int currentCost) = openSet.Dequeue();
+
+            if (gScore.TryGetValue(current, out int bestG) && currentCost != bestG)
             {
                 continue;
             }
 
-            _ = visited.Add(node);
+            if (current == goal)
-
-            foreach (int neighbor in adjacencyList.TryGetValue(node, out List<int>? neighbors) ? neighbors : [])
             {
-                if (!visited.Contains(neighbor) && rectangleMap.ContainsKey(neighbor))
+                List<int> path = [];
+                int node = goal;
+                while (node != start)
                 {
-                    List<int> newPath = [.. path, neighbor];
+                    path.Add(node);
+                    node = cameFrom[node];
+                }
+                path.Add(start);
+                path.Reverse();
+                return BuildCoordinatePath(path, startPoint, goalPoint);
+            }
 
-                    int newCost = cost + GetRectangleWeight(neighbor);
+            foreach (int neighbor in adjacencyList.TryGetValue(current, out List<int>? neighbors) ? neighbors : [])
+            {
+                if (!rectangleMap.ContainsKey(neighbor))
+                {
+                    continue;
+                }
+
+                int tentativeG = currentCost + GetRectangleWeight(neighbor);
 
                 if (penalizeStretchedRectangles)
                 {
                     Rectangle rectangle = rectangleMap[neighbor];
-                        newCost += Math.Max(rectangle.Width, rectangle.Height) / Math.Min(rectangle.Width, rectangle.Height);
+                    tentativeG += Math.Max(rectangle.Width, rectangle.Height) / Math.Min(rectangle.Width, rectangle.Height);
                 }
 
-                    int priority = newCost + GetHeuristicCost(neighbor, goal);
+                if (gScore.TryGetValue(neighbor, out int existingG) && tentativeG >= existingG)
+                {
+                    continue;
+                }
+
+                cameFrom[neighbor] = current;
+                gScore[neighbor] = tentativeG;
+                int priority = tentativeG + GetHeuristicCost(neighbor, goal);
 
                 if (pathRandomization)
                 {
                     priority += Random.Shared.Next(-1, 2);
                 }
 
-                    queue.Enqueue((neighbor, newPath, newCost), priority);
+                openSet.Enqueue((neighbor, tentativeG), priority);
-                }
             }
         }