private static void EnqueueIfPassable(ICollection <int> seen, int newCell, int cost,
PriorityDictionary <int, int> queue)
{
if (Grid.IsValidCell(newCell) && Grid.Element[newCell]?.IsSolid == false &&
!seen.Contains(newCell))
{
seen.Add(newCell);
queue.Enqueue(cost, newCell);
}
}
/// <summary>
/// Adds nodes to queue by priority, processing the specified node neighbors
/// </summary>
/// <param name="openPathsQueue">Queue</param>
/// <param name="nodesData">Nodes data dictionary</param>
/// <param name="currentNode">Current node</param>
/// <param name="end">End node</param>
/// <param name="heuristicMethod">Heuristic metod</param>
/// <param name="heuristicEstimateValue">Heuristic estimate value</param>
private static void ProcessNeighbors(PriorityDictionary <GridNode, float> openPathsQueue, Dictionary <GridNode, AStarQueryData> nodesData, GridNode currentNode, GridNode end, HeuristicMethods heuristicMethod, int heuristicEstimateValue)
{
//Search every possible direction from the current node
for (int i = 1; i < currentNode.Connections.Length; i++)
{
var nextNode = currentNode[i];
if (nextNode == null)
{
continue;
}
if (!nodesData.ContainsKey(nextNode))
{
nodesData.Add(nextNode, new AStarQueryData());
}
if (nextNode.State == GridNodeStates.Closed)
{
//Impassable node
continue;
}
var nextNodeData = nodesData[nextNode];
if (nextNodeData.State == GridNodeStates.Closed)
{
//Closed node
continue;
}
float newGone = currentNode.TotalCost + ((int)nextNodeData.State);
if (nextNodeData.State == GridNodeStates.Clear && nextNode.TotalCost < newGone)
{
continue;
}
nextNodeData.NextNode = currentNode;
nextNodeData.Cost = newGone;
nextNodeData.State = GridNodeStates.Clear;
//Calculate priority from next to end
float heuristicValue = CalcHeuristic(
nextNode.Center,
end.Center,
heuristicMethod);
openPathsQueue.Enqueue(nextNode, newGone + (heuristicEstimateValue * heuristicValue));
}
}
protected override void VisualizeCells(ICollection <VisCellData> newCells)
{
// Rotation is only used to rotate the offset, radius is the same in all directions
int startCell = RotateOffsetCell(Grid.PosToCell(gameObject), offset);
if (Grid.IsValidCell(startCell) && Grid.Element[startCell]?.IsSolid == false)
{
var queue = new PriorityDictionary <int, int>(radius * radius);
// Initial cell is seen
var seen = HashSetPool <int, ElementConsumerVisualizer> .Allocate();
try {
queue.Enqueue(0, startCell);
seen.Add(startCell);
// Dijkstra's algorithm
do
{
queue.Dequeue(out int cost, out int newCell);
if (cost < radius - 1)
{
// Cardinal directions
EnqueueIfPassable(seen, Grid.CellLeft(newCell), cost + 1, queue);
EnqueueIfPassable(seen, Grid.CellRight(newCell), cost + 1, queue);
EnqueueIfPassable(seen, Grid.CellAbove(newCell), cost + 1, queue);
EnqueueIfPassable(seen, Grid.CellBelow(newCell), cost + 1, queue);
}
} while (queue.Count > 0);
// Add all cells as normal color
foreach (var cell in seen)
{
newCells.Add(new VisCellData(cell, color));
}
} finally {
seen.Recycle();
}
}
}
/// <summary>
/// Gets the path from start to end
/// </summary>
/// <param name="start">Start node</param>
/// <param name="end">End node</param>
/// <param name="heuristicMethod">Heuristic metod</param>
/// <param name="heuristicEstimateValue">Heuristic estimate value</param>
/// <returns>Returns the path from start to end</returns>
private static Vector3[] CalcReturnPath(GridNode start, GridNode end, HeuristicMethods heuristicMethod, int heuristicEstimateValue)
{
//New queue
PriorityDictionary <GridNode, float> openPathsQueue = new PriorityDictionary <GridNode, float>();
//Data dictionary
Dictionary <GridNode, AStarQueryData> nodesData = new Dictionary <GridNode, AStarQueryData>();
//Add first node
openPathsQueue.Enqueue(start, 1);
nodesData.Add(start, new AStarQueryData());
bool nodeFound = false;
while (openPathsQueue.Count > 0)
{
//Dequeue the node with lower priority
var item = openPathsQueue.Dequeue();
var currentNode = item.Value;
var currentNodeData = nodesData[currentNode];
//If the node is not closed to continue the process
if (currentNodeData.State != GridNodeStates.Closed)
{
//Set the node status Closed
currentNodeData.State = GridNodeStates.Closed;
//If the current node is the destination node has found the way
if (currentNode == end)
{
currentNodeData.State = GridNodeStates.Closed;
nodeFound = true;
break;
}
else
{
//Process neigbors
ProcessNeighbors(
openPathsQueue, nodesData,
currentNode, end,
heuristicMethod, heuristicEstimateValue);
}
}
}
if (nodeFound)
{
//We found a valid path
List <Vector3> solvedList = new List <Vector3>();
var node = end;
while (node != null)
{
solvedList.Insert(0, node.Center);
node = nodesData[node].NextNode;
}
return(solvedList.ToArray());
}
else
{
//If no result...
return(new Vector3[] { });
}
}
