/// <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[] { });
}
}
/// <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));
}
}