Support weighted path finding

src/LargeGridPathfinding/GridFiller.cs

@@ -14,6 +14,7 @@ internal class GridFiller
     private int currentLabel = 1;
     private int currentObstacleLabel = -1;
     public int[,] Grid { get; }
+    public int[,] WeightGrid { get; }
     public ConcurrentDictionary<int, Rectangle> PlacedRectangles { get; }
     public int Width { get; }
     public int Height { get; }
@@ -23,8 +24,17 @@ internal class GridFiller
         Width = width;
         Height = height;
         Grid = new int[height, width];
+        WeightGrid = new int[height, width];
         PlacedRectangles = [];
 
+        for (int y = 0; y < height; y++)
+        {
+            for (int x = 0; x < width; x++)
+            {
+                WeightGrid[y, x] = 1;
+            }
+        }
+
         foreach (Rectangle rectangle in obstacles)
         {
             PlaceObstacle(rectangle);
@@ -54,7 +64,7 @@ internal class GridFiller
         }
 
         // Collect all valid rectangle placements
-        ConcurrentBag<(int x, int y, int w, int h)> candidates = [];
+        ConcurrentBag<(int x, int y, int w, int h, int weight)> candidates = [];
 
         int cells = Grid.GetLength(0) * Grid.GetLength(1);
         int areaPlaced = 0;
@@ -74,11 +84,12 @@ internal class GridFiller
                 {
                     if (Grid[y, x] == 0)
                     {
-                        (int w, int h) = GetMaxRectangleSize(x, y, maxRectangleSize, maxRectangleSize);
+                        int tileWeight = WeightGrid[y, x];
+                        (int w, int h) = GetMaxRectangleSize(x, y, maxRectangleSize, maxRectangleSize, tileWeight);
 
                         if (w > 0 && h > 0)
                         {
-                            candidates.Add((x, y, w, h));
+                            candidates.Add((x, y, w, h, tileWeight));
                         }
 
                         if (w >= maxRectangleSize && h >= maxRectangleSize)
@@ -99,9 +110,26 @@ internal class GridFiller
             Debug.WriteLine($"Candidates: {candidates.Count}");
             Debug.WriteLine("Sorting candidates...");
 
-            // Sort by area (largest first)
-            List<(int x, int y, int w, int h)> sortedCandidates = [.. candidates];
-            sortedCandidates.Sort((a, b) => (b.w * b.h).CompareTo(a.w * a.h));
+            // Sort by area (largest first) and prefer less stretched rectangles on ties
+            List<(int x, int y, int w, int h, int weight)> sortedCandidates = [.. candidates];
+            sortedCandidates.Sort((a, b) =>
+            {
+                int areaComparison = (b.w * b.h).CompareTo(a.w * a.h);
+                if (areaComparison != 0)
+                {
+                    return areaComparison;
+                }
+
+                int stretchA = GetStretchFactor(a.w, a.h);
+                int stretchB = GetStretchFactor(b.w, b.h);
+                int stretchComparison = stretchA.CompareTo(stretchB);
+                if (stretchComparison != 0)
+                {
+                    return stretchComparison;
+                }
+
+                return b.h.CompareTo(a.h);
+            });
 
             Debug.WriteLine("Placing rectangles...");
 
@@ -109,10 +137,10 @@ internal class GridFiller
             int reportIntervalPlacing = Math.Max(sortedCandidates.Count / 10, 1);
 
             // Place rectangles, ensuring no overlap
-            foreach ((int x, int y, int w, int h) in sortedCandidates)
+            foreach ((int x, int y, int w, int h, int weight) in sortedCandidates)
             {
                 // Check if area is still free
-                if (IsAreaFree(x, y, w, h))
+                if (IsAreaFree(x, y, w, h, weight))
                 {
                     Rectangle rectangle = new Rectangle(x, y, w, h);
                     PlaceRectangle(rectangle);
@@ -162,6 +190,7 @@ internal class GridFiller
                 if (Grid[dy, dx] >= 0 && dy < rectangle.Y + rectangle.Height && dx < rectangle.X + rectangle.Width && dy >= rectangle.Y && dx >= rectangle.X)
                 {
                     Grid[dy, dx] = obstacle;
+                    WeightGrid[dy, dx] = 1;
                 }
             }
         }
@@ -206,6 +235,7 @@ internal class GridFiller
                 if (Grid[dy, dx] < 0 && dy < rectangle.Y + rectangle.Height && dx < rectangle.X + rectangle.Width && dy >= rectangle.Y && dx >= rectangle.X)
                 {
                     Grid[dy, dx] = 0;
+                    WeightGrid[dy, dx] = 1;
                 }
             }
         }
