IEnumerator FindPath(Vector3 startPosition, Vector3 targetPosition) { Vector3[] waypoints = new Vector3[0]; bool success = false; Node startNode = grid.NodeFromWorldPoint(startPosition); Node targetNode = grid.NodeFromWorldPoint(targetPosition); Heap <Node> openSet = new Heap <Node>(grid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { success = true; break; } foreach (Node neighbor in grid.GetNeighbors(currentNode)) { if (!neighbor.walkable && neighbor != targetNode || closedSet.Contains(neighbor)) { continue; } int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor); if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor)) { neighbor.gCost = newMovementCostToNeighbor; neighbor.hCost = GetDistance(neighbor, targetNode); neighbor.parent = currentNode; if (!openSet.Contains(neighbor)) { openSet.Add(neighbor); } else { openSet.UpdateItem(neighbor); } } } } yield return(null); if (success) { waypoints = TracePath(startNode, targetNode); } requestManager.FinishProcessing(waypoints, success); }