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"); }