IEnumerator CreatePath(Vector3 startPos, Vector3 targetPos) { NavNode startNode = grid.GetNodeFromWorld(startPos); NavNode targetNode = grid.GetNodeFromWorld(targetPos); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Debug.Log("Creating Path"); if (startNode.walkable && targetNode.walkable) { Debug.Log("Creating Path inside first check"); Heap <NavNode> openSet = new Heap <NavNode>(grid.MaxSize); HashSet <NavNode> closedSet = new HashSet <NavNode>(); openSet.Add(startNode); while (openSet.Count > 0) { NavNode currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { Debug.Log("Path Found!"); pathSuccess = true; break; } foreach (NavNode neighbor in grid.GetNeighbors(currentNode)) { if (!neighbor.walkable || 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 (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } PathRequestManager.Instance.ProcessingComplete(waypoints, pathSuccess); }