@@ -222,14 +252,91 @@ internal class GridFiller
         Debug.WriteLine($"minX: {minStartX}, minY: {minStartY}, maxX: {maxEndX}, maxY: {maxEndY}");
     }
 
+    public void SetTileWeight(int x, int y, int weight)
+    {
+        SetTileWeights([new Point(x, y)], weight);
+    }
+
+    public void ResetTileWeight(int x, int y)
+    {
+        ResetTileWeights([new Point(x, y)]);
+    }
+
+    public void SetTileWeights(IEnumerable<Point> points, int weight)
+    {
+        int clampedWeight = Math.Max(1, weight);
+        HashSet<Point> changedPoints = [];
+
+        int minX = Width;
+        int minY = Height;
+        int maxX = -1;
+        int maxY = -1;
+
+        foreach (Point point in points)
+        {
+            if (point.X < 0 || point.Y < 0 || point.X >= Width || point.Y >= Height || Grid[point.Y, point.X] < 0 || WeightGrid[point.Y, point.X] == clampedWeight)
+            {
+                continue;
+            }
+
+            _ = changedPoints.Add(point);
+            minX = Math.Min(minX, point.X);
+            minY = Math.Min(minY, point.Y);
+            maxX = Math.Max(maxX, point.X);
+            maxY = Math.Max(maxY, point.Y);
+        }
+
+        if (changedPoints.Count == 0)
+        {
+            return;
+        }
+
+        int x = int.Clamp(minX - 1, 0, Width);
+        int y = int.Clamp(minY - 1, 0, Height);
+        int w = int.Clamp(maxX + 2, 0, Width);
+        int h = int.Clamp(maxY + 2, 0, Height);
+
+        List<Rectangle> removedRectangles = [new Rectangle(x, y, w - x, h - y)];
+        HashSet<int> removedLabels = [];
+
+        for (int dy = y; dy < h; dy++)
+        {
+            for (int dx = x; dx < w; dx++)
+            {
+                int label = Grid[dy, dx];
+                if (label > 0 && removedLabels.Add(label))
+                {
+                    removedRectangles.Add(RemoveRectangle(label));
+                }
+            }
+        }
+
+        foreach (Point point in changedPoints)
+        {
+            WeightGrid[point.Y, point.X] = clampedWeight;
+        }
+
+        int minStartX = removedRectangles.Min(r => r.X);
+        int minStartY = removedRectangles.Min(r => r.Y);
+        int maxEndX = removedRectangles.Max(r => r.X + r.Width);
+        int maxEndY = removedRectangles.Max(r => r.Y + r.Height);
+
+        FillGrid(minStartX, minStartY, maxEndX, maxEndY);
+    }
+
+    public void ResetTileWeights(IEnumerable<Point> points)
+    {
+        SetTileWeights(points, 1);
+    }
+
     // Helper method to check if a rectangle can still be placed
