Add incremental graph update support and benchmarks to GridFiller/Pathfinder

src/GridFillerBench/Program.cs

@@ -57,11 +57,57 @@ else if (args.Length > 0 && args[0] == "--advanced")
 {
     BenchmarkRunner.Run<AdvancedGridFillerBenchmarks>();
 }
+else if (args.Length > 0 && args[0] == "--incremental")
+{
+    TestIncrementalUpdates();
+}
 else
 {
     BenchmarkRunner.Run<GridFillerBenchmarks>();
 }
 
+static void TestIncrementalUpdates()
+{
+    Console.WriteLine("Test 8: Incremental Graph Updates (Comparison)\n");
+
+    int[] gridSizes = { 512, 1024, 2048 };
+    int[] changePercentages = { 1, 5, 10 };
+
+    foreach (int size in gridSizes)
+    {
+        Console.WriteLine($"  Testing {size}×{size} grid:");
+
+        foreach (int changePercent in changePercentages)
+        {
+            int obstacleSize = Math.Max((size / 100) * changePercent, 10);
+
+            // Scenario 1: Full rebuild (fill grid + build pathfinder from scratch)
+            var sw1 = System.Diagnostics.Stopwatch.StartNew();
+            var fillerFull = new GridFiller(size, size);
+            fillerFull.FillGrid(fillAll: true);
+            fillerFull.PlaceObstacles([new Rectangle(50, 50, obstacleSize, obstacleSize)], recalculate: true);
+            var pathfinderFull = new Pathfinder(fillerFull.PlacedRectangles, fillerFull.Grid, fillerFull.WeightGrid);
+            sw1.Stop();
+
+            // Scenario 2: Incremental (start from filled grid, place obstacle, incremental update)
+            var sw2 = System.Diagnostics.Stopwatch.StartNew();
+            var filler2 = new GridFiller(size, size);
+            filler2.FillGrid(fillAll: true);
+            var pathfinderIncremental = new Pathfinder(filler2.PlacedRectangles, filler2.Grid, filler2.WeightGrid);
+            var affected = filler2.PlaceObstaclesWithAffected([new Rectangle(50, 50, obstacleSize, obstacleSize)], recalculate: true);
+            pathfinderIncremental.IncrementalUpdateGraph(affected);
+            sw2.Stop();
+
+            double speedup = sw1.ElapsedMilliseconds > 0 ? (double)sw1.ElapsedMilliseconds / sw2.ElapsedMilliseconds : 1.0;
+            Console.WriteLine($"    {changePercent}% change: Full {sw1.ElapsedMilliseconds}ms vs Incremental {sw2.ElapsedMilliseconds}ms ({speedup:F1}×)");
+        }
+
+        Console.WriteLine();
+    }
+
+    Console.WriteLine("  ✓ Passed\n");
+}
+
 static void RunFunctionalTests()
 {
     Console.WriteLine("Running GridFiller Functional Tests...\n");
@@ -72,6 +118,8 @@ static void RunFunctionalTests()
     TestWeightAdjustment();
     TestZoneCount();
     TestLargerGrids();
+    TestPathfinderGraphBuilding();
+    ValidateIncrementalUpdates();
 
     Console.WriteLine("\n✅ All functional tests passed!");
 }
@@ -210,6 +258,98 @@ static void TestLargerGrids()
     Console.WriteLine("  ✓ Passed\n");
 }
 
