public IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathSucces = false; Node startNode = grids.nodeWorldPos(startPos); Node targetNode = grids.nodeWorldPos(targetPos); if (startNode.canWalk && targetNode.canWalk) { HeapOpt <Node> openSet = new HeapOpt <Node>(grids.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { sw.Stop(); print("Path Found" + sw.ElapsedMilliseconds + "ms"); pathSucces = true; break; } foreach (Node getNodes in grids.GetNodes(currentNode)) { if (!getNodes.canWalk || closedSet.Contains(getNodes)) { continue; } int newMovementCostToNode = currentNode.gCost + getDist(currentNode, getNodes); if (newMovementCostToNode < getNodes.gCost || !openSet.Contains(getNodes)) { getNodes.gCost = newMovementCostToNode; getNodes.hCost = getDist(getNodes, targetNode); getNodes.parent = currentNode; if (!openSet.Contains(getNodes)) { openSet.Add(getNodes); } else { openSet.UpdateItem(getNodes); } } } } yield return(null); if (pathSucces) { waypoints = RetracePath(startNode, targetNode); } requestManager.FinishedProccesingPath(waypoints, pathSucces); } }