Pathfinding.BinaryHeap.Remove C# (CSharp) Method

		public GraphNode Remove() {
			numberOfItems--;
			GraphNode returnItem = binaryHeap[1];
			
		 	//returnItem.heapIndex = 0;//Heap index
			
			binaryHeap[1] = binaryHeap[numberOfItems];
			//binaryHeap[1].heapIndex = 1;//Heap index
			
			int swapItem = 1, parent = 1;
			
			do {
				parent = swapItem;
				int p2 = parent * 2;
				if ((p2 + 1) <= numberOfItems) {
					// Both children exist
					if (binaryHeap[parent].f >= binaryHeap[p2].f) {
						swapItem = p2;//2 * parent;
					}
					if (binaryHeap[swapItem].f >= binaryHeap[p2 + 1].f) {
						swapItem = p2 + 1;
					}
				} else if ((p2) <= numberOfItems) {
					// Only one child exists
					if (binaryHeap[parent].f >= binaryHeap[p2].f) {
						swapItem = p2;
					}
				}
				
				// One if the parent's children are smaller or equal, swap them
				if (parent != swapItem) {
					GraphNode tmpIndex = binaryHeap[parent];
					//tmpIndex.heapIndex = swapItem;//Heap index
					
					binaryHeap[parent] = binaryHeap[swapItem];
					binaryHeap[swapItem] = tmpIndex;
					
					//binaryHeap[parent].heapIndex = parent;//Heap index
				}
			} while (parent != swapItem);
			
			return returnItem;
		}
		

        /** Initializes the path. Sets up the open list and adds the first node to it */
        public virtual void Initialize()
        {
            System.DateTime startTime = System.DateTime.Now;

            //Resets the binary heap, don't clear it because that takes an awful lot of time, instead we can just change the numberOfItems in it (which is just an int)
            //Binary heaps are just like a standard array but are always sorted so the node with the lowest F value can be retrieved faster

            open = AstarPath.active.binaryHeap;
            open.numberOfItems = 1;

            if (hasEndPoint && startNode == endNode)
            {
                endNode.parent = null;
                endNode.h      = 0;
                endNode.g      = 0;
                Trace(endNode);
                foundEnd = true;

                return;
            }

            //Adjust the costs for the end node
            if (hasEndPoint && recalcStartEndCosts)
            {
                endNodeCosts = endNode.InitialOpen(open, hTarget, (Int3)endPoint, this, false);
                callback    += ResetCosts;              /** \todo Might interfere with other paths since other paths might be calculated before #callback is called */
            }

            Node.activePath  = this;
            startNode.pathID = pathID;
            startNode.parent = null;
            startNode.cost   = 0;
            startNode.g      = startNode.penalty;
            startNode.UpdateH(hTarget, heuristic, heuristicScale);

            if (recalcStartEndCosts)
            {
                startNode.InitialOpen(open, hTarget, startIntPoint, this, true);
            }
            else
            {
                startNode.Open(open, hTarget, this);
            }

            searchedNodes++;

            //any nodes left to search?
            if (open.numberOfItems <= 1)
            {
                LogError("No open points, the start node didn't open any nodes");

                duration += (System.DateTime.Now.Ticks - startTime.Ticks) * 0.0001F;
                return;
            }

            current = open.Remove();

            duration += (System.DateTime.Now.Ticks - startTime.Ticks) * 0.0001F;
        }