diff --git a/src/LargeGridPathfinding/LargeGridPathfindingGame.cs b/src/LargeGridPathfinding/LargeGridPathfindingGame.cs index 511b5e9..f509008 100644 --- a/src/LargeGridPathfinding/LargeGridPathfindingGame.cs +++ b/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 pathMaxSX = Math.Min(sectorsX - 1, (int)((viewRight / 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 pathMinSX = Math.Max(0, (int)(viewLeft / 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 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 drawMaxSX = Math.Min(sectorsX - 1, (int)(xEnd / SectorSize) + 1); - int drawMinSY = Math.Max(0, (int)(yStart / SectorSize) - 1); - int drawMaxSY = Math.Min(sectorsY - 1, (int)(yEnd / SectorSize) + 1); + int drawMinSX = Math.Max(0, (xStart / SectorSize) - 1); + int drawMaxSX = Math.Min(sectorsX - 1, (xEnd / SectorSize) + 1); + int drawMinSY = Math.Max(0, (yStart / 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 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? path = CalculatePath(agent.GridPosition, agent.Destination) - ?? CalculatePath(); + List? path = CalculatePath(agent.GridPosition, agent.Destination, maxNodes: int.MaxValue) ?? CalculatePath(maxNodes: int.MaxValue); pendingPathResults.Enqueue((agent, path)); } @@ -1215,7 +1216,10 @@ public class LargeGridPathfindingGame : Game progressTracker.RemoveProgress(progressDataPaths); } - await Task.Delay(100); + if (agentsRequirePathList.Count < 1000) + { + await Task.Delay(100); + } } catch (Exception ex) { @@ -1244,19 +1248,31 @@ public class LargeGridPathfindingGame : Game agentSectors = newSectors; } - private List? CalculatePath(Point? start = null, Point? goal = null) + private List? 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 startY = Random.Shared.Next(0, height - 1); - int goalX = Random.Shared.Next(0, width - 1); - int goalY = Random.Shared.Next(0, height - 1); + int[,] g = gridFiller.Grid; - start ??= new Point(startX, startY); - goal ??= new Point(goalX, goalY); + if (start is null) + { + 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 points, PendingOperation operation) diff --git a/src/LargeGridPathfinding/Pathfinder.cs b/src/LargeGridPathfinding/Pathfinder.cs index fccabf9..c57805e 100644 --- a/src/LargeGridPathfinding/Pathfinder.cs +++ b/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 rectangleMap; private Dictionary> adjacencyList; + public bool EnablePathCache { get; set; } = false; + /// /// Returns the current zone adjacency list. /// @@ -60,7 +63,7 @@ public class Pathfinder /// /// Finds a path between two grid points as coordinate waypoints. /// - public List? FindPath(Point startPoint, Point goalPoint) + public List? 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 gScore = []; Dictionary 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? 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(rectangle.Width, rectangle.Height) / Math.Min(rectangle.Width, rectangle.Height); + tentativeG += Math.Max(neighborRect.Width, neighborRect.Height) / Math.Min(neighborRect.Width, neighborRect.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(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> incomingByAffected = []; foreach (int zoneId in affectedZones)