-    private bool IsAreaFree(int x, int y, int w, int h)
+    private bool IsAreaFree(int x, int y, int w, int h, int requiredWeight)
     {
         for (int dy = 0; dy < h; dy++)
         {
             for (int dx = 0; dx < w; dx++)
             {
-                if (Grid[y + dy, x + dx] != 0) // Not empty
+                if (Grid[y + dy, x + dx] != 0 || WeightGrid[y + dy, x + dx] != requiredWeight) // Not empty or mismatched weight
                 {
                     return false;
                 }
@@ -238,28 +345,54 @@ internal class GridFiller
         return true;
     }
 
-    private (int, int) GetMaxRectangleSize(int startX, int startY, int limitW, int limitH)
+    private (int, int) GetMaxRectangleSize(int startX, int startY, int limitW, int limitH, int requiredWeight)
     {
-        int maxWidth = 0, maxHeight = 0;
-
-        for (int x = startX; x < Width && maxWidth < limitW && Grid[startY, x] == 0; x++)
+        int maxHeight = Math.Min(Height - startY, limitH);
+        if (maxHeight <= 0)
         {
-            maxWidth++;
+            return (0, 0);
         }
 
-        for (int h = 1; h <= Math.Min(Height - startY, limitH); h++)
+        int initialWidth = GetRowContinuousWidth(startX, startY, limitW, requiredWeight);
+        if (initialWidth <= 0)
         {
-            for (int x = startX; x < startX + maxWidth; x++)
+            return (0, 0);
+        }
+
+        int currentWidth = initialWidth;
+        int bestWidth = 0;
+        int bestHeight = 0;
+        int bestArea = 0;
+        int bestStretch = int.MaxValue;
+
+        for (int h = 1; h <= maxHeight; h++)
+        {
+            int row = startY + h - 1;
+            int rowWidth = GetRowContinuousWidth(startX, row, currentWidth, requiredWeight);
+            currentWidth = Math.Min(currentWidth, rowWidth);
+
+            if (currentWidth <= 0)
             {
-                if (Grid[startY + h - 1, x] != 0)
-                {
-                    return (maxWidth, h - 1);
-                }
+                break;
+            }
+
+            int area = currentWidth * h;
+            int stretch = GetStretchFactor(currentWidth, h);
+
+            bool isBetterArea = area > bestArea;
+            bool isEqualAreaButBetterShape = area == bestArea && stretch < bestStretch;
+            bool isNearBestAreaMuchBetterShape = bestArea > 0 && area * 10 >= bestArea * 9 && stretch + 2 < bestStretch;
+
+            if (isBetterArea || isEqualAreaButBetterShape || isNearBestAreaMuchBetterShape)
+            {
+                bestArea = area;
+                bestWidth = currentWidth;
+                bestHeight = h;
+                bestStretch = stretch;
             }
-            maxHeight = h;
         }
 
-        return (maxWidth, maxHeight);
+        return (bestWidth, bestHeight);
     }
 
     private void PlaceRectangle(Rectangle rectangle)
@@ -294,4 +427,35 @@ internal class GridFiller
 
         return rectangle;
     }
-}
+
+    private int GetRowContinuousWidth(int startX, int y, int maxWidth, int requiredWeight)
+    {
+        int width = 0;
+        int maxX = Math.Min(Width, startX + maxWidth);
+
+        for (int x = startX; x < maxX; x++)
+        {
+            if (Grid[y, x] != 0 || WeightGrid[y, x] != requiredWeight)
+            {
+                break;
+            }
+
+            width++;
+        }
+
+        return width;
+    }
+
+    private static int GetStretchFactor(int width, int height)
+    {
+        if (width <= 0 || height <= 0)
+        {
+            return int.MaxValue;
+        }
+
+        int minSide = Math.Min(width, height);
+        int maxSide = Math.Max(width, height);
+        return maxSide / minSide;
+    }
+
+}

src/LargeGridPathfinding/LargeGridPathfindingGame.cs

@@ -17,6 +17,12 @@ namespace LargeGridPathfinding;
 
 public class LargeGridPathfindingGame : Game
 {
+    private enum BrushMode
+    {
+        Weight,
+        Obstacle
+    }
+
     private readonly ConcurrentDictionary<Vector2, Color> temporaryIndicators = [];
     private readonly ProgressTracker progressTracker = new ProgressTracker();
     private readonly List<Agent> agents = [];
@@ -31,6 +37,8 @@ public class LargeGridPathfindingGame : Game
     private bool showZones = true;
     private bool showGrid = false;
     private bool showPaths = false;
+    private BrushMode brushMode = BrushMode.Weight;
+    private int paintWeight = 5;
     private Vector2? previousMousePosition;
 
     public LargeGridPathfindingGame()
@@ -164,6 +172,48 @@ public class LargeGridPathfindingGame : Game
             showPaths = !showPaths;
         }
 
+        if (keyboardState.WasKeyPressed(Keys.D1) || keyboardState.WasKeyPressed(Keys.NumPad1))
+        {
+            paintWeight = 1;
+        }
+        else if (keyboardState.WasKeyPressed(Keys.D2) || keyboardState.WasKeyPressed(Keys.NumPad2))
+        {
+            paintWeight = 2;
+        }
+        else if (keyboardState.WasKeyPressed(Keys.D3) || keyboardState.WasKeyPressed(Keys.NumPad3))
+        {
+            paintWeight = 3;
+        }
+        else if (keyboardState.WasKeyPressed(Keys.D4) || keyboardState.WasKeyPressed(Keys.NumPad4))
+        {
+            paintWeight = 4;
+        }
+        else if (keyboardState.WasKeyPressed(Keys.D5) || keyboardState.WasKeyPressed(Keys.NumPad5))
+        {
+            paintWeight = 5;
+        }
+        else if (keyboardState.WasKeyPressed(Keys.D6) || keyboardState.WasKeyPressed(Keys.NumPad6))
+        {
+            paintWeight = 6;
+        }
+        else if (keyboardState.WasKeyPressed(Keys.D7) || keyboardState.WasKeyPressed(Keys.NumPad7))
+        {
+            paintWeight = 7;
+        }
+        else if (keyboardState.WasKeyPressed(Keys.D8) || keyboardState.WasKeyPressed(Keys.NumPad8))
+        {
+            paintWeight = 8;
+        }
+        else if (keyboardState.WasKeyPressed(Keys.D9) || keyboardState.WasKeyPressed(Keys.NumPad9))
+        {
+            paintWeight = 9;
+        }
+
+        if (keyboardState.WasKeyPressed(Keys.O))
+        {
+            brushMode = brushMode == BrushMode.Weight ? BrushMode.Obstacle : BrushMode.Weight;
+        }
+
         if (keyboardState.WasKeyPressed(Keys.F))
         {
             Debug.WriteLine("Filling grid...");
@@ -191,14 +241,20 @@ public class LargeGridPathfindingGame : Game
             });
         }
 
