public void FindPath(PathRequestData requestData, Action <PathResultData> callbackAction)
{
Stopwatch sw = new Stopwatch(); /* Used in order to see the performance of the path-finding system before/after heap tree optimization.
* Stopwatch creates some kind of overhead so disable it in the final build.*/
sw.Start();
// CallbackPathResult elements
Vector2[] pathWayPoints = new Vector2[0];
bool pathSuccess = false;
#region A* Algorithm
// Find the start and end nodes on the current grid system
Node startNode = _gridSystem.NodeFromWorldPoint(requestData.PathStart);
Node targetNode = _gridSystem.NodeFromWorldPoint(requestData.PathEnd);
// Only do calculations if nodes are walkable
if (/*startNode.Walkable &&*/ targetNode.Walkable)
{
HeapTree <Node> openNodeSet = new HeapTree <Node>(_gridSystem.MaxHeapSize); // Binary Heap Tree for A* iteration - optimization
HashSet <Node> closedNodeSet = new HashSet <Node>(); // Hash Set for the closed node set of the A* algorithm
//
openNodeSet.AddItem(startNode); // Begin with the starting node
while (openNodeSet.HeapTreeSize > 0) // While the open node set is not empty
{
Node currentNode = openNodeSet.RemoveFirst(); // First in heap tree is the one with the lowest score.
closedNodeSet.Add(currentNode); // Transfer this node from open set to closed set
if (currentNode == targetNode) // PathResult status check - if the target node is reached
{
sw.Stop();
print("Path found in : " + sw.ElapsedMilliseconds + " mili-seconds ");
pathSuccess = true;
break; // PathResult has been found. Exit the while loop
} // If not found start neighbor cost calculations
// List of the adjacent neighboring nodes. They are pre-calculated and stored in Nodes - optimization (Heap -> Stack Memory Allocation)
List <Node> neighborNodeSet = currentNode.NeighborNodes; // Get the pre-computed neighbor nodes
for (int i = 0; i < neighborNodeSet.Count; i++)
{
if (!neighborNodeSet[i].Walkable || closedNodeSet.Contains(neighborNodeSet[i]))
{
continue; // Non-walkable nodes and already closed nodes are passed
}
// New movement G cost with the path through current node
int movementCostToNeighbor = currentNode.GCost + GetDistance(currentNode, neighborNodeSet[i]);
// If the new distance from starting node is shorter and open node set does not contain this neighbor
if (movementCostToNeighbor < neighborNodeSet[i].GCost || !openNodeSet.ContainsItem(neighborNodeSet[i]))
{
neighborNodeSet[i].GCost = movementCostToNeighbor; // Update the distance from starting node with its new path
neighborNodeSet[i].HCost = GetDistance(neighborNodeSet[i], targetNode);
neighborNodeSet[i].ParentNode = currentNode; // Set the new parent of the node for final path detection
if (!openNodeSet.ContainsItem(neighborNodeSet[i])) // If it is not in the open set, add it to the set
{
openNodeSet.AddItem(neighborNodeSet[i]);
}
else
{
openNodeSet.UpdateItem(neighborNodeSet[i]); // If already in the set then the values are changed
}
}
}
}
}
#endregion
// If an available path is found
if (pathSuccess)
{
pathWayPoints = ReTracePath(startNode, targetNode); // Find the path way-points by re-tracing parent nodes
pathSuccess = pathWayPoints.Length > 0; // Set path to success if there is at least one way-point
}
// Trigger the callback, path is found. This will trigger the queue in the path-finding request manager
callbackAction(new PathResultData(pathWayPoints, _wayPointNodesCache, pathSuccess, requestData.CallbackPathRequest));
}
