private IEnumerator FindPath(Vector2 startPos, Vector2 targetPos)
{
//Diagnostic tools
Stopwatch sw = new Stopwatch();
sw.Start();
Vector2[] waypoints = new Vector2[0];
bool pathSuccess = false;
//Initialize the open set and closed set and stick
PathNode startNode = pathGrid.NodeFromWorldPoint(startPos);
PathNode targetNode = pathGrid.NodeFromWorldPoint(targetPos);
if (startNode.Traversable && targetNode.Traversable)
{
//List<PathNode> openSet = new List<PathNode>();
Heap <PathNode> openSet = new Heap <PathNode>(pathGrid.MaxSize);
HashSet <PathNode> closedSet = new HashSet <PathNode>();
openSet.Add(startNode);
//Main loop for search algo
while (openSet.Count > 0)
{
/*
* Causing issues when replaced with heap - stack overflows
*
* Note from a youtube comment: duh!!!
* +ReMi001 In the Node class, when you implement HeapIndex, be very careful and in get and set write heapIndex, not HeapIndex.
*
*/
PathNode currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
sw.Stop();
print("Path found: " + sw.ElapsedMilliseconds + "ms");
pathSuccess = true;
break;
}
foreach (PathNode neighbour in pathGrid.FindNeighbours(currentNode))
{
if (!neighbour.Traversable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.GCost + this.CalculateDistance(currentNode, neighbour) + neighbour.MovementPenalty;
if (newMovementCostToNeighbour < neighbour.GCost || !openSet.Contains(neighbour))
{
neighbour.GCost = newMovementCostToNeighbour;
neighbour.HCost = this.CalculateDistance(currentNode, targetNode);
neighbour.Parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
}
yield return(null); //Makes the co-routine wait one frame before returning?
if (pathSuccess)
{
waypoints = this.RetracePath(startNode, targetNode);
}
requestManager.FinishedProcessingPath(waypoints, pathSuccess);
}