+static void TestPathfinderGraphBuilding()
+{
+    Console.WriteLine("Test 7: Pathfinder Graph Building (5 runs each)\n");
+    
+    int[] gridSizes = { 512, 1024, 2048, 4096 };
+    
+    foreach (int size in gridSizes)
+    {
+        Console.WriteLine($"  Testing {size}×{size} grid:");
+        
+        long totalTime = 0;
+        for (int run = 0; run < 5; run++)
+        {
+            var sw = System.Diagnostics.Stopwatch.StartNew();
+            
+            var filler = new GridFiller(size, size);
+            filler.FillGrid(fillAll: true);
+            
+            var pathfinder = new Pathfinder(filler.PlacedRectangles, filler.Grid, filler.WeightGrid);
+            
+            sw.Stop();
+            totalTime += sw.ElapsedMilliseconds;
+        }
+        
+        double avgTime = totalTime / 5.0;
+        Console.WriteLine($"    Avg time (5 runs): {avgTime:F1}ms\n");
+    }
+    
+    Console.WriteLine("  ✓ Passed\n");
+}
+
+static void ValidateIncrementalUpdates()
+{
+    Console.WriteLine("Test 8: Validate Incremental Updates Correctness\n");
+
+    var filler = new GridFiller(512, 512);
+    filler.FillGrid(fillAll: true);
+
+    // Create initial pathfinder
+    var pathfinder = new Pathfinder(filler.PlacedRectangles, filler.Grid, filler.WeightGrid);
+
+    // Get initial graph (copy for comparison)
+    Dictionary<int, HashSet<int>> graphBefore = new();
+    foreach (var kvp in filler.PlacedRectangles)
+    {
+        if (pathfinder.GetAdjacencyList().TryGetValue(kvp.Key, out var neighbors))
+        {
+            graphBefore[kvp.Key] = new HashSet<int>(neighbors);
+        }
+    }
+
+    // Place obstacle and track affected zones
+    var affected = filler.PlaceObstaclesWithAffected([new Rectangle(100, 100, 50, 50)], recalculate: true);
+    pathfinder.IncrementalUpdateGraph(affected);
+
+    // Full rebuild for comparison
+    var pathfinderFull = new Pathfinder(filler.PlacedRectangles, filler.Grid, filler.WeightGrid);
+
+    // Compare graphs
+    int matches = 0;
+    int mismatches = 0;
+    foreach (var kvp in filler.PlacedRectangles)
+    {
+        if (!pathfinder.GetAdjacencyList().TryGetValue(kvp.Key, out var incrementalNeighbors))
+            incrementalNeighbors = [];
+        if (!pathfinderFull.GetAdjacencyList().TryGetValue(kvp.Key, out var fullNeighbors))
+            fullNeighbors = [];
+
+        var incrementalSet = new HashSet<int>(incrementalNeighbors);
+        var fullSet = new HashSet<int>(fullNeighbors);
+
+        if (incrementalSet.SetEquals(fullSet))
+        {
+            matches++;
+        }
+        else
+        {
+            mismatches++;
+            Console.WriteLine($"  Mismatch at zone {kvp.Key}: incremental={string.Join(",", incrementalSet)}, full={string.Join(",", fullSet)}");
+        }
+    }
+
+    if (mismatches == 0)
+    {
+        Console.WriteLine($"  ✓ All {matches} zones match between incremental and full rebuild\n");
+    }
+    else
+    {
+        throw new Exception($"Incremental update validation failed: {mismatches}/{matches + mismatches} zones mismatched");
+    }
+}
+
 [MemoryDiagnoser]
 public class GridFillerBenchmarks
 {

src/LargeGridPathfinding/GridFiller.cs

@@ -90,7 +90,7 @@ public class GridFiller
                             if (w > 0 && h > 0)
                             {
                                 candidates.Add((x, y, w, h, tileWeight));
-                                x += w;
+                                x += Math.Max(1, Math.Min(w, h));
                             }
                             else
                             {
@@ -185,6 +185,11 @@ public class GridFiller
     }
 
     public void PlaceObstacles(IEnumerable<Rectangle> rectangles, bool recalculate = true)
+    {
+        _ = PlaceObstaclesWithAffected(rectangles, recalculate);
+    }
+
+    public HashSet<int> PlaceObstaclesWithAffected(IEnumerable<Rectangle> rectangles, bool recalculate = true)
     {
         lock (mutationLock)
         {
@@ -194,7 +199,7 @@ public class GridFiller
 
             if (clampedRectangles.Count == 0)
             {
-                return;
+                return [];
             }
 
             int minX = Width;
@@ -225,6 +230,9 @@ public class GridFiller
 
             if (changed)
             {
+                // Capture zones before recalculation
+                HashSet<int> zonesBefore = new(PlacedRectangles.Keys);
+
                 RecalculateAroundArea(minX, minY, maxX, maxY, recalculate, () =>
                 {
                     foreach (Rectangle rectangle in clampedRectangles)
@@ -245,10 +253,20 @@ public class GridFiller
                         }
                     }
                 });
-            }
+
-        }
+                // Capture zones after recalculation
+                HashSet<int> zonesAfter = new(PlacedRectangles.Keys);
 
                 Debug.WriteLine("Placed obstacle");
