C# Heap.RemoveFirst方法代码示例

	IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) 
	{
		
		Stopwatch sw = new Stopwatch();
		sw.Start();
		
		Vector3[] waypoints = new Vector3[0];
		bool pathSuccess = false;
		
		Node startNode = grid.NodeFromWorldPoint(startPos);
		Node targetNode = grid.NodeFromWorldPoint(targetPos);
		
		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) 
				{
					sw.Stop();
					print ("Path found: " + sw.ElapsedMilliseconds + " ms");
					pathSuccess = true;
					break;
				}
				
				foreach (Node neighbour in grid.GetNeighbours(currentNode)) 
				{
					if (!neighbour.walkable || closedSet.Contains(neighbour)) 
					{
						continue;
					}
					
					int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
					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.FinishedProcessingPath(waypoints,pathSuccess);
		
	}

        /// <summary>
        /// De hoofdfunctie van de pathfinding.
        /// </summary>
        /// <param name="a">Start positie als AstarObject</param>
        /// <param name="b">Eind positie als AstarObject</param>
        /// <param name="T"> Het type weg waarin hij moet zoeken</param>
        /// <returns></returns>
        List<Point> FindRoadPath(Road a, Road b, RoadType T)
        {
            AstarObject[,] Set = new AstarObject[14, 9];
            for (int x = 0; x < 14; x++)
            {
                for (int y = 0; y < 9; y++)
                {
                    Set[x, y] = new AstarObject(x, y, this);
                }
            }

            Heap<AstarObject> OpenSet = new Heap<AstarObject>(14 * 9);
            HashSet<AstarObject> ClosedSet = new HashSet<AstarObject>();
            AstarObject Start = Set[a.X, a.Y];
            AstarObject End = Set[b.X, b.Y];
            OpenSet.Add(Start);

            while (OpenSet.Count > 0)
            {
                AstarObject CurrentLocation = OpenSet.RemoveFirst();

                ClosedSet.Add(CurrentLocation);

                if (CurrentLocation == End)
                {
                    return RetracePath(Start, End);
                    //Retracepath and stuff.
                }

                List<AstarObject> Neighbours = GetNeighbours(CurrentLocation, ref Set, NeighbourhoodType.Neumann, MapsizeXR, MapsizeYR);
                foreach (AstarObject neighbour in Neighbours)
                {
                    if (neighbour.RType != T || ClosedSet.Contains(neighbour))
                    {
                        continue;
                    }

                    int newMovementCostToNeighbour = CurrentLocation.gCost + GetDistance(CurrentLocation, neighbour);
                    if (newMovementCostToNeighbour < neighbour.gCost || !OpenSet.Contains(neighbour))
                    {
                        neighbour.gCost = newMovementCostToNeighbour;
                        neighbour.hCost = GetDistance(neighbour, End);
                        neighbour.parent = CurrentLocation;

                        if (!OpenSet.Contains(neighbour))
                        {
                            OpenSet.Add(neighbour);
                        }
                        else
                        {
                            OpenSet.UpdateItem(neighbour);
                        }

                    }

                }

            }
            return new List<Point>();
        }

    public void aStar(Vector3 startPos, Vector3 endPos)
    {
        Stopwatch watch = new Stopwatch();
        watch.Start();

        //List<Node_K> openSet = new List<Node_K>();
        Heap<Node_K> openSet = new Heap<Node_K>(AIarea.GetMaxSize);
        HashSet<Node_K> closedSet = new HashSet<Node_K>();
        Node_K sourceNode = AIarea.getNodeAtPos(startPos);
        Node_K targetNode = AIarea.getNodeAtPos(endPos);
        openSet.Add(sourceNode);

        while(openSet.Count > 0)
        {
            Node_K currentNode = openSet.RemoveFirst();
            //Node_K currentNode = openSet[0];
            //for (int i = 1; i < openSet.Count; i++)
            //{
            //    if (openSet[i].fCost < currentNode.fCost || openSet[i].fCost == currentNode.fCost && openSet[i].hCost < currentNode.hCost)
            //    {
            //        currentNode = openSet[i];
            //    }
            //}

            //openSet.Remove(currentNode);
            closedSet.Add(currentNode);

            if(currentNode == targetNode)
            {
                reversePath(sourceNode, targetNode);
                watch.Stop();
                time = watch.ElapsedMilliseconds.ToString();
                print("Astar " + watch.ElapsedMilliseconds + "ms");
                return;
            }

            List<Node_K> neighbors = new List<Node_K>();
            neighbors = AIarea.neighbors(currentNode);
            for (int i = 0; i < neighbors.Count; i++)
            {
                if (neighbors[i].walkable == false || closedSet.Contains(neighbors[i]))
                    continue;

                int cost = currentNode.gCost + moveCost(currentNode, neighbors[i]);
                if (!openSet.Contains(neighbors[i]) || (cost < neighbors[i].gCost))
                {
                    neighbors[i].gCost = cost;
                    neighbors[i].hCost = moveCost(neighbors[i], targetNode);
                    neighbors[i].parent = currentNode;

                    if (!openSet.Contains(neighbors[i]))
                        openSet.Add(neighbors[i]);
                    else
                        openSet.UpdateItem(neighbors[i]);
                }
            }
        }
    }

    //Generate path from start to destination using A*
    public Path findPath(Vector3 startPos, Vector3 targetPos, int steps) {
        //Convert 3D Positions to GridNodes
        GridNode startNode = grid.getNode(startPos);
        GridNode targetNode = grid.getNode(targetPos);
        //Exit if nodes are out of bounds
        if (startNode == null || targetNode == null)
        {
            return null;
        }

        //Instantiate Open and Closed set DataStructures for A* Algorithm
        HashSet<GridNode> closedSet = new HashSet<GridNode>();
        Heap<GridNode> openSet = new Heap<GridNode>(grid.MaxSize);        
        openSet.Add(startNode); //Add start node to the open set

        int count = 0;

        //Iterate through as long as openset is not empty
        while(openSet.Count > 0 && count++ < steps) {
            //Store first node in collection 
            GridNode currentNode = openSet.RemoveFirst();
            closedSet.Add(currentNode);

            //Check if arrived at target node
            if(currentNode == targetNode) {
                //Retrace and get coordinates
                List<GridNode> points = retracePath(startNode, targetNode);
                //Convert to a path
                return getPath(points);
            }

            //Iterate through all neighbor nodes
            foreach(GridNode neighbor in grid.GetNeighbors(currentNode)) {
                //Skip node if not walkable or already used
                if (!neighbor.isWalkable() || closedSet.Contains(neighbor))
                {
                    continue;
                }

                //Calculate the move cost to the neighbor nodes
                int moveCost = currentNode.gCost + GridNode.getDistance(currentNode, neighbor);

                //Calculate neighbor's costs
                if(moveCost < neighbor.gCost || !openSet.Contains(neighbor)) {
                    //Set Costs
                    neighbor.gCost = moveCost;
                    neighbor.hCost = GridNode.getDistance(neighbor, targetNode);
                    neighbor.parent = currentNode;

                    //If the open set doesn't already contain the neighboring node, add it
                    if (!openSet.Contains(neighbor))
                        openSet.Add(neighbor);
                }
            }
        }
        //Unable to find a path
        return null;
    }

    IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
    {
        Vector3[] waypoints = new Vector3[0];
        bool pathSuccess = false;

        Node startNode = grid.NodeFromWorlPoint (startPos);
        Node targetNode = grid.NodeFromWorlPoint (targetPos);

        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)
                {
                    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.FinishedProcessingPath(waypoints, pathSuccess);
        } else {
            requestManager.FinishedProcessingPath(waypoints, pathSuccess);
            yield break;
        }
    }

	//A* implementation
	public static IEnumerator CalculatePath(PathNode startNode, PathNode endNode, PathNode[] allNodes, Action<Vector2[], bool> callback){

		#if DEBUG
		Stopwatch sw = new Stopwatch();
		sw.Start();
		#endif

		var openList = new Heap<PathNode>(allNodes.Length);
		var closedList = new HashSet<PathNode>();
		var success = false;

		openList.Add(startNode);

		while (openList.Count > 0){

			var currentNode = openList.RemoveFirst();
			closedList.Add(currentNode);

			if (currentNode == endNode){
				
				#if DEBUG
				sw.Stop();
				//UnityEngine.Debug.Log("Path Found: " + sw.ElapsedMilliseconds + " ms.");
				#endif

				success = true;
				break;
			}

			
			foreach (var neighbour in currentNode.links.Select( index => allNodes[index] )){

				if (closedList.Contains(neighbour))
					continue;

				var costToNeighbour = currentNode.gCost + GetDistance( currentNode, neighbour );
				if (costToNeighbour < neighbour.gCost || !openList.Contains(neighbour) ){
					neighbour.gCost = costToNeighbour;
					neighbour.hCost = GetDistance(neighbour, endNode);
					neighbour.parent = currentNode;

					if (!openList.Contains(neighbour)){
						openList.Add(neighbour);
						openList.UpdateItem(neighbour);
					}
				}
			}
		}

		yield return null;
		if (success){
			callback( RetracePath(startNode, endNode), true );
		} else {
			callback( new Vector2[0], false );
		}
	}

    Vector2[] FindPath(Vector2 from, Vector2 to)
    {
        Stopwatch sw = new Stopwatch();
        sw.Start();

        Vector2[] waypoints = new Vector2[0];
        bool pathSuccess = false;

        Node startNode = grid.NodeFromWorldPoint(from);
        Node targetNode = grid.NodeFromWorldPoint(to);
        startNode.parent = startNode;

        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) {
                    sw.Stop();
                    print ("Path found: " + sw.ElapsedMilliseconds + " ms");
                    pathSuccess = true;
                    break;
                }

                foreach (Node neighbour in grid.GetNeighbours(currentNode)) {
                    if (!neighbour.walkable || closedSet.Contains(neighbour)) {
                        continue;
                    }

                    int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour)+TurningCost(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);
                    }
                }
            }
        }

        if (pathSuccess) {
            waypoints = RetracePath(startNode,targetNode);
        }

        return waypoints;
    }

    void FindPath(Vector2 startPos, Vector2 targetPos)
    {
        Stopwatch sw = new Stopwatch();
        sw.Start();

        Node startNode = grid.NodeFromWorldPoint(startPos);
        Node targetNode = grid.NodeFromWorldPoint(targetPos);

        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){
                sw.Stop();
                print("Path found" + sw.ElapsedMilliseconds + "ms");
                RetracePath(startNode,targetNode);

                return;

            }

            //check if neighbour is walkable
            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);

                }

            }
        }
    }

    IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
    {
        //Create and start stopwatch to log method completion time
        Stopwatch sw = new Stopwatch();
        sw.Start();

        //Initialize waypoints array
        Vector3[] waypoints = new Vector3[0];
        bool pathSuccess = false;

        //Determine the start and target node based on the Gameobjects Vector 3 positions
        Node startNode = grid.NodeFromWorldPoint(startPos);
        Node targetNode = grid.NodeFromWorldPoint(targetPos);
        if(startNode.walkable && targetNode.walkable) {
            Heap<Node> openSet = new Heap<Node>(grid.MaxSize); //Holds all the nodes in the grid
            HashSet<Node> closedSet = new HashSet<Node>(); //Used to store nodes that are
            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");
                    pathSuccess = true;
                    break;
                }

                foreach(Node neighbour in grid.GetNeighbours(currentNode)) {
                    if(!neighbour.walkable || closedSet.Contains(neighbour)) {
                        continue;
                    }

                    int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
                    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.FinishedProcessingPath(waypoints, pathSuccess);
    }

    public bool FindPathImmediate(GridNode start, GridNode end, List<List<GridNode>> grid, out List<GridNode> path)
    {
        path = new List<GridNode>();
        bool success = false;
        if (start.walkable && end.walkable)
        {
            Heap<GridNode> openSet = new Heap<GridNode>(grid.Count * grid[0].Count);
            HashSet<GridNode> closedSet = new HashSet<GridNode>();
            openSet.Add(start);

            while (openSet.Count > 0)
            {
                GridNode currentNode = openSet.RemoveFirst();
                closedSet.Add(currentNode);

                //path found
                if (currentNode == end)
                {
                    success = true;
                    break;
                }

                foreach (GridNode neighbour in currentNode.GetNeighbours())
                {
                    if (!neighbour.walkable || closedSet.Contains(neighbour))
                        continue;

                    int newNeighbourMoveCost = currentNode.gCost + GetDistanceManhatten(currentNode, neighbour);
                    if (newNeighbourMoveCost < neighbour.gCost || !openSet.Contains(neighbour))
                    {
                        neighbour.gCost = newNeighbourMoveCost;
                        neighbour.hCost = GetDistanceManhatten(neighbour, end);
                        neighbour.parent = currentNode;

                        if (!openSet.Contains(neighbour))
                            openSet.Add(neighbour);
                        else
                            openSet.UpdateItem(neighbour);
                    }
                }
            }
        }
        if (success)
        {
            path = RetracePath(start, end);
        }

        return success;
    }

    void CreatePath(Node startNode, Node targetNode)
    {
        bool pathSuccess = false;
        int traffic;
        int trafficCap;
        openSet = new Heap<Node>(grid.RoadSet.Count);
        closedSet = new HashSet<Node>();
        openSet.Add(startNode);
        
        while (openSet.Count > 0)
        {
            // remove the smallest fCost from Heap
            Node currentNode = openSet.RemoveFirst();
            closedSet.Add(currentNode);

            if (currentNode == targetNode)
            {
                pathSuccess = true;
                break;
            }
            
            foreach (Node neighbour in grid.PathGetRoadNeighbours(currentNode))
            {
                if (closedSet.Contains(neighbour))
                    continue;

                lock (currentNode)
                    traffic = currentNode.trafficTimer.TrafficIntensity;
                trafficCap = currentNode.trafficTimer.InitialTrafficIntensity;
                int penalty = 17 - 17 * traffic / (trafficCap + 17/trafficCap);

                int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbour) + penalty;
                
                if (newMovementCostToNeighbor < neighbour.gCost || !openSet.Contains(neighbour))
                {
                    neighbour.gCost = newMovementCostToNeighbor;
                    neighbour.hCost = GetDistance(neighbour, targetNode);
                    neighbour.parent = currentNode;


                    if (!openSet.Contains(neighbour))
                        openSet.Add(neighbour);
                    else
                        openSet.UpdateItem(neighbour);
                }
            }
        }
        requestManager.FinishedProcessingPath(ReverseParents(startNode, targetNode), pathSuccess);
    }

	IEnumerator FindPath(Vector3 startPosition, Vector3 targetPosition)//here we get 2 positions from the game and check on wich node they are, we do that with NodeFromWorldPoint wich we have in the grid script
    {
		Vector3[] waypoints = new Vector3[0];
		bool pathSuccess = false;

		Node startNode = grid.NodeFromWorldPoint(startPosition);//this is going to be the startng point f the path
        Node targetNode = grid.NodeFromWorldPoint(targetPosition);//this wil be the  base or player etc, just the target

        if (startNode.walkable && targetNode.walkable) {
			Heap<Node> openSet = new Heap<Node>(grid.MaxSize);//list of nodes that havent been checked yet
            HashSet<Node> closedSet = new HashSet<Node>();//a list for the nodes that have been checked
            openSet.Add(startNode);//the verry first node that wil be checked, this is the starting position

            while (openSet.Count > 0)//as long as there are nodes to be checked, go on :)
            {
				Node currentNode = openSet.RemoveFirst();//first to be checked a
                closedSet.Add(currentNode);

				if (currentNode == targetNode)
                {
					pathSuccess = true;
					break;
				}

				foreach (Node neighbour in grid.GetNeighbours(currentNode)) {
					if (!neighbour.walkable || closedSet.Contains(neighbour)) {
						continue;
					}

					int newCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
					if (newCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) {
						neighbour.gCost = newCostToNeighbour;
						neighbour.hCost = GetDistance(neighbour, targetNode);
						neighbour.parent = currentNode;

						if (!openSet.Contains(neighbour))
							openSet.Add(neighbour);
					}
				}
			}
		}
		yield return null;
		if (pathSuccess) {
			waypoints = RetracePath(startNode,targetNode);
		}
		requestManager.FinishedProcessingPath(waypoints,pathSuccess);

	}

	void FindPath(Vector3 startPos, Vector3 targetPos) { 
		Stopwatch sw = new Stopwatch ();
		sw.Start ();
		Node startNode = grid.NodeFromWorldPoint (startPos);
		Node targetNode = grid.NodeFromWorldPoint (targetPos);
		//List<Node> openSet = new List<Node>();
		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();
		//	for (int i = 1; i < openSet.Count; i++) {
		//		if (openSet [i].fCost < currentNode.fCost || 
	//				openSet[i].fCost == currentNode.fCost && 
	//				openSet[i].hCost < currentNode.hCost) {
	//				currentNode = openSet [i];
	//			}
	//		}
	//		openSet.Remove (currentNode);
			closedSet.Add (currentNode);
			if (currentNode == targetNode) {
				// retrace steps to get the path using parent
				sw.Stop();
				print("Path found in " + sw.ElapsedMilliseconds + " ms");
				RetracePath(startNode,targetNode);
				return;
			}
			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);
				}
			}
		}
		
	}

    IEnumerator CalculatePath(Vector3 startPosition, Vector3 targetPosition, LongPathGrid longPathGrid)
    {
        Vector3[] waypoints = new Vector3[0];
        bool pathSuccess = false;

        GridNode startNode = longPathGrid.GetGridNodeFromWorldPoint(startPosition);
        GridNode targetNode = longPathGrid.GetGridNodeFromWorldPoint(targetPosition);

        if (targetNode.walkable) {
            Heap<GridNode> openSet = new Heap<GridNode>(longPathGrid.MaxSize);
            HashSet<GridNode> closedSet = new HashSet<GridNode>();

            openSet.Add(startNode);

            while(openSet.Count > 0) {
                GridNode currentNode = openSet.RemoveFirst();

                closedSet.Add(currentNode);

                if (currentNode == targetNode) {
                    pathSuccess = true;
                    break;
                }

                foreach (GridNode neighbour in longPathGrid.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);
        navGridManager.FinishProcessingLongPath(waypoints,pathSuccess);
    }

    public List<Portal> FindPath()
    {
        Stopwatch sw = new Stopwatch();
        sw.Start();
        bool success = false;
        CreateStartNode(startPos);
        CreateEndNode(targetPos);
        if (startPortal == null || endPortal == null)
            return null;

        Heap<Portal> frontier = new Heap<Portal>(portals.Count+2);
        HashSet<Portal> visited = new HashSet<Portal>();
        frontier.Add(startPortal);

        while(frontier.size() > 0){
            Portal current = frontier.RemoveFirst();
            visited.Add(current);

            if (current == endPortal){
                sw.Stop();
                success = true;
                print("Path found in: "+ sw.ElapsedMilliseconds + " ms");
                break;
            }

            foreach(KeyValuePair<Portal, int> nb in current.GetNeighbours()){
                if (visited.Contains(nb.Key)){
                    continue;
                }

                int newDistance = current.gCost + nb.Value;
                if (newDistance < nb.Key.gCost || !frontier.Contains(nb.Key)){
                    nb.Key.gCost = newDistance;
                    nb.Key.hCost = CalcDistance(nb.Key);
                    nb.Key.SetParent(current);

                    if (!frontier.Contains(nb.Key)){
                        frontier.Add(nb.Key);
                    }
                }
            }
        }
        if (success){
            return RetracePath();
        }
        else return null;
    }