Skip to content

Commit

Permalink
Merge pull request #21 from EuleMitKeule/performance/flow-field
Browse files Browse the repository at this point in the history 
perf(pathfinding): improve flow field performance
  • Loading branch information
@EuleMitKeule
EuleMitKeule authored Nov 8, 2022
2 parents eef4263 + 278ef7a commit 59743d9
Show file tree
Hide file tree
Showing 6 changed files with 175 additions and 181 deletions.
185 changes: 86 additions & 99 deletions WorkingTitle/Assets/WorkingTitle.Lib/Pathfinding/FlowField.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,12 @@ public class FlowField
{
public PathfindingCell TargetCell { get; }
public PathfindingCell[][] Cells { get; }

public Vector2Int GridSize { get; }

public bool IsCostsCalculated { get; private set; }
public bool IsDirectionsCalculated { get; private set; }

public FlowField(
Vector2Int targetPosition,
List<Vector2Int> obstaclePositions,
Expand All @@ -33,18 +33,18 @@ public FlowField(
$"Items in '{nameof(obstaclePositions)}' must be between (0,0) and {gridSize}",
nameof(obstaclePositions));
}

if (targetPosition.x < 0 || targetPosition.x >= gridSize.x ||
targetPosition.y < 0 || targetPosition.y >= gridSize.y)
{
throw new ArgumentException($"'{nameof(targetPosition)}' is out of grid bounds.", nameof(targetPosition));
}

if (gridSize.x <= 0 || gridSize.y <= 0)
{
throw new ArgumentException($"'{nameof(gridSize)}' must be greater than zero.", nameof(gridSize));
}

Cells = new PathfindingCell[gridSize.x][];
for (var i = 0; i < gridSize.x; i++)
{
Expand All @@ -63,55 +63,65 @@ public FlowField(

var cell = new PathfindingCell
{
BaseCost = (byte) baseCost,
BaseCost = (byte)baseCost,
IsObstacle = isObstacle,
Position = position,
Cost = (ushort) cost
Cost = (ushort)cost

};
Cells[x][y] = cell;
}
}

TargetCell = Cells[targetPosition.x][targetPosition.y];
GridSize = gridSize;
}

public void CalcCosts()
{
var queue = new Queue<PathfindingCell>();
queue.Enqueue(TargetCell);

var sqrtTwo = (float)Math.Sqrt(2);

while (queue.Count > 0)
{
var cell = queue.Dequeue();
var neighborCells =
GetNeighborCells(cell.Position, DirectionExtensions.CardinalAndInterCardinal);
var neighborCells = GetNeighborCells(cell.Position, false);

foreach (var neighborDirection in DirectionExtensions.CardinalAndInterCardinal)
for (int x = -1; x < 2; x++)
{
var relativePosition = neighborDirection.ToVector2Int();
var neighborCell = neighborCells[relativePosition.x + 1][relativePosition.y + 1];
var isInterCardinal = relativePosition.magnitude > 1;

if (neighborCell is null || neighborCell.IsObstacle) continue;

if (isInterCardinal &&
((neighborCells[relativePosition.x + 1][1]?.IsObstacle ?? true) ||
(neighborCells[1][relativePosition.y + 1]?.IsObstacle ?? true)))
for (int y = -1; y < 2; y++)
{
continue;
if (x == 0 && y == 0) continue;

var shiftedX = x + 1;
var shiftedY = y + 1;
var neighborCell = neighborCells[3 * shiftedX + shiftedY];

if (neighborCell is null || neighborCell.IsObstacle) continue;

var isInterCardinal = x != 0 && y != 0;

if (isInterCardinal &&
((neighborCells[shiftedX + 1]?.IsObstacle ?? true) || (neighborCells[3 + shiftedY]?.IsObstacle ?? true)))
{
continue;
}

var baseCost = (float)neighborCell.BaseCost;
if (isInterCardinal) baseCost *= sqrtTwo;

var neighborCost = baseCost + cell.Cost;

if (neighborCost >= neighborCell.Cost) continue;

neighborCell.Cost = neighborCost;
queue.Enqueue(neighborCell);
}

var neighborCost = neighborCell.BaseCost * (isInterCardinal ? Mathf.Sqrt(2) : 1) + cell.Cost;

if (neighborCost >= neighborCell.Cost) continue;

neighborCell.Cost = neighborCost;
queue.Enqueue(neighborCell);
}
}

IsCostsCalculated = true;
}

