IEnumerator FindPath(Vector3 StartPosition, Vector3 TargetPosition) { Node StartNode = Grid.WorldPointToNode(StartPosition); Node TargetNode = Grid.WorldPointToNode(TargetPosition); Vector3[] waypoints = new Vector3[0]; bool pathsuccess = false; if (StartNode.Walkable && TargetNode.Walkable) { 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) { //path found 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.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, TargetNode); neighbour.Parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } yield return(null); if (pathsuccess) { waypoints = RetracePath(StartNode, TargetNode); } requestManager.FinisedProcessingPath(waypoints, pathsuccess); }