/// <summary>
/// Initializes a new instance of the <see cref="PathingAStar"/> class.
/// </summary>
/// <param name="heapInitialSize">Initial size of the heap.</param>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <param name="pathSmoother">The path smoother to use.</param>
public PathingAStar(int heapInitialSize, IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths pathSmoother)
: base(moveCostProvider, cellCostStrategy, pathSmoother)
{
_openSet = new BinaryHeap<IPathNode>(heapInitialSize, new PathNodeComparer());
_expandedSet = new List<IPathNode>();
_successorArray = new DynamicArray<IPathNode>(15);
}
/// <summary>
/// Initializes a new instance of the <see cref="PathingAStar"/> class.
/// </summary>
/// <param name="heapInitialSize">Initial size of the heap.</param>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <param name="cellCostStrategy">The cell cost provider.</param>
/// <param name="pathSmoother">The path smoother to use.</param>
/// <param name="requestPreprocessors">The list of request preprocessors to use.</param>
public PathingAStar(int heapInitialSize, IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths pathSmoother, IRequestPreProcessor[] requestPreprocessors)
: base(moveCostProvider, cellCostStrategy, pathSmoother, requestPreprocessors)
{
_openSet = new BinaryHeap <IPathNode>(heapInitialSize, new PathNodeComparer());
_expandedSet = new List <IPathNode>();
_successorArray = new DynamicArray <IPathNode>(15);
}
/// <summary>
/// Initializes a new instance of the <see cref="PathingEngineBase"/> class.
/// </summary>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <param name="smoother">The path smoother to use</param>
protected PathingEngineBase(IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths smoother)
{
Ensure.ArgumentNotNull(moveCostProvider, "moveCostProvider");
Ensure.ArgumentNotNull(cellCostStrategy, "cellCostStrategy");
Ensure.ArgumentNotNull(smoother, "smoother");
_costProvider = moveCostProvider;
_smoother = smoother;
_cellCostStrategy = cellCostStrategy;
_coroutineIter = new SafeIterator(this);
}
/// <summary>
/// Initializes a new instance of the <see cref="PathingEngineBase"/> class.
/// </summary>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <param name="smoother">The path smoother to use</param>
protected PathingEngineBase(IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths smoother)
{
Ensure.ArgumentNotNull(moveCostProvider, "moveCostProvider");
Ensure.ArgumentNotNull(cellCostStrategy, "cellCostStrategy");
Ensure.ArgumentNotNull(smoother, "smoother");
_costProvider = moveCostProvider;
_smoother = smoother;
_cellCostStrategy = cellCostStrategy;
_coroutineIter = new SafeIterator(this);
}
/// <summary>
/// Initializes a new instance of the <see cref="PathingEngineBase"/> class.
/// </summary>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <param name="cellCostStrategy">The cell cost provider.</param>
/// <param name="smoother">The path smoother to use</param>
/// <param name="requestPreprocessors">The list of request preprocessors to use.</param>
protected PathingEngineBase(IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths smoother, IRequestPreProcessor[] requestPreprocessors)
{
Ensure.ArgumentNotNull(moveCostProvider, "moveCostProvider");
Ensure.ArgumentNotNull(cellCostStrategy, "cellCostStrategy");
Ensure.ArgumentNotNull(smoother, "smoother");
_costProvider = moveCostProvider;
_smoother = smoother;
_cellCostStrategy = cellCostStrategy;
_coroutineIter = new SafeIterator(this);
_segmentRequest = new SegmentRequest();
_segments = new DynamicArray <Path>(10);
_requestPreprocessors = requestPreprocessors;
}
/// <summary>
/// Smooths a path.
/// </summary>
/// <param name="goal">The goal node of the calculated path.</param>
/// <param name="maxPathLength">Maximum length of the path.</param>
/// <param name="request">The path request.</param>
/// <param name="costStrategy">The cell cost provider.</param>
/// <returns>
/// The path in smoothed form
/// </returns>
public Path Smooth(IPathNode goal, int maxPathLength, IPathRequest request, ICellCostStrategy costStrategy)
{
//The goal node represents the end of the path. Using it you can traverse backwards using the predecessor property,
//until you reach the start node, which is identified by having no predecessor.
//Your aim then is to process the nodes and remove superfluous nodes and add the significant nodes to the resulting stack.
//For this example we simply do nothing at all and return the path as it was intended by the path finder
var result = new Path(maxPathLength);
var node = goal;
while (node != null)
{
result.Push(node);
node = node.predecessor;
}
return(result);
}
/// <summary>
/// Initializes a new instance of the <see cref="PathingJumpPointSearch"/> class.
/// </summary>
/// <param name="heapInitialSize">Initial size of the heap.</param>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <param name="pathSmoother">The path smoother to use.</param>
public PathingJumpPointSearch(int heapInitialSize, IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths pathSmoother)
: base(heapInitialSize, moveCostProvider, cellCostStrategy, pathSmoother)
{
_neighbours = new DynamicArray<IPathNode>(8);
}
/// <summary>
/// Initializes a new instance of the <see cref="PathingJumpPointSearch"/> class.
/// </summary>
/// <param name="heapInitialSize">Initial size of the heap.</param>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <param name="cellCostStrategy">The cell cost provider.</param>
/// <param name="pathSmoother">The path smoother to use.</param>
/// <param name="requestPreprocessors">The list of request preprocessors to use.</param>
public PathingJumpPointSearch(int heapInitialSize, IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths pathSmoother, IRequestPreProcessor[] requestPreprocessors)
: base(heapInitialSize, moveCostProvider, cellCostStrategy, pathSmoother, requestPreprocessors)
{
_neighbours = new DynamicArray <IPathNode>(8);
}
public VisualizedJPS(int heapInitialSize, IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths pathSmoother, IRequestPreProcessor[] requestPreprocessors)
: base(heapInitialSize, moveCostProvider, cellCostStrategy, pathSmoother, requestPreprocessors)
{
}
public static bool CanReducePath(IPositioned point1, IPositioned point3, IUnitProperties unitProps, CellMatrix matrix, ICellCostStrategy costStrategy)
{
Vector3 p1;
Vector3 p3;
//Assign the points so we start with the point with the lowest x-value to simplify things
if (point1.position.x > point3.position.x)
{
p1 = point3.position;
p3 = point1.position;
}
else
{
p1 = point1.position;
p3 = point3.position;
}
var requesterRadius = unitProps.radius;
var tan = Tangents.Create(p1, p3, requesterRadius);
var incZ = tan.slopeDir;
var cellSize = matrix.cellSize;
var halfCell = cellSize / 2.0f;
//Adjust the start and end cells to possibly include their immediate neighbour if the unit's radius crossed said boundary.
var radiusAdjust = new Vector3(requesterRadius, 0.0f, requesterRadius * incZ);
//Get the start and end cells, get the cost of the actual start and end, and then reassign the start and end with the above adjustment.
var startCell = matrix.GetCell(p1, true);
var startCost = costStrategy.GetCellCost(startCell, unitProps);
startCell = matrix.GetCell(p1 - radiusAdjust, true);
var endCell = matrix.GetCell(p3, true);
var endCost = costStrategy.GetCellCost(endCell, unitProps);
endCell = matrix.GetCell(p3 + radiusAdjust, true);
//We want x to end up on cell boundaries, the first of which is this far from the first points position
var xAdj = p1.x + (startCell.position.x - p1.x) + halfCell;
//We want to adjust z so that we correctly count impacted cells, this adjusts z so it starts at the bottom boundary of the first cell (for purposes of calculation)
var zAdj = p1.z - (halfCell + ((p1.z - startCell.position.z) * incZ));
//The movement across the x-axis
float deltaX = 0.0f;
var cellMatrix = matrix.rawMatrix;
int indexX = 0;
for (int x = startCell.matrixPosX; x <= endCell.matrixPosX; x++)
{
//So instead of just checking all cells in the bounding rect defined by the two cells p1 and p3,
//we limit it to the cells immediately surrounding the proposed line (tangents), including enough cells that we ensure the unit will be able to pass through,
//at the extreme routes between the two cells (i.e top corner to top corner and bottom corner to bottom corner
int startZ;
int endZ;
//If the tangents are horizontal or vertical z range is obvious
if (tan.isAxisAligned)
{
startZ = startCell.matrixPosZ;
endZ = endCell.matrixPosZ + incZ;
}
else
{
if (indexX == 0)
{
startZ = startCell.matrixPosZ;
}
else
{
var startCellsPassed = Mathf.FloorToInt((tan.LowTangent(deltaX) - zAdj) / cellSize) * incZ;
startZ = LimitStart(
startCell.matrixPosZ + startCellsPassed,
startCell.matrixPosZ,
incZ);
}
//The movement this step will perform across the x-axis
deltaX = xAdj + (indexX * cellSize);
var endCellsIntercepted = Mathf.FloorToInt((tan.HighTangent(deltaX) - zAdj) / cellSize) * incZ;
endZ = LimitEnd(
startCell.matrixPosZ + endCellsIntercepted,
endCell.matrixPosZ,
incZ) + incZ;
}
indexX++;
for (int z = startZ; z != endZ; z += incZ)
{
var intermediary = cellMatrix[x, z];
var intermediaryCost = costStrategy.GetCellCost(intermediary, unitProps);
if (!intermediary.isWalkableFrom(startCell, unitProps) || (startCost < intermediaryCost && endCost < intermediaryCost))
{
return false;
}
}
}
return true;
}
/// <summary>
/// Smooths a path.
/// </summary>
/// <param name="goal">The goal node of the calculated path.</param>
/// <param name="maxPathLength">Maximum length of the path.</param>
/// <param name="request">The path request.</param>
/// <param name="costStrategy">The cell cost provider.</param>
/// <returns>
/// The path in smoothed form
/// </returns>
public Path Smooth(IPathNode goal, int maxPathLength, IPathRequest request, ICellCostStrategy costStrategy)
{
var unitProps = request.requesterProperties;
//Next prune superfluous path nodes
var reversePath = new List<IPositioned>(maxPathLength);
var current = goal;
var next = current.predecessor;
int bends = -1;
var prevDir = Vector3.zero;
while (next != null)
{
var dir = next.position - current.position;
if ((dir != prevDir) || (next is IPortalNode))
{
reversePath.Add(current);
prevDir = dir;
bends++;
}
current = next;
next = current.predecessor;
}
//Correct the end nodes and inject a mid point if too much was pruned (can happen on straight paths with no direction change, which can lead to obstacle collision if the unit is offset)
if (reversePath.Count == 0)
{
reversePath.Add(new Position(request.to));
}
else
{
reversePath[0] = new Position(request.to);
}
if (reversePath.Count == 1 && bends <= 0)
{
reversePath.Add(goal.predecessor);
}
reversePath.Add(new Position(request.from));
int pathLength = reversePath.Count;
//Next see if we can reduce the path further by excluding unnecessary bends
if (!request.pathFinderOptions.preventDiagonalMoves)
{
var matrix = goal.parent;
int indexLimit = reversePath.Count - 2;
for (int i = 0; i < indexLimit; i++)
{
var c1 = reversePath[i];
var c2 = reversePath[i + 1];
var c3 = reversePath[i + 2];
var skip = AdjustIfPortal(c1, c2, c3);
if (skip > -1)
{
//One of the candidate nodes is a portal so skip to the node following the portal and resolve the grid at the other end of the portal.
//Since a portal node will never be the last node we can safely do this here. Since we are moving in the reverse direction here the portal will be on the other side.
i += skip;
matrix = ((IPortalNode)reversePath[i]).parent;
continue;
}
while (CanReducePath(c1, c3, unitProps, matrix, costStrategy))
{
reversePath[i + 1] = null;
pathLength--;
i++;
if (i >= indexLimit)
{
break;
}
c3 = reversePath[i + 2];
if (c3 is IPortalNode)
{
break;
}
}
}
}
//Construct the final path
var path = new Path(pathLength);
var count = reversePath.Count;
for (int i = 0; i < count; i++)
{
var node = reversePath[i];
if (node != null)
{
path.Push(node);
}
}
return path;
}
/// <summary>
/// Determines whether an unobstructed path exists between two points.
/// </summary>
/// <param name="from">The from point.</param>
/// <param name="to">The to point.</param>
/// <param name="unitProps">The unit properties to test against (Walkability).</param>
/// <param name="matrix">The matrix where both <paramref name="from"/> and <paramref name="to"/> must be part of.</param>
/// <param name="costStrategy">The cost strategy.</param>
/// <returns><c>true</c> if an unobstructed path exists between the two points; otherwise <c>false</c>.</returns>
public static bool CanReducePath(IPositioned from, IPositioned to, IUnitProperties unitProps, CellMatrix matrix, ICellCostStrategy costStrategy)
{
//The reference cell is always the from cell. The fact the points are swapped to simplify tangents does not affect this.
var refCell = matrix.GetCell(from.position, false);
Vector3 p1;
Vector3 p2;
//Assign the points so we start with the point with the lowest x-value to simplify things
if (from.position.x > to.position.x)
{
p1 = to.position;
p2 = from.position;
}
else
{
p1 = from.position;
p2 = to.position;
}
var unitRadius = unitProps.radius;
var tan = Tangents.Create(p1, p2, unitRadius, matrix);
var slopeDir = tan.slopeDir;
var cellSize = matrix.cellSize;
var halfCell = cellSize * 0.5f;
//Get the start and end cells, get the cost of the actual start and end, and then reassign the start and end to accommodate for unit size.
var startCell = matrix.GetCell(p1, false);
var startCost = costStrategy.GetCellCost(startCell, unitProps);
startCell = matrix.GetCell(new Vector3(p1.x - unitRadius, p1.y, p1.z), true);
var endCell = matrix.GetCell(p2, false);
var endCost = costStrategy.GetCellCost(endCell, unitProps);
endCell = matrix.GetCell(new Vector3(p2.x + unitRadius, p2.y, p2.z), true);
//The movement across the x-axis
float minXCoord = (startCell.position.x - matrix.start.x) - halfCell;
float maxXCoord = (startCell.position.x - matrix.start.x) + halfCell;
int minZ;
int maxZ;
if (slopeDir < 0)
{
var distLowerCellBoundary = halfCell - (endCell.position.z - p2.z);
var minOverlap = Mathf.CeilToInt((unitRadius - distLowerCellBoundary) / cellSize);
minZ = Math.Max(0, endCell.matrixPosZ - Math.Max(0, minOverlap));
var distUpperCellBoundary = halfCell - (p1.z - startCell.position.z);
var maxOverlap = Mathf.CeilToInt((unitRadius - distUpperCellBoundary) / cellSize);
maxZ = Math.Min(matrix.rows - 1, startCell.matrixPosZ + Math.Max(0, maxOverlap));
}
else
{
var distLowerCellBoundary = halfCell - (startCell.position.z - p1.z);
var minOverlap = Mathf.CeilToInt((unitRadius - distLowerCellBoundary) / cellSize);
minZ = Math.Max(0, startCell.matrixPosZ - Math.Max(0, minOverlap));
var distUpperCellBoundary = halfCell - (p2.z - endCell.position.z);
var maxOverlap = Mathf.CeilToInt((unitRadius - distUpperCellBoundary) / cellSize);
maxZ = Math.Min(matrix.rows - 1, endCell.matrixPosZ + Math.Max(0, maxOverlap));
}
int startX = startCell.matrixPosX;
int endX = endCell.matrixPosX;
bool isVertical = tan.isVertical;
var cellMatrix = matrix.rawMatrix;
for (int x = startX; x <= endX; x++)
{
int startZ;
int endZ;
if (isVertical)
{
startZ = Math.Min(startCell.matrixPosZ, endCell.matrixPosZ);
endZ = Math.Max(startCell.matrixPosZ, endCell.matrixPosZ);
}
else
{
if (slopeDir < 0)
{
startZ = Math.Max((int)(tan.LowTangent(maxXCoord) / cellSize), minZ);
endZ = Math.Min((int)(tan.HighTangent(minXCoord) / cellSize), maxZ);
}
else
{
startZ = Math.Max((int)(tan.LowTangent(minXCoord) / cellSize), minZ);
endZ = Math.Min((int)(tan.HighTangent(maxXCoord) / cellSize), maxZ);
}
}
for (int z = startZ; z <= endZ; z++)
{
var intermediary = cellMatrix[x, z];
if (!isVertical && tan.IsOutsideSecants(intermediary.position))
{
continue;
}
var intermediaryCost = costStrategy.GetCellCost(intermediary, unitProps);
if (!intermediary.IsWalkableFrom(refCell, unitProps) || (startCost < intermediaryCost || endCost < intermediaryCost))
{
return(false);
}
}
minXCoord = maxXCoord;
maxXCoord = maxXCoord + cellSize;
}
return(true);
}
/// <summary>
/// Smooths a path.
/// </summary>
/// <param name="goal">The goal node of the calculated path.</param>
/// <param name="maxPathLength">Maximum length of the path.</param>
/// <param name="request">The path request.</param>
/// <param name="costStrategy">The cell cost provider.</param>
/// <returns>
/// The path in smoothed form
/// </returns>
public Path Smooth(IPathNode goal, int maxPathLength, IPathRequest request, ICellCostStrategy costStrategy)
{
var unitProps = request.requesterProperties;
//Next prune superfluous path nodes
var reversePath = new List <IPositioned>(maxPathLength);
var current = goal;
var next = current.predecessor;
int bends = -1;
var prevDir = Vector3.zero;
while (next != null)
{
var dir = next.position - current.position;
if ((dir != prevDir) || (next is IPortalNode))
{
reversePath.Add(current);
prevDir = dir;
bends++;
}
current = next;
next = current.predecessor;
}
//Correct the end nodes and inject a mid point if too much was pruned (can happen on straight paths with no direction change, which can lead to obstacle collision if the unit is offset)
if (reversePath.Count == 0)
{
reversePath.Add(new Position(request.to));
}
else
{
reversePath[0] = new Position(request.to);
}
if (reversePath.Count == 1 && bends <= 0)
{
reversePath.Add(goal.predecessor);
}
reversePath.Add(new Position(request.from));
int pathLength = reversePath.Count;
//Next see if we can reduce the path further by excluding unnecessary bends
var startGrid = GridManager.instance.GetGrid(request.from);
if (!request.pathFinderOptions.preventDiagonalMoves && startGrid != null)
{
var matrix = startGrid.cellMatrix;
int indexLimit = 2;
for (int i = reversePath.Count - 1; i >= indexLimit; i--)
{
var c1 = reversePath[i];
var c2 = reversePath[i - 1];
var c3 = reversePath[i - 2];
var skip = AdjustIfPortal(c1, c2, c3);
if (skip > -1)
{
//One of the candidate nodes is a portal so skip to the node following the portal and resolve the grid at the other end of the portal.
//Since a portal node will never be the last node we can safely do this here. Since we are moving in the reverse direction here the portal will be on the other side.
i -= skip;
matrix = ((IPortalNode)reversePath[i]).partner.parent;
continue;
}
while (CanReducePath(c1, c3, unitProps, matrix, costStrategy))
{
reversePath[i - 1] = null;
pathLength--;
i--;
if (i < indexLimit)
{
break;
}
c3 = reversePath[i - 2];
if (c3 is IPortalNode)
{
break;
}
}
}
}
//Construct the final path
var path = new Path(pathLength);
var count = reversePath.Count;
for (int i = 0; i < count; i++)
{
var node = reversePath[i];
if (node != null)
{
path.Push(node);
}
}
return(path);
}
private static bool CanReducePath(IPositioned point1, IPositioned point3, IUnitProperties unitProps, CellMatrix matrix, ICellCostStrategy costStrategy)
{
Vector3 p1;
Vector3 p3;
//Assign the points so we start with the point with the lowest x-value to simplify things
if (point1.position.x > point3.position.x)
{
p1 = point3.position;
p3 = point1.position;
}
else
{
p1 = point1.position;
p3 = point3.position;
}
var requesterRadius = unitProps.radius;
var tan = Tangents.Create(p1, p3, requesterRadius);
var incZ = tan.slopeDir;
var cellSize = matrix.cellSize;
var halfCell = cellSize / 2.0f;
//Adjust the start and end cells to possibly include their immediate neighbour if the unit's radius crossed said boundary.
var radiusAdjust = new Vector3(requesterRadius, 0.0f, requesterRadius * incZ);
//Get the start and end cells, get the cost of the actual start and end, and then reassign the start and end with the above adjustment.
var startCell = matrix.GetCell(p1, true);
var startCost = costStrategy.GetCellCost(startCell, unitProps);
startCell = matrix.GetCell(p1 - radiusAdjust, true);
var endCell = matrix.GetCell(p3, true);
var endCost = costStrategy.GetCellCost(endCell, unitProps);
endCell = matrix.GetCell(p3 + radiusAdjust, true);
//We want x to end up on cell boundaries, the first of which is this far from the first points position
var xAdj = p1.x + (startCell.position.x - p1.x) + halfCell;
//We want to adjust z so that we correctly count impacted cells, this adjusts z so it starts at the bottom boundary of the first cell (for purposes of calculation)
var zAdj = p1.z - (halfCell + ((p1.z - startCell.position.z) * incZ));
//The movement across the x-axis
float deltaX = 0.0f;
var cellMatrix = matrix.rawMatrix;
int indexX = 0;
for (int x = startCell.matrixPosX; x <= endCell.matrixPosX; x++)
{
//So instead of just checking all cells in the bounding rect defined by the two cells p1 and p3,
//we limit it to the cells immediately surrounding the proposed line (tangents), including enough cells that we ensure the unit will be able to pass through,
//at the extreme routes between the two cells (i.e top corner to top corner and bottom corner to bottom corner
int startZ;
int endZ;
//If the tangents are horizontal or vertical z range is obvious
if (tan.isAxisAligned)
{
startZ = startCell.matrixPosZ;
endZ = endCell.matrixPosZ + incZ;
}
else
{
if (indexX == 0)
{
startZ = startCell.matrixPosZ;
}
else
{
var startCellsPassed = Mathf.FloorToInt((tan.LowTangent(deltaX) - zAdj) / cellSize) * incZ;
startZ = LimitStart(
startCell.matrixPosZ + startCellsPassed,
startCell.matrixPosZ,
incZ);
}
//The movement this step will perform across the x-axis
deltaX = xAdj + (indexX * cellSize);
var endCellsIntercepted = Mathf.FloorToInt((tan.HighTangent(deltaX) - zAdj) / cellSize) * incZ;
endZ = LimitEnd(
startCell.matrixPosZ + endCellsIntercepted,
endCell.matrixPosZ,
incZ) + incZ;
}
indexX++;
for (int z = startZ; z != endZ; z += incZ)
{
var intermediary = cellMatrix[x, z];
var intermediaryCost = costStrategy.GetCellCost(intermediary, unitProps);
if (!intermediary.isWalkableFrom(startCell, unitProps) || (startCost < intermediaryCost && endCost < intermediaryCost))
{
return(false);
}
}
}
return(true);
}