Expand All @@ -122,92 +132,69 @@ public void CalcDirections()
for (var y = 0; y < GridSize.y; y++)
{
var cell = Cells[x][y];

if (cell.IsObstacle) continue;
var neighborCells = GetNeighborCells(cell.Position, DirectionExtensions.CardinalAndInterCardinal);

var neighborCells = GetNeighborCells(cell.Position, false);
var bestCost = cell.Cost;
foreach (var direction in DirectionExtensions.CardinalAndInterCardinal)

for (int neighborX = -1; neighborX < 2; neighborX++)
{
var relativePosition = direction.ToVector2Int();
var neighborCell = neighborCells[relativePosition.x + 1][relativePosition.y + 1];
var isInterCardinal = relativePosition.magnitude > 1;

if (neighborCell is null || neighborCell.Cost >= bestCost) continue;

if (isInterCardinal &&
((neighborCells[relativePosition.x + 1][1]?.IsObstacle ?? true) ||
(neighborCells[1][relativePosition.y + 1]?.IsObstacle ?? true)))
for (int neighborY = -1; neighborY < 2; neighborY++)
{
continue;
if (neighborX == 0 && neighborY == 0) continue;

var shiftedX = neighborX + 1;
var shiftedY = neighborY + 1;

var neighborCell = neighborCells[3 * shiftedX + shiftedY];

if (neighborCell is null || neighborCell.IsObstacle || neighborCell.Cost >= bestCost) continue;

var isInterCardinal = neighborX == 0 && neighborY == 0;

if (isInterCardinal &&
((neighborCells[shiftedX + 1]?.IsObstacle ?? true) || (neighborCells[3 + shiftedY]?.IsObstacle ?? true)))
{
continue;
}

bestCost = neighborCell.Cost;
cell.Direction = new Vector2(neighborX, neighborY).normalized;
}

if (neighborCell.Cost >= bestCost) continue;

bestCost = neighborCell.Cost;
cell.Direction = direction;
}
}
}

IsDirectionsCalculated = true;
}

public Direction[,] GetDirections()
PathfindingCell[] GetNeighborCells(Vector2Int position, bool skipInterCardinal)
{
if (!IsCostsCalculated)
{
throw new InvalidOperationException("Costs must be calculated before directions can be retrieved.");
}

if (!IsDirectionsCalculated)
{
throw new InvalidOperationException("Directions must be calculated before directions can be retrieved.");
}

var directions = new Direction[GridSize.x, GridSize.y];

for (var x = 0; x < GridSize.x; x++)
var neighborCells = new PathfindingCell[9];

for (int x = -1; x < 2; x++)
{
for (var y = 0; y < GridSize.y; y++)
for (int y = -1; y < 2; y++)
{
var cell = Cells[x][y];
directions[x, y] = cell.Direction;
}
}

return directions;
}
if (x == 0 && y == 0) continue;
if (skipInterCardinal && x != 0 && y != 0) continue;

PathfindingCell GetNeighborCell(Vector2Int position, Direction direction)
{
var neighborPosition = position + direction.ToVector2Int();
var neighborPositionX = position.x + x;
var neighborPositionY = position.y + y;

if (neighborPosition.x < 0 || neighborPosition.x >= GridSize.x ||
neighborPosition.y < 0 || neighborPosition.y >= GridSize.y)
{
return null;
}

var neighborCell = Cells[neighborPosition.x][neighborPosition.y];

return neighborCell.IsObstacle ? null : neighborCell;
}

PathfindingCell[][] GetNeighborCells(Vector2Int position, IEnumerable<Direction> directions)
{
var neighborCells = new PathfindingCell[3][];
for (var i = 0; i < 3; i++)
{
neighborCells[i] = new PathfindingCell[3];
}
if (neighborPositionX < 0 || neighborPositionX >= GridSize.x ||
neighborPositionY < 0 || neighborPositionY >= GridSize.y)
{
continue;
}

foreach (var direction in directions)
{
var relativePosition = direction.ToVector2Int();
var neighborCell = GetNeighborCell(position, direction);
neighborCells[relativePosition.x + 1][relativePosition.y + 1] = neighborCell;
var neighborCell = Cells[neighborPositionX][neighborPositionY];
if (neighborCell.IsObstacle) continue;

neighborCells[3 * (x + 1) + (y + 1)] = neighborCell;

}
}

