Stone_Red/LargeGridPathfinding
1
0
Fork 0
mirror of https://github.com/Stone-Red-Code/LargeGridPathfinding.git synced 2026-09-04 00:56:10 +02:00
Code Issues 2 Packages Projects Releases Wiki Activity

Add maxNodes limit, path caching, and rectangle weight fixes to pathfinding

Browse Source
This commit is contained in:
Stone_Red
2026-07-08 02:09:21 +02:00
parent 79ee1e06f4
commit e8ee6a9398
2 changed files with 79 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/LargeGridPathfinding/LargeGridPathfindingGame.cs
+36 -20
View File
@@ -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<float> 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<Vector2>? path = CalculatePath(agent.GridPosition, agent.Destination)
?? CalculatePath();
List<Vector2>? path = CalculatePath(agent.GridPosition, agent.Destination, maxNodes: int.MaxValue) ?? CalculatePath(maxNodes: int.MaxValue);
pendingPathResults.Enqueue((agent, path));
}
@@ -1215,8 +1216,11 @@ public class LargeGridPathfindingGame : Game
progressTracker.RemoveProgress(progressDataPaths);
}
if (agentsRequirePathList.Count < 1000)
{
await Task.Delay(100);
}
}
catch (Exception ex)
{
Trace.WriteLine(ex);
@@ -1244,19 +1248,31 @@ public class LargeGridPathfindingGame : Game
agentSectors = newSectors;
}
private List<Vector2>? CalculatePath(Point? start = null, Point? goal = null)
private List<Vector2>? 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<Point> points, PendingOperation operation)
src/LargeGridPathfinding/Pathfinder.cs
+42 -7
View File
@@ -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<int, Rectangle> rectangleMap;
private Dictionary<int, List<int>> adjacencyList;
public bool EnablePathCache { get; set; } = false;
/// <summary>
/// Returns the current zone adjacency list.
/// </summary>
@@ -60,7 +63,7 @@ public class Pathfinder
/// <summary>
/// Finds a path between two grid points as coordinate waypoints.
/// </summary>
public List<Vector2>? FindPath(Point startPoint, Point goalPoint)
public List<Vector2>? 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<int, int> gScore = [];
Dictionary<int, int> 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<int>? 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<int, Rectangle>(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<int, List<int>> incomingByAffected = [];
foreach (int zoneId in affectedZones)