-        // Place or remove obstacles via mouse input
+        // Paint and clear tiles via mouse input
 
         if (mouseState.IsButtonDown(MouseButton.Left))
         {
             Vector2 mousePosition = camera.ScreenToWorld(mouseState.Position.ToVector2());
             Vector2 gridPosition = new Vector2((int)mousePosition.X / 10, (int)mousePosition.Y / 10);
 
-            if (gridPosition.X < 0 || gridPosition.X >= gridFiller.Width || gridPosition.Y < 0 || gridPosition.Y >= gridFiller.Height || gridFiller.Grid[(int)gridPosition.Y, (int)gridPosition.X] < 0 || temporaryIndicators.ContainsKey(gridPosition))
+            if (gridPosition.X < 0 || gridPosition.X >= gridFiller.Width || gridPosition.Y < 0 || gridPosition.Y >= gridFiller.Height || temporaryIndicators.ContainsKey(gridPosition))
+            {
+                return;
+            }
+
+            int cellValue = gridFiller.Grid[(int)gridPosition.Y, (int)gridPosition.X];
+            if ((brushMode == BrushMode.Weight && cellValue < 0) || (brushMode == BrushMode.Obstacle && cellValue < 0))
             {
                 return;
             }
@@ -209,37 +265,41 @@ public class LargeGridPathfindingGame : Game
                 previousGridPosition = new Vector2((int)previousMousePosition.Value.X / 10, (int)previousMousePosition.Value.Y / 10);
             }
 
-            // Place temporary indicators to show the obstacle is scheduled for placement
-            temporaryIndicators[gridPosition] = Color.Red;
+            Color indicatorColor = brushMode == BrushMode.Weight ? Color.Orange : Color.Red;
+            BrushMode currentBrushMode = brushMode;
+            int currentPaintWeight = paintWeight;
+            temporaryIndicators[gridPosition] = indicatorColor;
 
             previousMousePosition = mousePosition;
 
             _ = Task.Run(() =>
             {
-                if (previousGridPosition is not null)
+                List<Point> brushPoints = GetBrushPoints(gridPosition, previousGridPosition);
+                foreach (Point point in brushPoints)
                 {
-                    List<Vector2> interpolatedPositions = [];
+                    temporaryIndicators[point.ToVector2()] = indicatorColor;
+                }
 
-                    // Interpolate between previous and current mouse position to place obstacles in a straight line to prevent gaps
-                    float distance = Vector2.Distance(previousGridPosition.Value, gridPosition);
-                    float lerpSteps = distance * 10;
-
-                    for (float t = 0; t <= 1; t += 1 / lerpSteps)
+                if (currentBrushMode == BrushMode.Weight)
+                {
+                    gridFiller.SetTileWeights(brushPoints, currentPaintWeight);
+                }
+                else
+                {
+                    foreach (Point point in brushPoints)
                     {
-                        Vector2 interpolatedPosition = Vector2.Lerp(previousGridPosition.Value, gridPosition, t);
-                        interpolatedPosition.Round();
-                        interpolatedPositions.Add(interpolatedPosition);
-                        temporaryIndicators[interpolatedPosition] = Color.Red;
-                    }
-
-                    foreach (Vector2 interpolatedPosition in interpolatedPositions.Distinct())
-                    {
-                        gridFiller.PlaceObstacle(new Rectangle((int)interpolatedPosition.X, (int)interpolatedPosition.Y, 1, 1));
-                        _ = temporaryIndicators.TryRemove(interpolatedPosition, out _);
+                        if (point.X >= 0 && point.Y >= 0 && point.X < gridFiller.Width && point.Y < gridFiller.Height && gridFiller.Grid[point.Y, point.X] >= 0)
+                        {
+                            gridFiller.PlaceObstacle(new Rectangle(point.X, point.Y, 1, 1));
+                        }
                     }
                 }
 
-                gridFiller.PlaceObstacle(new Rectangle((int)gridPosition.X, (int)gridPosition.Y, 1, 1));
+                foreach (Point point in brushPoints)
+                {
+                    _ = temporaryIndicators.TryRemove(point.ToVector2(), out _);
+                }
+
                 gridChanged = true;
                 _ = temporaryIndicators.TryRemove(gridPosition, out _);
             });
