IEnumerator FindPath(Vector3 startPosition, Vector3 targetPosition,Action<Vector3[]> callBack ){
running = true;
yield return new WaitForEndOfFrame();
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = grid.NodeFromWorldPosition(startPosition);
Node targetNode = grid.NodeFromWorldPosition(targetPosition);
if(startNode != targetNode){
if(startNode.walkable & targetNode.walkable){
Heap<Node> openSet = new Heap<Node>(grid.maxHeapSize);
HashSet<Node> closedSet = new HashSet<Node>();
openSet.Add(startNode);
while(openSet.Count >0){
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if(currentNode == targetNode){
pathSuccess = true;
break;
}
foreach(Node neighbour in grid.GetNeighbours(currentNode)){
if(!neighbour.walkable || closedSet.Contains(neighbour)){
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + Getdistance(currentNode,neighbour);
if(newMovementCostToNeighbour < neighbour.gCost || !openSet.Containts(neighbour)){
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = Getdistance(neighbour,targetNode);
neighbour.parent =currentNode;
if(!openSet.Containts(neighbour)){
openSet.Add(neighbour);
}else{
openSet.UpdateItem(neighbour);
}
}
}
}
}
}
if(pathSuccess){
waypoints = RetracePath(startNode,targetNode);
}
callBack(waypoints);
sw.Stop();
running = false;
print("Found path in "+sw.ElapsedMilliseconds+" ms");
}