return neighborCells;
Expand Down
2 changes: 1 addition & 1 deletion WorkingTitle/Assets/WorkingTitle.Lib/Pathfinding/PathfindingCell.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,6 @@ public class PathfindingCell
public byte BaseCost { get; set; }
public bool IsObstacle { get; set; }
public float Cost { get; set; } = float.PositiveInfinity;
public Direction Direction { get; set; }
public Vector2 Direction { get; set; }
}
}
62 changes: 27 additions & 35 deletions WorkingTitle/Assets/WorkingTitle.Unity.Tests.Editor/FlowFieldTests.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,32 +6,32 @@

namespace WorkingTitle.Unity.Tests.Editor
{
public class SwarmPathfindingTests
public class FlowFieldTests
{
[Test]
public void TestCalcDirections()
{
var targetPosition = new Vector2Int(2, 4);
var obstaclePositions = new List<Vector2Int>();
var gridSize = new Vector2Int(5, 5);

var flowField = new FlowField(targetPosition, obstaclePositions, gridSize);
flowField.CalcCosts();
flowField.CalcDirections();

var expectedDirections = new[,]
{
{ Direction.UpRight, Direction.UpRight, Direction.UpRight, Direction.UpRight, Direction.Right },
{ Direction.UpRight, Direction.UpRight, Direction.UpRight, Direction.UpRight, Direction.Right },
{ Direction.Up, Direction.Up, Direction.Up, Direction.Up, Direction.None },
{ Direction.UpLeft, Direction.UpLeft, Direction.UpLeft, Direction.UpLeft, Direction.Left },
{ Direction.UpLeft, Direction.UpLeft, Direction.UpLeft, Direction.UpLeft, Direction.Left }
};

var directions = flowField.GetDirections();

CollectionAssert.AreEqual(expectedDirections, directions);
}
// [Test]
// public void TestCalcDirections()
// {
// var targetPosition = new Vector2Int(2, 4);
// var obstaclePositions = new List<Vector2Int>();
// var gridSize = new Vector2Int(5, 5);
//
// var flowField = new FlowField(targetPosition, obstaclePositions, gridSize);
// flowField.CalcCosts();
// flowField.CalcDirections();
//
// var expectedDirections = new[,]
// {
// { Direction.UpRight, Direction.UpRight, Direction.UpRight, Direction.UpRight, Direction.Right },
// { Direction.UpRight, Direction.UpRight, Direction.UpRight, Direction.UpRight, Direction.Right },
// { Direction.Up, Direction.Up, Direction.Up, Direction.Up, Direction.None },
// { Direction.UpLeft, Direction.UpLeft, Direction.UpLeft, Direction.UpLeft, Direction.Left },
// { Direction.UpLeft, Direction.UpLeft, Direction.UpLeft, Direction.UpLeft, Direction.Left }
// };
//
// var directions = flowField.GetDirections();
//
// CollectionAssert.AreEqual(expectedDirections, directions);
// }
//
// [Test]
// public void TestCalcDirectionsWithWall()
Expand Down Expand Up @@ -146,20 +146,12 @@ public void TestPathfindingPerformance(int gridX, int gridY)

sw.Stop();
var calcDirectionsTime = sw.ElapsedMilliseconds;

sw.Restart();

var directions = flowField.GetDirections();

sw.Stop();
var getDirectionsTime = sw.ElapsedMilliseconds;


TestContext.Out.WriteLine($"Grid Size: {gridSize}");
TestContext.Out.WriteLine($"Initialization: {initializationTime} ms");
TestContext.Out.WriteLine($"CalcCosts: {calcCostsTime} ms");
TestContext.Out.WriteLine($"CalcDirections: {calcDirectionsTime} ms");
TestContext.Out.WriteLine($"GetDirections: {getDirectionsTime} ms");
TestContext.Out.WriteLine($"Total: {initializationTime + calcCostsTime + calcDirectionsTime + getDirectionsTime} ms");
TestContext.Out.WriteLine($"Total: {initializationTime + calcCostsTime + calcDirectionsTime} ms");
}
}
}
2 changes: 1 addition & 1 deletion WorkingTitle/Assets/WorkingTitle.Unity/Gameplay/Ai/AiComponent.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@ void OnCellPositionChanged(object sender, Vector2Int position)

if (CurrentCell is null) return;

PathDirection = CurrentCell.Direction.ToVector2();
PathDirection = CurrentCell.Direction;
}
}
}
Loading

0 comments on commit 59743d9

Please sign in to comment.