/// <summary> /// Removes a node from the queue. The node does not need to be the head of the queue. /// If the node is not in the queue, the result is undefined. If unsure, check Contains() first /// O(log n) /// </summary> public void Remove(PriorityQueueNode <TPriority, TValue> node) { if (node == null) { throw new ArgumentNullException(nameof(node)); } if (!Contains(node)) { throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + node); } //If the node is already the last node, we can remove it immediately if (node.QueueIndex == _numNodes) { _nodes[_numNodes] = null; _numNodes--; return; } //Swap the node with the last node PriorityQueueNode <TPriority, TValue> formerLastNode = _nodes[_numNodes]; _nodes[node.QueueIndex] = formerLastNode; formerLastNode.QueueIndex = node.QueueIndex; _nodes[_numNodes] = null; _numNodes--; //Now bubble formerLastNode (which is no longer the last node) up or down as appropriate OnNodeUpdated(formerLastNode); }
/// <summary> /// Returns (in O(1)!) whether the given node is in the queue. O(1) /// </summary> public bool Contains(PriorityQueueNode <TPriority, TValue> node) { if (node == null) { throw new ArgumentNullException(nameof(node)); } if (node.QueueIndex < 0 || node.QueueIndex >= _nodes.Length) { throw new InvalidOperationException( "node.QueueIndex has been corrupted. Did you change it manually? Or add this node to another queue?"); } return(_nodes[node.QueueIndex] == node); }
void OnNodeUpdated(PriorityQueueNode <TPriority, TValue> node) { //Bubble the updated node up or down as appropriate int parentIndex = node.QueueIndex >> 1; if (parentIndex > 0 && HasHigherPriority(node, _nodes[parentIndex])) { CascadeUp(node); } else { //Note that CascadeDown will be called if parentNode == node (that is, node is the root) CascadeDown(node); } }
/// <summary> /// This method must be called on a node every time its priority changes while it is in the queue. /// <b>Forgetting to call this method will result in a corrupted queue!</b> /// Calling this method on a node not in the queue results in undefined behavior /// O(log n) /// </summary> public void UpdatePriority(PriorityQueueNode <TPriority, TValue> node, TPriority priority) { if (node == null) { throw new ArgumentNullException(nameof(node)); } if (!Contains(node)) { throw new InvalidOperationException( "Cannot call UpdatePriority() on a node which is not enqueued: " + node); } node.Priority = priority; OnNodeUpdated(node); }
/// <summary> /// Resize the queue so it can accept more nodes. All currently enqueued nodes are remain. /// Attempting to decrease the queue size to a size too small to hold the existing nodes results in undefined behavior /// O(n) /// </summary> public void Resize(int maxNodes) { if (maxNodes <= 0) { throw new ArgumentOutOfRangeException(nameof(maxNodes), "Queue size cannot be smaller than 1"); } if (maxNodes < _numNodes) { throw new InvalidOperationException("Called Resize(" + maxNodes + "), but current queue contains " + _numNodes + " nodes"); } var newArray = new PriorityQueueNode <TPriority, TValue> [maxNodes + 1]; int highestIndexToCopy = Math.Min(maxNodes, _numNodes); Array.Copy(_nodes, newArray, highestIndexToCopy + 1); _nodes = newArray; }
void CascadeUp(PriorityQueueNode <TPriority, TValue> node) { //aka Heapify-up int parent; if (node.QueueIndex > 1) { parent = node.QueueIndex >> 1; PriorityQueueNode <TPriority, TValue> parentNode = _nodes[parent]; if (HasHigherPriority(parentNode, node)) { return; } //Node has lower priority value, so move parent down the heap to make room _nodes[node.QueueIndex] = parentNode; parentNode.QueueIndex = node.QueueIndex; node.QueueIndex = parent; } else { return; } while (parent > 1) { parent >>= 1; PriorityQueueNode <TPriority, TValue> parentNode = _nodes[parent]; if (HasHigherPriority(parentNode, node)) { break; } //Node has lower priority value, so move parent down the heap to make room _nodes[node.QueueIndex] = parentNode; parentNode.QueueIndex = node.QueueIndex; node.QueueIndex = parent; } _nodes[node.QueueIndex] = node; }
/// <summary> /// Removes the head of the queue (node with minimum priority; ties are broken by order of insertion), and returns it. /// If queue is empty, result is undefined /// O(log n) /// </summary> public PriorityQueueNode <TPriority, TValue> Dequeue() { if (_numNodes <= 0) { throw new InvalidOperationException("Cannot call Dequeue() on an empty queue"); } if (!IsValidQueue()) { throw new InvalidOperationException( "Queue has been corrupted (Did you update a node priority manually instead of calling " + "UpdatePriority()? Or add the same node to two different queues?)"); } PriorityQueueNode <TPriority, TValue> returnMe = _nodes[1]; //If the node is already the last node, we can remove it immediately if (_numNodes == 1) { _nodes[1] = null; _numNodes = 0; return(returnMe); } //Swap the node with the last node PriorityQueueNode <TPriority, TValue> formerLastNode = _nodes[_numNodes]; _nodes[1] = formerLastNode; formerLastNode.QueueIndex = 1; _nodes[_numNodes] = null; _numNodes--; //Now bubble formerLastNode (which is no longer the last node) down CascadeDown(formerLastNode); return(returnMe); }
/// <summary> /// <para> /// Enqueue a node to the priority queue. Lower values are placed in front. Ties are broken by /// first-in-first-out. /// </para> /// If the queue is full, the result is undefined. If the node is already enqueued, the result is undefined. /// O(log n) /// </summary> public void Enqueue(PriorityQueueNode <TPriority, TValue> node, TPriority priority) { if (node == null) { throw new ArgumentNullException(nameof(node)); } if (_numNodes >= _nodes.Length - 1) { throw new InvalidOperationException("Queue is full - node cannot be added: " + node); } if (Contains(node)) { throw new InvalidOperationException("Node is already enqueued: " + node); } node.Priority = priority; _numNodes++; _nodes[_numNodes] = node; node.QueueIndex = _numNodes; node.InsertionIndex = _numNodesEverEnqueued++; CascadeUp(node); }
/// <summary> /// Returns true if 'higher' has higher priority than 'lower', false otherwise. /// Note that calling HasHigherPriority(node, node) (ie. both arguments the same node) will return false /// </summary> bool HasHigherPriority(PriorityQueueNode <TPriority, TValue> higher, PriorityQueueNode <TPriority, TValue> lower) { int cmp = _comparer(higher.Priority, lower.Priority); return(cmp < 0 || (cmp == 0 && higher.InsertionIndex < lower.InsertionIndex)); }
void CascadeDown(PriorityQueueNode <TPriority, TValue> node) { //aka Heapify-down int finalQueueIndex = node.QueueIndex; int childLeftIndex = 2 * finalQueueIndex; // If leaf node, we're done if (childLeftIndex > _numNodes) { return; } // Check if the left-child is higher-priority than the current node int childRightIndex = childLeftIndex + 1; PriorityQueueNode <TPriority, TValue> childLeft = _nodes[childLeftIndex]; if (HasHigherPriority(childLeft, node)) { // Check if there is a right child. If not, swap and finish. if (childRightIndex > _numNodes) { node.QueueIndex = childLeftIndex; childLeft.QueueIndex = finalQueueIndex; _nodes[finalQueueIndex] = childLeft; _nodes[childLeftIndex] = node; return; } // Check if the left-child is higher-priority than the right-child PriorityQueueNode <TPriority, TValue> childRight = _nodes[childRightIndex]; if (HasHigherPriority(childLeft, childRight)) { // left is highest, move it up and continue childLeft.QueueIndex = finalQueueIndex; _nodes[finalQueueIndex] = childLeft; finalQueueIndex = childLeftIndex; } else { // right is even higher, move it up and continue childRight.QueueIndex = finalQueueIndex; _nodes[finalQueueIndex] = childRight; finalQueueIndex = childRightIndex; } } // Not swapping with left-child, does right-child exist? else if (childRightIndex > _numNodes) { return; } else { // Check if the right-child is higher-priority than the current node PriorityQueueNode <TPriority, TValue> childRight = _nodes[childRightIndex]; if (HasHigherPriority(childRight, node)) { childRight.QueueIndex = finalQueueIndex; _nodes[finalQueueIndex] = childRight; finalQueueIndex = childRightIndex; } // Neither child is higher-priority than current, so finish and stop. else { return; } } while (true) { childLeftIndex = 2 * finalQueueIndex; // If leaf node, we're done if (childLeftIndex > _numNodes) { node.QueueIndex = finalQueueIndex; _nodes[finalQueueIndex] = node; break; } // Check if the left-child is higher-priority than the current node childRightIndex = childLeftIndex + 1; childLeft = _nodes[childLeftIndex]; if (HasHigherPriority(childLeft, node)) { // Check if there is a right child. If not, swap and finish. if (childRightIndex > _numNodes) { node.QueueIndex = childLeftIndex; childLeft.QueueIndex = finalQueueIndex; _nodes[finalQueueIndex] = childLeft; _nodes[childLeftIndex] = node; break; } // Check if the left-child is higher-priority than the right-child PriorityQueueNode <TPriority, TValue> childRight = _nodes[childRightIndex]; if (HasHigherPriority(childLeft, childRight)) { // left is highest, move it up and continue childLeft.QueueIndex = finalQueueIndex; _nodes[finalQueueIndex] = childLeft; finalQueueIndex = childLeftIndex; } else { // right is even higher, move it up and continue childRight.QueueIndex = finalQueueIndex; _nodes[finalQueueIndex] = childRight; finalQueueIndex = childRightIndex; } } // Not swapping with left-child, does right-child exist? else if (childRightIndex > _numNodes) { node.QueueIndex = finalQueueIndex; _nodes[finalQueueIndex] = node; break; } else { // Check if the right-child is higher-priority than the current node PriorityQueueNode <TPriority, TValue> childRight = _nodes[childRightIndex]; if (HasHigherPriority(childRight, node)) { childRight.QueueIndex = finalQueueIndex; _nodes[finalQueueIndex] = childRight; finalQueueIndex = childRightIndex; } // Neither child is higher-priority than current, so finish and stop. else { node.QueueIndex = finalQueueIndex; _nodes[finalQueueIndex] = node; break; } } } }