|
@@ -1,402 +0,0 @@
|
|
-using System;
|
|
|
|
-using System.Collections;
|
|
|
|
-using System.Collections.Generic;
|
|
|
|
-using System.Runtime.CompilerServices;
|
|
|
|
-
|
|
|
|
-//TODO Fix namespace or replace
|
|
|
|
-namespace Priority_Queue
|
|
|
|
-{
|
|
|
|
- /// <summary>
|
|
|
|
- /// Credit: https://github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp
|
|
|
|
- /// A copy of StablePriorityQueue which also has generic priority-type
|
|
|
|
- /// </summary>
|
|
|
|
- /// <typeparam name="TItem">The values in the queue. Must extend the GenericPriorityQueue class</typeparam>
|
|
|
|
- /// <typeparam name="TPriority">The priority-type. Must extend IComparable<TPriority></typeparam>
|
|
|
|
- public sealed class GenericPriorityQueue<TItem, TPriority> : IFixedSizePriorityQueue<TItem, TPriority>
|
|
|
|
- where TItem : GenericPriorityQueueNode<TPriority>
|
|
|
|
- where TPriority : IComparable<TPriority>
|
|
|
|
- {
|
|
|
|
- private int _numNodes;
|
|
|
|
- private TItem[] _nodes;
|
|
|
|
- private long _numNodesEverEnqueued;
|
|
|
|
-
|
|
|
|
- /// <summary>
|
|
|
|
- /// Instantiate a new Priority Queue
|
|
|
|
- /// </summary>
|
|
|
|
- /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
|
|
|
|
- public GenericPriorityQueue(int maxNodes)
|
|
|
|
- {
|
|
|
|
-#if DEBUG
|
|
|
|
- if (maxNodes <= 0)
|
|
|
|
- {
|
|
|
|
- throw new InvalidOperationException("New queue size cannot be smaller than 1");
|
|
|
|
- }
|
|
|
|
-#endif
|
|
|
|
-
|
|
|
|
- _numNodes = 0;
|
|
|
|
- _nodes = new TItem[maxNodes + 1];
|
|
|
|
- _numNodesEverEnqueued = 0;
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- /// <summary>
|
|
|
|
- /// Returns the number of nodes in the queue.
|
|
|
|
- /// O(1)
|
|
|
|
- /// </summary>
|
|
|
|
- public int Count => _numNodes;
|
|
|
|
-
|
|
|
|
- /// <summary>
|
|
|
|
- /// Returns the maximum number of items that can be enqueued at once in this queue. Once you hit this number (ie. once Count == MaxSize),
|
|
|
|
- /// attempting to enqueue another item will cause undefined behavior. O(1)
|
|
|
|
- /// </summary>
|
|
|
|
- public int MaxSize => _nodes.Length - 1;
|
|
|
|
-
|
|
|
|
- /// <summary>
|
|
|
|
- /// Removes every node from the queue.
|
|
|
|
- /// O(n) (So, don't do this often!)
|
|
|
|
- /// </summary>
|
|
|
|
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
|
|
- public void Clear()
|
|
|
|
- {
|
|
|
|
- Array.Clear(_nodes, 1, _numNodes);
|
|
|
|
- _numNodes = 0;
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- /// <summary>
|
|
|
|
- /// Returns (in O(1)!) whether the given node is in the queue. O(1)
|
|
|
|
- /// </summary>
|
|
|
|
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
|
|
- public bool Contains(TItem node)
|
|
|
|
- {
|
|
|
|
-#if DEBUG
|
|
|
|
- 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?");
|
|
|
|
- }
|
|
|
|
-#endif
|
|
|
|
-
|
|
|
|
- return (_nodes[node.QueueIndex] == node);
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- /// <summary>
|
|
|
|
- /// Enqueue a node to the priority queue. Lower values are placed in front. Ties are broken by first-in-first-out.
|
|
|
|
- /// If the queue is full, the result is undefined.
|
|
|
|
- /// If the node is already enqueued, the result is undefined.
|
|
|
|
- /// O(log n)
|
|
|
|
- /// </summary>
|
|
|
|
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
|
|
- public void Enqueue(TItem node, TPriority priority)
|
|
|
|
- {
|
|
|
|
-#if DEBUG
|
|
|
|
- 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);
|
|
|
|
- }
|
|
|
|
-#endif
|
|
|
|
-
|
|
|
|
- node.Priority = priority;
|
|
|
|
- _numNodes++;
|
|
|
|
- _nodes[_numNodes] = node;
|
|
|
|
- node.QueueIndex = _numNodes;
|
|
|
|
- node.InsertionIndex = _numNodesEverEnqueued++;
|
|
|
|
- CascadeUp(_nodes[_numNodes]);
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
|
|
- private void Swap(TItem node1, TItem node2)
|
|
|
|
- {
|
|
|
|
- //Swap the nodes
|
|
|
|
- _nodes[node1.QueueIndex] = node2;
|
|
|
|
- _nodes[node2.QueueIndex] = node1;
|
|
|
|
-
|
|
|
|
- //Swap their indicies
|
|
|
|
- int temp = node1.QueueIndex;
|
|
|
|
- node1.QueueIndex = node2.QueueIndex;
|
|
|
|
- node2.QueueIndex = temp;
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- //Performance appears to be slightly better when this is NOT inlined o_O
|
|
|
|
- private void CascadeUp(TItem node)
|
|
|
|
- {
|
|
|
|
- //aka Heapify-up
|
|
|
|
- int parent = node.QueueIndex / 2;
|
|
|
|
- while (parent >= 1)
|
|
|
|
- {
|
|
|
|
- var parentNode = _nodes[parent];
|
|
|
|
- if (HasHigherPriority(parentNode, node))
|
|
|
|
- break;
|
|
|
|
-
|
|
|
|
- //Node has lower priority value, so move it up the heap
|
|
|
|
- Swap(node, parentNode); //For some reason, this is faster with Swap() rather than (less..?) individual operations, like in CascadeDown()
|
|
|
|
-
|
|
|
|
- parent = node.QueueIndex / 2;
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
|
|
- private void CascadeDown(TItem node)
|
|
|
|
- {
|
|
|
|
- //aka Heapify-down
|
|
|
|
- TItem newParent;
|
|
|
|
- int finalQueueIndex = node.QueueIndex;
|
|
|
|
- while (true)
|
|
|
|
- {
|
|
|
|
- newParent = node;
|
|
|
|
- int childLeftIndex = 2 * finalQueueIndex;
|
|
|
|
-
|
|
|
|
- //Check if the left-child is higher-priority than the current node
|
|
|
|
- if (childLeftIndex > _numNodes)
|
|
|
|
- {
|
|
|
|
- //This could be placed outside the loop, but then we'd have to check newParent != node twice
|
|
|
|
- node.QueueIndex = finalQueueIndex;
|
|
|
|
- _nodes[finalQueueIndex] = node;
|
|
|
|
- break;
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- var childLeft = _nodes[childLeftIndex];
|
|
|
|
- if (HasHigherPriority(childLeft, newParent))
|
|
|
|
- {
|
|
|
|
- newParent = childLeft;
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- //Check if the right-child is higher-priority than either the current node or the left child
|
|
|
|
- int childRightIndex = childLeftIndex + 1;
|
|
|
|
- if (childRightIndex <= _numNodes)
|
|
|
|
- {
|
|
|
|
- var childRight = _nodes[childRightIndex];
|
|
|
|
- if (HasHigherPriority(childRight, newParent))
|
|
|
|
- {
|
|
|
|
- newParent = childRight;
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- //If either of the children has higher (smaller) priority, swap and continue cascading
|
|
|
|
- if (newParent != node)
|
|
|
|
- {
|
|
|
|
- //Move new parent to its new index. node will be moved once, at the end
|
|
|
|
- //Doing it this way is one less assignment operation than calling Swap()
|
|
|
|
- _nodes[finalQueueIndex] = newParent;
|
|
|
|
-
|
|
|
|
- int temp = newParent.QueueIndex;
|
|
|
|
- newParent.QueueIndex = finalQueueIndex;
|
|
|
|
- finalQueueIndex = temp;
|
|
|
|
- }
|
|
|
|
- else
|
|
|
|
- {
|
|
|
|
- //See note above
|
|
|
|
- node.QueueIndex = finalQueueIndex;
|
|
|
|
- _nodes[finalQueueIndex] = node;
|
|
|
|
- break;
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- /// <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>
|
|
|
|
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
|
|
- private bool HasHigherPriority(TItem higher, TItem lower)
|
|
|
|
- {
|
|
|
|
- var cmp = higher.Priority.CompareTo(lower.Priority);
|
|
|
|
- return (cmp < 0 || (cmp == 0 && higher.InsertionIndex < lower.InsertionIndex));
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- /// <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 bool TryDequeue(out TItem item)
|
|
|
|
- {
|
|
|
|
- if (_numNodes <= 0)
|
|
|
|
- {
|
|
|
|
- item = default(TItem);
|
|
|
|
- return false;
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
-#if DEBUG
|
|
|
|
-
|
|
|
|
- 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?)");
|
|
|
|
- }
|
|
|
|
-#endif
|
|
|
|
-
|
|
|
|
- var returnMe = _nodes[1];
|
|
|
|
- Remove(returnMe);
|
|
|
|
- item = returnMe;
|
|
|
|
- return true;
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- /// <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 DEBUG
|
|
|
|
- if (maxNodes <= 0)
|
|
|
|
- {
|
|
|
|
- throw new InvalidOperationException("Queue size cannot be smaller than 1");
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- if (maxNodes < _numNodes)
|
|
|
|
- {
|
|
|
|
- throw new InvalidOperationException("Called Resize(" + maxNodes + "), but current queue contains " + _numNodes + " nodes");
|
|
|
|
- }
|
|
|
|
-#endif
|
|
|
|
-
|
|
|
|
- TItem[] newArray = new TItem[maxNodes + 1];
|
|
|
|
- int highestIndexToCopy = Math.Min(maxNodes, _numNodes);
|
|
|
|
- for (int i = 1; i <= highestIndexToCopy; i++)
|
|
|
|
- {
|
|
|
|
- newArray[i] = _nodes[i];
|
|
|
|
- }
|
|
|
|
- _nodes = newArray;
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- /// <summary>
|
|
|
|
- /// Returns the head of the queue, without removing it (use Dequeue() for that).
|
|
|
|
- /// If the queue is empty, behavior is undefined.
|
|
|
|
- /// O(1)
|
|
|
|
- /// </summary>
|
|
|
|
- public TItem First
|
|
|
|
- {
|
|
|
|
- get
|
|
|
|
- {
|
|
|
|
-#if DEBUG
|
|
|
|
- if (_numNodes <= 0)
|
|
|
|
- {
|
|
|
|
- throw new InvalidOperationException("Cannot call .First on an empty queue");
|
|
|
|
- }
|
|
|
|
-#endif
|
|
|
|
-
|
|
|
|
- return _nodes[1];
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- /// <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>
|
|
|
|
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
|
|
- public void UpdatePriority(TItem node, TPriority priority)
|
|
|
|
- {
|
|
|
|
-#if DEBUG
|
|
|
|
- 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);
|
|
|
|
- }
|
|
|
|
-#endif
|
|
|
|
-
|
|
|
|
- node.Priority = priority;
|
|
|
|
- OnNodeUpdated(node);
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- private void OnNodeUpdated(TItem node)
|
|
|
|
- {
|
|
|
|
- //Bubble the updated node up or down as appropriate
|
|
|
|
- int parentIndex = node.QueueIndex / 2;
|
|
|
|
- var parentNode = _nodes[parentIndex];
|
|
|
|
-
|
|
|
|
- if (parentIndex > 0 && HasHigherPriority(node, parentNode))
|
|
|
|
- {
|
|
|
|
- CascadeUp(node);
|
|
|
|
- }
|
|
|
|
- else
|
|
|
|
- {
|
|
|
|
- //Note that CascadeDown will be called if parentNode == node (that is, node is the root)
|
|
|
|
- CascadeDown(node);
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- /// <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(TItem node)
|
|
|
|
- {
|
|
|
|
-#if DEBUG
|
|
|
|
- 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);
|
|
|
|
- }
|
|
|
|
-#endif
|
|
|
|
-
|
|
|
|
- //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
|
|
|
|
- var formerLastNode = _nodes[_numNodes];
|
|
|
|
- Swap(node, formerLastNode);
|
|
|
|
- _nodes[_numNodes] = null;
|
|
|
|
- _numNodes--;
|
|
|
|
-
|
|
|
|
- //Now bubble formerLastNode (which is no longer the last node) up or down as appropriate
|
|
|
|
- OnNodeUpdated(formerLastNode);
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- public IEnumerator<TItem> GetEnumerator()
|
|
|
|
- {
|
|
|
|
- for (int i = 1; i <= _numNodes; i++)
|
|
|
|
- yield return _nodes[i];
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- IEnumerator IEnumerable.GetEnumerator()
|
|
|
|
- {
|
|
|
|
- return GetEnumerator();
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- /// <summary>
|
|
|
|
- /// <b>Should not be called in production code.</b>
|
|
|
|
- /// Checks to make sure the queue is still in a valid state. Used for testing/debugging the queue.
|
|
|
|
- /// </summary>
|
|
|
|
- public bool IsValidQueue()
|
|
|
|
- {
|
|
|
|
- for (int i = 1; i < _nodes.Length; i++)
|
|
|
|
- {
|
|
|
|
- if (_nodes[i] != null)
|
|
|
|
- {
|
|
|
|
- int childLeftIndex = 2 * i;
|
|
|
|
- if (childLeftIndex < _nodes.Length && _nodes[childLeftIndex] != null && HasHigherPriority(_nodes[childLeftIndex], _nodes[i]))
|
|
|
|
- return false;
|
|
|
|
-
|
|
|
|
- int childRightIndex = childLeftIndex + 1;
|
|
|
|
- if (childRightIndex < _nodes.Length && _nodes[childRightIndex] != null && HasHigherPriority(_nodes[childRightIndex], _nodes[i]))
|
|
|
|
- return false;
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
- return true;
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
-}
|
|
|