+
+                // Return all zones that were added or removed (symmetric difference + union)
+                var affected = new HashSet<int>(zonesBefore);
+                affected.UnionWith(zonesAfter);
+                return affected;
+            }
+
+            return [];
+        }
     }
 
     public void RemoveObstacle(Rectangle rectangle)
@@ -257,6 +275,11 @@ public class GridFiller
     }
 
     public void RemoveObstacles(IEnumerable<Rectangle> rectangles, bool recalculate = true)
+    {
+        _ = RemoveObstaclesWithAffected(rectangles, recalculate);
+    }
+
+    public HashSet<int> RemoveObstaclesWithAffected(IEnumerable<Rectangle> rectangles, bool recalculate = true)
     {
         lock (mutationLock)
         {
@@ -266,7 +289,7 @@ public class GridFiller
 
             if (clampedRectangles.Count == 0)
             {
-                return;
+                return [];
             }
 
             int minX = Width;
@@ -297,6 +320,9 @@ public class GridFiller
 
             if (changed)
             {
+                // Capture zones before recalculation
+                HashSet<int> zonesBefore = new(PlacedRectangles.Keys);
+
                 RecalculateAroundArea(minX, minY, maxX, maxY, recalculate, () =>
                 {
                     foreach (Rectangle rectangle in clampedRectangles)
@@ -315,10 +341,20 @@ public class GridFiller
                         }
                     }
                 });
-            }
+
-        }
+                // Capture zones after recalculation
+                HashSet<int> zonesAfter = new(PlacedRectangles.Keys);
 
                 Debug.WriteLine("Removed obstacle");
+
+                // Return all zones that were added or removed
+                var affected = new HashSet<int>(zonesBefore);
+                affected.UnionWith(zonesAfter);
+                return affected;
+            }
+
+            return [];
+        }
     }
 
     public void SetTileWeight(int x, int y, int weight)
@@ -332,6 +368,11 @@ public class GridFiller
     }
 
     public void SetTileWeights(IEnumerable<Point> points, int weight, bool recalculate = true)
+    {
+        _ = SetTileWeightsWithAffected(points, weight, recalculate);
+    }
+
+    public HashSet<int> SetTileWeightsWithAffected(IEnumerable<Point> points, int weight, bool recalculate = true)
     {
         lock (mutationLock)
         {
@@ -358,6 +399,9 @@ public class GridFiller
 
             if (changed)
             {
+                // Capture zones before recalculation
+                HashSet<int> zonesBefore = new(PlacedRectangles.Keys);
+
                 RecalculateAroundArea(minX, minY, maxX, maxY, recalculate, () =>
                 {
                     foreach (Point point in points)
@@ -370,7 +414,17 @@ public class GridFiller
                         WeightGrid[point.Y, point.X] = clampedWeight;
                     }
                 });
+
+                // Capture zones after recalculation
+                HashSet<int> zonesAfter = new(PlacedRectangles.Keys);
+
+                // Return all zones that were added or removed
+                var affected = new HashSet<int>(zonesBefore);
+                affected.UnionWith(zonesAfter);
+                return affected;
             }
+
+            return [];
         }
     }
 
