private static Tuple <Dictionary <Vector3, Vector3>, Vector3?> ConstructPath(Vector3 startCubeCoord, Vector3 endCubeCoord, int rangeLimiter, Vector2 gridSize, Vector3[] impassableCoords)
{
FastPriorityQueue <CubeCoordNode> boundary = new FastPriorityQueue <CubeCoordNode>(100);
boundary.Enqueue(new CubeCoordNode(startCubeCoord), 0); //begin the boundary at the start point
//(Imagine this as an expanding frontier of tiles searched in the direction towards the end point)
Dictionary <Vector3, Vector3> path = new Dictionary <Vector3, Vector3>();
path[startCubeCoord] = Vector3.Zero; //the start coord does not point to anything; this can be any value
Dictionary <Vector3, int> cost = new Dictionary <Vector3, int>();
cost[startCubeCoord] = 0; //initialize cost of path.
Vector3 currentCoord;
Vector3?calculatedDestination = null; //this determines if a path is found to a tile in range, and is used to reconstruct a path into an array of coordinates.
int updatedCost;
while (!(boundary.Count == 0))
{
currentCoord = boundary.Dequeue().cubeCoord;
//early exit if coordinate is found. Can be used as manhattan distance is a good heurstic,
// meaning the first path found should always be the shortest path.
if (CoordRange.CoordInRange(currentCoord, endCubeCoord, rangeLimiter))
{
calculatedDestination = currentCoord; //path found ending in range.
break;
}
foreach (Vector3 neighbour in Neighbours.GetNeighbours(currentCoord, impassableCoords, gridSize))
{
updatedCost = cost[currentCoord] + 1; //calculate a new cost for the current path. (add 1 as movements are of equal value on any tile)
if (!cost.ContainsKey(neighbour) || updatedCost < cost[neighbour])
{
cost[neighbour] = updatedCost;
int priority = updatedCost + ManhattanDistance.GetDistance(neighbour, endCubeCoord);
boundary.Enqueue(new CubeCoordNode(neighbour), priority);
path[neighbour] = currentCoord;
}
}
}
return(new Tuple <Dictionary <Vector3, Vector3>, Vector3?>(path, calculatedDestination));
}