@@ -249,7 +309,13 @@ public class LargeGridPathfindingGame : Game
             Vector2 mousePosition = camera.ScreenToWorld(mouseState.Position.ToVector2());
             Vector2 gridPosition = new Vector2((int)mousePosition.X / 10, (int)mousePosition.Y / 10);
 
-            if (gridPosition.X < 0 || gridPosition.X >= gridFiller.Width || gridPosition.Y < 0 || gridPosition.Y >= gridFiller.Height || gridFiller.Grid[(int)gridPosition.Y, (int)gridPosition.X] > 0 || temporaryIndicators.ContainsKey(gridPosition))
+            if (gridPosition.X < 0 || gridPosition.X >= gridFiller.Width || gridPosition.Y < 0 || gridPosition.Y >= gridFiller.Height || temporaryIndicators.ContainsKey(gridPosition))
+            {
+                return;
+            }
+
+            int cellValue = gridFiller.Grid[(int)gridPosition.Y, (int)gridPosition.X];
+            if ((brushMode == BrushMode.Weight && cellValue < 0) || (brushMode == BrushMode.Obstacle && cellValue >= 0))
             {
                 return;
             }
@@ -260,38 +326,40 @@ public class LargeGridPathfindingGame : Game
                 previousGridPosition = new Vector2((int)previousMousePosition.Value.X / 10, (int)previousMousePosition.Value.Y / 10);
             }
 
-            // Place temporary indicators to show the obstacle is scheduled for removal
-            temporaryIndicators[gridPosition] = Color.Yellow;
+            Color indicatorColor = brushMode == BrushMode.Weight ? Color.LightGray : Color.Yellow;
+            BrushMode currentBrushMode = brushMode;
+            temporaryIndicators[gridPosition] = indicatorColor;
 
             previousMousePosition = mousePosition;
 
             _ = Task.Run(() =>
             {
-                if (previousGridPosition is not null)
+                List<Point> brushPoints = GetBrushPoints(gridPosition, previousGridPosition);
+                foreach (Point point in brushPoints)
                 {
-                    List<Vector2> interpolatedPositions = [];
+                    temporaryIndicators[point.ToVector2()] = indicatorColor;
+                }
 
-                    // Interpolate between previous and current mouse position to place obstacles in a straight line to prevent gaps
-
-                    float distance = Vector2.Distance(previousGridPosition.Value, gridPosition);
-                    float lerpSteps = distance * 10;
-
-                    for (float t = 0; t <= 1; t += 1 / lerpSteps)
+                if (currentBrushMode == BrushMode.Weight)
+                {
+                    gridFiller.ResetTileWeights(brushPoints);
+                }
+                else
+                {
+                    foreach (Point point in brushPoints)
                     {
-                        Vector2 interpolatedPosition = Vector2.Lerp(previousGridPosition.Value, gridPosition, t);
-                        interpolatedPosition.Round();
-                        interpolatedPositions.Add(interpolatedPosition);
-                        temporaryIndicators[interpolatedPosition] = Color.Yellow;
-                    }
-
-                    foreach (Vector2 interpolatedPosition in interpolatedPositions.Distinct())
-                    {
-                        gridFiller.RemoveObstacle(new Rectangle((int)interpolatedPosition.X, (int)interpolatedPosition.Y, 1, 1));
-                        _ = temporaryIndicators.TryRemove(interpolatedPosition, out _);
+                        if (point.X >= 0 && point.Y >= 0 && point.X < gridFiller.Width && point.Y < gridFiller.Height && gridFiller.Grid[point.Y, point.X] < 0)
+                        {
+                            gridFiller.RemoveObstacle(new Rectangle(point.X, point.Y, 1, 1));
+                        }
                     }
                 }
 
-                gridFiller.RemoveObstacle(new Rectangle((int)gridPosition.X, (int)gridPosition.Y, 1, 1));
+                foreach (Point point in brushPoints)
+                {
+                    _ = temporaryIndicators.TryRemove(point.ToVector2(), out _);
+                }
+
                 gridChanged = true;
                 _ = temporaryIndicators.TryRemove(gridPosition, out _);
             });