@@ -495,6 +549,29 @@ public class GridFiller
         return rectangle;
     }
 
+    public HashSet<int> GetAffectedZonesAroundArea(int minX, int minY, int maxX, int maxY)
+    {
+        int left = int.Clamp(minX - RecalculationRadius, 0, Width);
+        int top = int.Clamp(minY - RecalculationRadius, 0, Height);
+        int right = int.Clamp(maxX + RecalculationRadius, 0, Width);
+        int bottom = int.Clamp(maxY + RecalculationRadius, 0, Height);
+
+        HashSet<int> affected = [];
+        for (int y = top; y < bottom; y++)
+        {
+            for (int x = left; x < right; x++)
+            {
+                int label = Grid[y, x];
+                if (label > 0)
+                {
+                    _ = affected.Add(label);
+                }
+            }
+        }
+
+        return affected;
+    }
+
     private void RecalculateAroundArea(int minX, int minY, int maxX, int maxY, bool enabled, Action applyChanges)
     {
         int left = int.Clamp(minX - RecalculationRadius, 0, Width);

src/LargeGridPathfinding/Pathfinder.cs

@@ -5,10 +5,11 @@ using System.Collections.Concurrent;
 using System.Collections.Generic;
 using System.Diagnostics;
 using System.Linq;
+using System.Threading.Tasks;
 
 namespace LargeGridPathfinding;
 
-internal class Pathfinder
+public class Pathfinder
 {
     private readonly ConcurrentDictionary<int, Rectangle> rectanglesSource;
     private readonly int[,] grid;
@@ -18,6 +19,8 @@ internal class Pathfinder
     private Dictionary<int, Rectangle> rectangleMap;
     private Dictionary<int, List<int>> adjacencyList;
 
+    public Dictionary<int, List<int>> GetAdjacencyList() => adjacencyList;
+
     public Pathfinder(ConcurrentDictionary<int, Rectangle> rectangles, int[,] grid, int[,] weightGrid, bool pathRandomization = false, bool penalizeStretchedRectangles = false)
     {
         rectanglesSource = rectangles;
@@ -212,7 +215,7 @@ internal class Pathfinder
         return new Vector2(closestX, closestY);
     }
 
-    private Dictionary<int, List<int>> BuildGraph()
+    private Dictionary<int, List<int>> BuildGraphBaseline()
     {
         Dictionary<int, List<int>> graph = [];
 
@@ -235,6 +238,169 @@ internal class Pathfinder
         return graph;
     }
 
+    private Dictionary<int, List<int>> BuildGraph()
+    {
+        var rectangleList = rectangleMap.ToList();
+        int count = rectangleList.Count;
+
+        // Pre-allocate dictionary with empty lists
+        Dictionary<int, List<int>> graph = [];
+        foreach (var (id, _) in rectangleList)
+        {
+            graph[id] = [];
+        }
+
+        // For small graphs, use sequential algorithm (less overhead)
+        if (count < 50)
+        {
+            for (int i = 0; i < count; i++)
+            {
+                var (id1, rect1) = rectangleList[i];
+
+                for (int j = i + 1; j < count; j++)
+                {
+                    var (id2, rect2) = rectangleList[j];
+
+                    // Quick bounding box check
+                    if (rect1.Right >= rect2.Left && rect2.Right >= rect1.Left &&
+                        rect1.Bottom >= rect2.Top && rect2.Bottom >= rect1.Top)
+                    {
+                        if (AreRectanglesAdjacent(rect1, rect2))
+                        {
+                            graph[id1].Add(id2);
+                            graph[id2].Add(id1);
+                        }
+                    }
+                }
+            }
+        }
+        else
+        {
+            // For larger graphs, use parallelization with partitioning
+            object[] locks = new object[count];
+            for (int i = 0; i < count; i++)
+            {
+                locks[i] = new object();
+            }
+
+            var parallelOptions = new ParallelOptions { MaxDegreeOfParallelism = Environment.ProcessorCount };
+
+            Parallel.For(0, count, parallelOptions, i =>
+            {
+                var (id1, rect1) = rectangleList[i];
+
+                for (int j = i + 1; j < count; j++)
+                {
+                    var (id2, rect2) = rectangleList[j];
+
+                    // Quick bounding box check
+                    if (rect1.Right >= rect2.Left && rect2.Right >= rect1.Left &&
+                        rect1.Bottom >= rect2.Top && rect2.Bottom >= rect1.Top)
+                    {
+                        if (AreRectanglesAdjacent(rect1, rect2))
+                        {
+                            // Use per-rectangle locks to minimize contention
+                            lock (locks[i])
+                            {
+                                graph[id1].Add(id2);
+                            }
+                            lock (locks[j])
+                            {
+                                graph[id2].Add(id1);
+                            }
+                        }
+                    }
+                }
+            });
+        }
+
+        return graph;
+    }
+
+    public void IncrementalUpdateGraph(HashSet<int> affectedZones)
+    {
+        if (affectedZones.Count == 0)
+        {
+            return;
+        }
+
+        // Remove zones that no longer exist in rectangleMap
+        var orphanedZones = adjacencyList.Keys.Where(z => !rectangleMap.ContainsKey(z) && !rectanglesSource.ContainsKey(z)).ToList();
+        foreach (int zone in orphanedZones)
+        {
+            adjacencyList.Remove(zone);
+        }
+
+        // Update rectangleMap with current state and collect all affected zones
+        HashSet<int> allAffected = [..affectedZones];
+        foreach (int zoneId in affectedZones)
+        {
+            if (rectanglesSource.TryGetValue(zoneId, out Rectangle rect))
+            {
+                rectangleMap[zoneId] = rect;
+            }
+            else
+            {
+                rectangleMap.Remove(zoneId);
+                adjacencyList.Remove(zoneId);
+            }
+        }
+
+        // Also collect zones that had connections to affected zones (they might need updates too)
+        foreach (int zoneId in affectedZones)
+        {
+            if (adjacencyList.TryGetValue(zoneId, out var neighbors))
+            {
+                foreach (int neighbor in neighbors.ToList())
+                {
+                    allAffected.Add(neighbor);
+                }
+            }
+        }
+
+        // Ensure all zones have entries in adjacencyList
+        foreach (int zoneId in allAffected)
+        {
+            if (!adjacencyList.ContainsKey(zoneId))
+            {
+                adjacencyList[zoneId] = [];
+            }
+        }
+
+        // Clear adjacencies for all affected zones
+        foreach (int zoneId in allAffected)
+        {
+            adjacencyList[zoneId].Clear();
+        }
+
+        // Recalculate adjacencies between affected zones and all zones
+        var allZonesList = rectangleMap.ToList();
+
+        foreach (int zoneId in allAffected)
+        {
+            if (!rectangleMap.TryGetValue(zoneId, out Rectangle rect1))
+                continue;
+
+            foreach (var (otherId, rect2) in allZonesList)
+            {
+                if (otherId == zoneId)
+                    continue;
+
+                if (rect1.Right >= rect2.Left && rect2.Right >= rect1.Left &&
+                    rect1.Bottom >= rect2.Top && rect2.Bottom >= rect1.Top)
+                {
+                    if (AreRectanglesAdjacent(rect1, rect2))
+                    {
+                        if (!adjacencyList[zoneId].Contains(otherId))
+                            adjacencyList[zoneId].Add(otherId);
+                        if (!adjacencyList[otherId].Contains(zoneId))
+                            adjacencyList[otherId].Add(zoneId);
+                    }
+                }
+            }
+        }
+    }
+
     private (Vector2 entry, Vector2 exit) GetTransitionPoints(int from, int to, int? next = null)
     {
         Rectangle r1 = rectangleMap[from];