/// <summary>
/// Smooths a path, without affecting its cost, using the string pulling algorithm.
/// </summary>
/// <param name="grid">The grid</param>
/// <param name="path">The path (updated in place)</param>
/// <param name="movementPattern">Movement pattern for the agent</param>
/// <param name="maxSmoothDistance">Max distance to consider when 'pulling the string'. A larger distance smooths the path over longer distances but costs more</param>
public static void SmoothPath(Grid grid, Position[] path, Offset[] movementPattern, int maxSmoothDistance)
{
ClearSwapList();
// The tail of the string slowly moves forward
for (var tail = 0; tail < path.Length - 2; tail++)
{
// While the head of the string moves from its max length (or the end of the path
// towards the tail
for (var head = Math.Min(path.Length - 1, tail + maxSmoothDistance); head > tail + 1; head--)
{
// Meanwhile a path segment between the head and tail is adjusted to
// fit on a straight line between the head and the tail, if possible.
var current = tail + 1;
MessageIterate(path[current], path[current]);
// Find if there is a cell on the straightest path between head and tail
// using the movement pattern of the agent
var direction = GetDirection(path[tail], path[head], movementPattern);
if (!direction.HasValue)
{
continue;
}
var originalPosition = path[current];
var candidatePosition = path[current - 1] + direction.Value;
// If the best candidate is what we've already got, we can't improve
if (originalPosition == candidatePosition)
{
continue;
}
// If the candidate would disconnect our path we can't improve
if (!Connected(candidatePosition, path[current + 1], movementPattern))
{
continue;
}
var originalCost = grid.GetCellCostUnchecked(originalPosition);
var candidateCost = grid.GetCellCostUnchecked(candidatePosition);
// If the candidate is more costly than our original we can't improve
if (candidateCost > originalCost)
{
continue;
}
path[current] = candidatePosition;
MessageSwap(originalPosition, candidatePosition);
}
}
}
private static void Step(
Grid grid,
Boundary boundary,
MinHeap open,
Position[] cameFrom,
float[] costSoFar,
Offset[] movementPattern,
AgentShape shape,
Position current,
Position end)
{
// Get the cost associated with getting to the current position
var initialCost = costSoFar[grid.GetIndexUnchecked(current)];
// Get all directions we can move to according to the movement pattern and the dimensions of the grid
foreach (var option in GetMovementOptions(current, boundary, movementPattern))
{
var position = current + option;
var cellCost = grid.GetCellCostUnchecked(position, shape);
// Ignore this option if the cell is blocked
if (float.IsInfinity(cellCost))
{
continue;
}
var index = grid.GetIndexUnchecked(position);
// Compute how much it would cost to get to the new position via this path
var newCost = initialCost + cellCost * option.Cost;
// Compare it with the best cost we have so far, 0 means we don't have any path that gets here yet
var oldCost = costSoFar[index];
if (!(oldCost <= 0) && !(newCost < oldCost))
{
continue;
}
// Update the best path and the cost if this path is cheaper
costSoFar[index] = newCost;
cameFrom[index] = current;
// Use the heuristic to compute how much it will probably cost
// to get from here to the end, and store the node in the open list
var expectedCost = newCost + ManhattanDistance(position, end);
open.Push(new MinHeapNode(position, expectedCost));
MessageOpen(position);
}
}
public static SearchNode FindPath(Grid grid, Position start, Position end, Offset[] movementPattern)
{
ClearStepList();
var head = new SearchNode(start);
var open = new MinHeap();
open.Push(head);
var marked = new bool[grid.DimX * grid.DimY];
while (open.HasNext())
{
var current = open.Pop();
MessageCurrent(current);
if (current.Position == end)
{
return(current);
}
foreach (var p in GetNeighbours(current.Position, grid.DimX, grid.DimY, movementPattern))
{
var index = grid.GetIndexUnchecked(p.X, p.Y);
// Use the unchecked variant here since GetNeighbours already filters out positions that are out of bounds
var cellCost = grid.GetCellCostUnchecked(p);
if (!marked[index] && !float.IsInfinity(cellCost))
{
marked[index] = true;
MessageOpen(p);
var costSoFar = current.CostSoFar + cellCost;
var expectedCost = costSoFar + ChebyshevDistance(p, end);
open.Push(new SearchNode(p, expectedCost, costSoFar)
{
Next = current
});
}
}
MessageClose(current.Position);
}
return(null);
}