@@ -358,6 +426,14 @@ public class LargeGridPathfindingGame : Game
                 cellRect.Y = y * 10;
 
                 int cellValue = grid[y, x];
+                int cellWeight = gridFiller.WeightGrid[y, x];
+
+                if (cellWeight > 1 && cellValue >= 0)
+                {
+                    float blendFactor = Math.Clamp((cellWeight - 1) / 8f, 0f, 1f);
+                    Color weightColor = Color.Lerp(Color.LightYellow, Color.DarkOrange, blendFactor);
+                    spriteBatch.FillRectangle(cellRect, weightColor * 0.75f, layerDepth: 0.25f);
+                }
 
                 if (cellValue < 0)
                 {
@@ -431,6 +507,8 @@ public class LargeGridPathfindingGame : Game
         uiSpriteBatch.DrawString(uiFont, $"Grid: {gridFiller.Width}x{gridFiller.Height}", new Vector2(10, 30), Color.Black);
         uiSpriteBatch.DrawString(uiFont, $"Zones: {gridFiller.PlacedRectangles.Count}", new Vector2(10, 50), Color.Black);
         uiSpriteBatch.DrawString(uiFont, $"Agents: {agents.Count}", new Vector2(10, 70), Color.Black);
+        uiSpriteBatch.DrawString(uiFont, $"Brush mode: {brushMode} [O]", new Vector2(10, 90), Color.Black);
+        uiSpriteBatch.DrawString(uiFont, $"Weight: {paintWeight} [Keys 1-9, Weight mode]", new Vector2(10, 110), Color.Black);
 
         IReadOnlyList<ProgressTracker.ProgressData> progresses = progressTracker.GetProgresses();
 
@@ -440,7 +518,7 @@ public class LargeGridPathfindingGame : Game
 
             string progress = progressData.Indeterminate ? "..." : progressData.Progress.ToString("P0");
 
-            uiSpriteBatch.DrawString(uiFont, $"{progressData.Name}: {progress}", new Vector2(10, 90 + (i * 20)), Color.Black);
+            uiSpriteBatch.DrawString(uiFont, $"{progressData.Name}: {progress}", new Vector2(10, 130 + (i * 20)), Color.Black);
         }
 
         uiSpriteBatch.End();
@@ -455,7 +533,7 @@ public class LargeGridPathfindingGame : Game
             // Configuration options
             int width = 1000;
             int height = 1000;
-            int agentCount = 10000;
+            int agentCount = 1000;
 
             bool pathRandomization = false; // Randomize path costs to prevent agents from following the same path
             bool penalizeStretchedRectangles = false; // (EXPERIMENTAL) Penalize paths that go through stretched rectangles to prevent too many agents in a small area
@@ -556,7 +634,7 @@ public class LargeGridPathfindingGame : Game
 
             gridFiller.FillGrid(totalProgress: fillingGridProgressReporter, calculatingCandidatesProgress: calculatingCandidatesProgressReporter, placingCandidatesProgress: placingCandidatesProgressReporter);
 
-            pathfinder = new Pathfinder(gridFiller.PlacedRectangles, pathRandomization, penalizeStretchedRectangles);
+            pathfinder = new Pathfinder(gridFiller.PlacedRectangles, gridFiller.Grid, gridFiller.WeightGrid, pathRandomization, penalizeStretchedRectangles);
             gridChanged = true;
             inputBlocked = false;
             showZones = false;
@@ -636,17 +714,31 @@ public class LargeGridPathfindingGame : Game
         start ??= new Point(startX, startY);
         goal ??= new Point(goalX, goalY);
 
-        int[,] grid = gridFiller.Grid;
+        return pathfinder.FindPath(start.Value, goal.Value);
+    }
 
