IEnumerator HighlightPaths(Vector3 startPos, float magnitude) { HashSet <Node> highlight = new HashSet <Node>(); Node startNode = grid.NodeFromWorldPoint(startPos); 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); Node neighbor; for (int i = 0; i < currentNode.Neighbors.Count; i++) { neighbor = currentNode.Neighbors[i]; if (!neighbor.walkable || neighbor.isOccupied || closedSet.Contains(neighbor)) { continue; } int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor) + neighbor.movementPenalty; if (!highlight.Contains(neighbor) || newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor)) { neighbor.gCost = newMovementCostToNeighbor; neighbor.hCost = 0; neighbor.parent = currentNode; if (newMovementCostToNeighbor > magnitude) { neighbor.gCost = 0; continue; } highlight.Add(neighbor); if (!openSet.Contains(neighbor)) { openSet.Add(neighbor); } else { openSet.UpdateItem(neighbor); } } } } yield return(null); foreach (Node node in closedSet) { node.resetCosts(); } while (openSet.Count > 0) { Node node = openSet.RemoveFirst(); node.resetCosts(); } pathRequestManager.FinishedProcessingHighlight(highlight); }