From cd9718d61c43cf0f2b9c5e278303d265e6306b91 Mon Sep 17 00:00:00 2001 From: Stone_Red <56473591+Stone-Red-Code@users.noreply.github.com> Date: Tue, 5 May 2026 01:16:19 +0200 Subject: [PATCH] Support weighted path finding --- src/LargeGridPathfinding/GridFiller.cs | 212 ++++++++++++++++-- .../LargeGridPathfindingGame.cs | 202 ++++++++++++----- src/LargeGridPathfinding/Pathfinder.cs | 112 ++++++--- 3 files changed, 420 insertions(+), 106 deletions(-) diff --git a/src/LargeGridPathfinding/GridFiller.cs b/src/LargeGridPathfinding/GridFiller.cs index 06282df..ccb66b7 100644 --- a/src/LargeGridPathfinding/GridFiller.cs +++ b/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 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 points, int weight) + { + int clampedWeight = Math.Max(1, weight); + HashSet 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 removedRectangles = [new Rectangle(x, y, w - x, h - y)]; + HashSet 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 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; } -} \ No newline at end of file + + 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; + } + +} diff --git a/src/LargeGridPathfinding/LargeGridPathfindingGame.cs b/src/LargeGridPathfinding/LargeGridPathfindingGame.cs index 19a2edb..a6cf782 100644 --- a/src/LargeGridPathfinding/LargeGridPathfindingGame.cs +++ b/src/LargeGridPathfinding/LargeGridPathfindingGame.cs @@ -17,6 +17,12 @@ namespace LargeGridPathfinding; public class LargeGridPathfindingGame : Game { + private enum BrushMode + { + Weight, + Obstacle + } + private readonly ConcurrentDictionary temporaryIndicators = []; private readonly ProgressTracker progressTracker = new ProgressTracker(); private readonly List 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 brushPoints = GetBrushPoints(gridPosition, previousGridPosition); + foreach (Point point in brushPoints) { - List 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 brushPoints = GetBrushPoints(gridPosition, previousGridPosition); + foreach (Point point in brushPoints) { - List 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 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? path = pathfinder.FindPath(ref grid[start.Value.Y, start.Value.X], ref grid[goal.Value.Y, goal.Value.X]); + private static List GetBrushPoints(Vector2 currentGridPosition, Vector2? previousGridPosition) + { + HashSet 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]; } -} \ No newline at end of file +} diff --git a/src/LargeGridPathfinding/Pathfinder.cs b/src/LargeGridPathfinding/Pathfinder.cs index 786a42a..1c01fdd 100644 --- a/src/LargeGridPathfinding/Pathfinder.cs +++ b/src/LargeGridPathfinding/Pathfinder.cs @@ -11,23 +11,40 @@ namespace LargeGridPathfinding; internal class Pathfinder { private readonly ConcurrentDictionary rectanglesSource; + private readonly int[,] grid; + private readonly int[,] weightGrid; private readonly bool pathRandomization; private readonly bool penalizeStretchedRectangles; private Dictionary rectangleMap; private Dictionary> adjacencyList; - public Pathfinder(ConcurrentDictionary rectangles, bool pathRandomization = false, bool penalizeStretchedRectangles = false) + public Pathfinder(ConcurrentDictionary 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(rectangles); adjacencyList = BuildGraph(); } - public List? FindPath(ref int start, ref int goal) + public List? FindPath(Point startPoint, Point goalPoint) { - PriorityQueue<(int node, List path, int cost), int> queue = new PriorityQueue<(int node, List 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 path, int cost), int> queue = new(); HashSet visited = []; queue.Enqueue((start, new List { start }, 0), 0); @@ -43,17 +60,7 @@ internal class Pathfinder if (node == goal) { - // Convert rectangle path to coordinate transitions - List 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 newPath = new List(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? BuildCoordinatePath(IReadOnlyList path, Point startPoint, Point goalPoint) + { + if (path.Count == 0) + { + return null; + } + + if (path.Count == 1) + { + return [startPoint.ToVector2(), goalPoint.ToVector2()]; + } + + List 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> BuildGraph() { Dictionary> 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]; } -} \ No newline at end of file +}