-        List<Vector2>? path = pathfinder.FindPath(ref grid[start.Value.Y, start.Value.X], ref grid[goal.Value.Y, goal.Value.X]);
+    private static List<Point> GetBrushPoints(Vector2 currentGridPosition, Vector2? previousGridPosition)
+    {
+        HashSet<Point> points = [];
+        Point currentPoint = new Point((int)currentGridPosition.X, (int)currentGridPosition.Y);
+        _ = points.Add(currentPoint);
 
-        if (path is not null)
+        if (previousGridPosition is null)
         {
-            // Add start and goal positions to the path because the pathfinder only returns transitions between rectangles
-            path.Insert(0, start.Value.ToVector2());
-            path.Add(goal.Value.ToVector2());
+            return [.. points];
         }
 
-        return path;
+        float distance = Vector2.Distance(previousGridPosition.Value, currentGridPosition);
+        int steps = Math.Max(1, (int)Math.Ceiling(distance * 10));
+
+        for (int i = 0; i <= steps; i++)
+        {
+            float t = i / (float)steps;
+            Vector2 interpolatedPosition = Vector2.Lerp(previousGridPosition.Value, currentGridPosition, t);
+            Point interpolatedPoint = new Point((int)Math.Round(interpolatedPosition.X), (int)Math.Round(interpolatedPosition.Y));
+            _ = points.Add(interpolatedPoint);
+        }
+
+        return [.. points];
     }
-}
+}

src/LargeGridPathfinding/Pathfinder.cs

@@ -11,23 +11,40 @@ namespace LargeGridPathfinding;
 internal class Pathfinder
 {
     private readonly ConcurrentDictionary<int, Rectangle> rectanglesSource;
+    private readonly int[,] grid;
+    private readonly int[,] weightGrid;
     private readonly bool pathRandomization;
     private readonly bool penalizeStretchedRectangles;
     private Dictionary<int, Rectangle> rectangleMap;
     private Dictionary<int, List<int>> adjacencyList;
 
-    public Pathfinder(ConcurrentDictionary<int, Rectangle> rectangles, bool pathRandomization = false, bool penalizeStretchedRectangles = false)
+    public Pathfinder(ConcurrentDictionary<int, Rectangle> rectangles, int[,] grid, int[,] weightGrid, bool pathRandomization = false, bool penalizeStretchedRectangles = false)
     {
         rectanglesSource = rectangles;
+        this.grid = grid;
+        this.weightGrid = weightGrid;
         this.pathRandomization = pathRandomization;
         this.penalizeStretchedRectangles = penalizeStretchedRectangles;
         rectangleMap = new Dictionary<int, Rectangle>(rectangles);
         adjacencyList = BuildGraph();
     }
 
-    public List<Vector2>? FindPath(ref int start, ref int goal)
+    public List<Vector2>? FindPath(Point startPoint, Point goalPoint)
     {
-        PriorityQueue<(int node, List<int> path, int cost), int> queue = new PriorityQueue<(int node, List<int> path, int cost), int>();
+        if (!IsInBounds(startPoint.X, startPoint.Y) || !IsInBounds(goalPoint.X, goalPoint.Y))
+        {
+            return null;
+        }
+
+        int start = grid[startPoint.Y, startPoint.X];
+        int goal = grid[goalPoint.Y, goalPoint.X];
+
+        if (start <= 0 || goal <= 0)
+        {
+            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);
 
@@ -43,17 +60,7 @@ internal class Pathfinder
 
             if (node == goal)
             {
-                // Convert rectangle path to coordinate transitions
-                List<Vector2> coordinatePath = [];
-
-                for (int i = 0; i < path.Count - 1; i++)
-                {
-                    (Vector2 entry, Vector2 exit) = GetTransitionPoints(path[i], path[i + 1], i + 2 < path.Count ? path[i + 2] : null);
-                    coordinatePath.Add(entry);
-                    coordinatePath.Add(exit);
-                }
-
-                return coordinatePath;
+                return BuildCoordinatePath(path, startPoint, goalPoint);
             }
 
             if (visited.Contains(node))
@@ -69,19 +76,22 @@ internal class Pathfinder
                 {
                     List<int> newPath = new List<int>(path) { neighbor };
 
-                    int newCost = cost + 1;
+                    int newCost = cost + GetRectangleWeight(neighbor);
 
                     if (penalizeStretchedRectangles)
                     {
-                        newCost += Math.Max(rectangleMap[neighbor].Width, rectangleMap[neighbor].Height) / Math.Min(rectangleMap[neighbor].Width, rectangleMap[neighbor].Height);
+                        Rectangle rectangle = rectangleMap[neighbor];
+                        newCost += Math.Max(rectangle.Width, rectangle.Height) / Math.Min(rectangle.Width, rectangle.Height);
                     }
 
+                    int priority = newCost + GetHeuristicCost(neighbor, goal);
+
                     if (pathRandomization)
                     {
-                        newCost += Random.Shared.Next(-1, 2);
+                        priority += Random.Shared.Next(-1, 2);
                     }
 
-                    queue.Enqueue((neighbor, newPath, newCost), newCost);
+                    queue.Enqueue((neighbor, newPath, newCost), priority);
                 }
             }
         }
@@ -96,6 +106,62 @@ internal class Pathfinder
         adjacencyList = BuildGraph();
     }
 
+    private bool IsInBounds(int x, int y) => x >= 0 && y >= 0 && y < grid.GetLength(0) && x < grid.GetLength(1);
+
+    private int GetRectangleWeight(int rectangleLabel)
+    {
+        Rectangle rectangle = rectangleMap[rectangleLabel];
+        return Math.Max(1, weightGrid[rectangle.Y, rectangle.X]);
+    }
+
+    private int GetHeuristicCost(int from, int to)
+    {
+        Rectangle r1 = rectangleMap[from];
+        Rectangle r2 = rectangleMap[to];
+        Point c1 = r1.Center;
+        Point c2 = r2.Center;
+        return Math.Abs(c1.X - c2.X) + Math.Abs(c1.Y - c2.Y);
+    }
+
+    private List<Vector2>? BuildCoordinatePath(IReadOnlyList<int> path, Point startPoint, Point goalPoint)
+    {
+        if (path.Count == 0)
+        {
+            return null;
+        }
+
+        if (path.Count == 1)
+        {
+            return [startPoint.ToVector2(), goalPoint.ToVector2()];
+        }
+
+        List<Vector2> coordinatePath = [startPoint.ToVector2()];
+
+        for (int i = 0; i < path.Count - 1; i++)
+        {
+            (Vector2 entry, Vector2 exit) = GetTransitionPoints(path[i], path[i + 1], i + 2 < path.Count ? path[i + 2] : null);
+
+            if (entry.X < 0 || exit.X < 0)
+            {
+                return null;
+            }
+
+            if (coordinatePath[^1] != entry)
+            {
+                coordinatePath.Add(entry);
+            }
+            coordinatePath.Add(exit);
+        }
+
+        Vector2 goalVector = goalPoint.ToVector2();
+        if (coordinatePath[^1] != goalVector)
+        {
+            coordinatePath.Add(goalVector);
+        }
+
+        return coordinatePath;
+    }
+
     private static bool AreRectanglesAdjacent(Rectangle r1, Rectangle r2)
     {
         bool adjacentHorizontally = (r1.X + r1.Width == r2.X || r2.X + r2.Width == r1.X) && r1.Y < r2.Y + r2.Height && r1.Y + r1.Height > r2.Y;
@@ -112,11 +178,9 @@ internal class Pathfinder
             return new Vector2(randomX, randomY);
         }
 
-        // Clamp the point to the closest edge of the rectangle
         float closestX = Math.Clamp(point.X, rect.Left, rect.Right - 1);
         float closestY = Math.Clamp(point.Y, rect.Top, rect.Bottom - 1);
 
-        // If point is inside the rectangle, push it to the nearest edge
         if (point.X >= rect.Left && point.X < rect.Right &&
             point.Y >= rect.Top && point.Y < rect.Bottom)
         {
@@ -148,7 +212,6 @@ internal class Pathfinder
         return new Vector2(closestX, closestY);
     }
 
-    // Build a graph where each rectangle is a node and each edge represents an adjacency
     private Dictionary<int, List<int>> BuildGraph()
     {
         Dictionary<int, List<int>> graph = [];
@@ -172,13 +235,10 @@ internal class Pathfinder
         return graph;
     }
 
-    // Find the transition points between two rectangles
     private (Vector2 entry, Vector2 exit) GetTransitionPoints(int from, int to, int? next = null)
     {
         Rectangle r1 = rectangleMap[from];
         Rectangle r2 = rectangleMap[to];
-
-        // Find all possible valid transition points
         List<(Vector2 entry, Vector2 exit)> possibleTransitions = [];
 
         if (r1.Right == r2.Left)
@@ -215,7 +275,6 @@ internal class Pathfinder
             return (new Vector2(-1, -1), new Vector2(-1, -1));
         }
 
-        // If there's a next rectangle, optimize the transition to minimize the distance
         if (next is not null && rectangleMap.TryGetValue(next.Value, out Rectangle r3))
         {
             (Vector2 entry, Vector2 exit) bestTransition = possibleTransitions
@@ -225,7 +284,6 @@ internal class Pathfinder
             return bestTransition;
         }
 
-        // Default: Pick the middle transition point
         return possibleTransitions[possibleTransitions.Count / 2];
     }
-}
+}