From d0dba9752d0a8e787d9ae1d4d25542bd4b386df6 Mon Sep 17 00:00:00 2001 From: Roy Ben Shabat Date: Fri, 20 Mar 2020 04:03:05 +0200 Subject: Implemented priority queues for Transport Layer. Working on file/folder download. --- .../SideChains/Priority Queue/FastPriorityQueue.cs | 593 ++++++++++++++++++++ .../Priority Queue/FastPriorityQueueNode.cs | 25 + .../Priority Queue/GenericPriorityQueue.cs | 602 +++++++++++++++++++++ .../Priority Queue/GenericPriorityQueueNode.cs | 29 + .../Priority Queue/IFixedSizePriorityQueue.cs | 31 ++ .../SideChains/Priority Queue/IPriorityQueue.cs | 55 ++ .../Priority Queue/Priority Queue.csproj | 89 +++ .../Priority Queue/Priority Queue.nuspec | 42 ++ .../Priority Queue/Properties/AssemblyInfo.cs | 38 ++ .../Priority Queue/SimplePriorityQueue.cs | 588 ++++++++++++++++++++ .../Priority Queue/StablePriorityQueue.cs | 587 ++++++++++++++++++++ .../Priority Queue/StablePriorityQueueNode.cs | 10 + 12 files changed, 2689 insertions(+) create mode 100644 Software/Visual_Studio/SideChains/Priority Queue/FastPriorityQueue.cs create mode 100644 Software/Visual_Studio/SideChains/Priority Queue/FastPriorityQueueNode.cs create mode 100644 Software/Visual_Studio/SideChains/Priority Queue/GenericPriorityQueue.cs create mode 100644 Software/Visual_Studio/SideChains/Priority Queue/GenericPriorityQueueNode.cs create mode 100644 Software/Visual_Studio/SideChains/Priority Queue/IFixedSizePriorityQueue.cs create mode 100644 Software/Visual_Studio/SideChains/Priority Queue/IPriorityQueue.cs create mode 100644 Software/Visual_Studio/SideChains/Priority Queue/Priority Queue.csproj create mode 100644 Software/Visual_Studio/SideChains/Priority Queue/Priority Queue.nuspec create mode 100644 Software/Visual_Studio/SideChains/Priority Queue/Properties/AssemblyInfo.cs create mode 100644 Software/Visual_Studio/SideChains/Priority Queue/SimplePriorityQueue.cs create mode 100644 Software/Visual_Studio/SideChains/Priority Queue/StablePriorityQueue.cs create mode 100644 Software/Visual_Studio/SideChains/Priority Queue/StablePriorityQueueNode.cs (limited to 'Software/Visual_Studio/SideChains') diff --git a/Software/Visual_Studio/SideChains/Priority Queue/FastPriorityQueue.cs b/Software/Visual_Studio/SideChains/Priority Queue/FastPriorityQueue.cs new file mode 100644 index 000000000..927a20d41 --- /dev/null +++ b/Software/Visual_Studio/SideChains/Priority Queue/FastPriorityQueue.cs @@ -0,0 +1,593 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.Runtime.CompilerServices; + +namespace Priority_Queue +{ + /// + /// An implementation of a min-Priority Queue using a heap. Has O(1) .Contains()! + /// See https://github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp/wiki/Getting-Started for more information + /// + /// The values in the queue. Must extend the FastPriorityQueueNode class + public sealed class FastPriorityQueue : IFixedSizePriorityQueue + where T : FastPriorityQueueNode + { + private int _numNodes; + private T[] _nodes; + + /// + /// Instantiate a new Priority Queue + /// + /// The max nodes ever allowed to be enqueued (going over this will cause undefined behavior) + public FastPriorityQueue(int maxNodes) + { +#if DEBUG + if (maxNodes <= 0) + { + throw new InvalidOperationException("New queue size cannot be smaller than 1"); + } +#endif + + _numNodes = 0; + _nodes = new T[maxNodes + 1]; + } + + /// + /// Returns the number of nodes in the queue. + /// O(1) + /// + public int Count + { + get + { + return _numNodes; + } + } + + /// + /// 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) + /// + public int MaxSize + { + get + { + return _nodes.Length - 1; + } + } + + /// + /// Removes every node from the queue. + /// O(n) (So, don't do this often!) + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public void Clear() + { + Array.Clear(_nodes, 1, _numNodes); + _numNodes = 0; + } + + /// + /// Returns (in O(1)!) whether the given node is in the queue. + /// If node is or has been previously added to another queue, the result is undefined unless oldQueue.ResetNode(node) has been called + /// O(1) + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public bool Contains(T node) + { +#if DEBUG + if(node == null) + { + throw new ArgumentNullException("node"); + } + if (node.Queue != null && !Equals(node.Queue)) + { + throw new InvalidOperationException("node.Contains was called on a node from another queue. Please call originalQueue.ResetNode() first"); + } + 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); + } + + /// + /// Enqueue a node to the priority queue. Lower values are placed in front. Ties are broken arbitrarily. + /// If the queue is full, the result is undefined. + /// If the node is already enqueued, the result is undefined. + /// If node is or has been previously added to another queue, the result is undefined unless oldQueue.ResetNode(node) has been called + /// O(log n) + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public void Enqueue(T node, float priority) + { +#if DEBUG + if(node == null) + { + throw new ArgumentNullException("node"); + } + if(_numNodes >= _nodes.Length - 1) + { + throw new InvalidOperationException("Queue is full - node cannot be added: " + node); + } + if (node.Queue != null && !Equals(node.Queue)) + { + throw new InvalidOperationException("node.Enqueue was called on a node from another queue. Please call originalQueue.ResetNode() first"); + } + if (Contains(node)) + { + throw new InvalidOperationException("Node is already enqueued: " + node); + } + node.Queue = this; +#endif + + node.Priority = priority; + _numNodes++; + _nodes[_numNodes] = node; + node.QueueIndex = _numNodes; + CascadeUp(node); + } + +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + private void CascadeUp(T node) + { + //aka Heapify-up + int parent; + if(node.QueueIndex > 1) + { + parent = node.QueueIndex >> 1; + T parentNode = _nodes[parent]; + if(HasHigherOrEqualPriority(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; + T parentNode = _nodes[parent]; + if(HasHigherOrEqualPriority(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; + } + +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + private void CascadeDown(T 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; + T 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 + T 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 + T 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 + T 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 + T 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; + } + } + } + } + + /// + /// 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 + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + private bool HasHigherPriority(T higher, T lower) + { + return (higher.Priority < lower.Priority); + } + + /// + /// Returns true if 'higher' has higher priority than 'lower', false otherwise. + /// Note that calling HasHigherOrEqualPriority(node, node) (ie. both arguments the same node) will return true + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + private bool HasHigherOrEqualPriority(T higher, T lower) + { + return (higher.Priority <= lower.Priority); + } + + /// + /// Removes the head of the queue and returns it. + /// If queue is empty, result is undefined + /// O(log n) + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public T Dequeue() + { +#if DEBUG + 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?)"); + } +#endif + + T 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 + T 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; + } + + /// + /// 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) + /// + 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 + + T[] newArray = new T[maxNodes + 1]; + int highestIndexToCopy = Math.Min(maxNodes, _numNodes); + Array.Copy(_nodes, newArray, highestIndexToCopy + 1); + _nodes = newArray; + } + + /// + /// Returns the head of the queue, without removing it (use Dequeue() for that). + /// If the queue is empty, behavior is undefined. + /// O(1) + /// + public T First + { + get + { +#if DEBUG + if(_numNodes <= 0) + { + throw new InvalidOperationException("Cannot call .First on an empty queue"); + } +#endif + + return _nodes[1]; + } + } + + /// + /// This method must be called on a node every time its priority changes while it is in the queue. + /// Forgetting to call this method will result in a corrupted queue! + /// Calling this method on a node not in the queue results in undefined behavior + /// O(log n) + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public void UpdatePriority(T node, float priority) + { +#if DEBUG + if(node == null) + { + throw new ArgumentNullException("node"); + } + if (node.Queue != null && !Equals(node.Queue)) + { + throw new InvalidOperationException("node.UpdatePriority was called on a node from another queue"); + } + if (!Contains(node)) + { + throw new InvalidOperationException("Cannot call UpdatePriority() on a node which is not enqueued: " + node); + } +#endif + + node.Priority = priority; + OnNodeUpdated(node); + } + +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + private void OnNodeUpdated(T 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); + } + } + + /// + /// 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) + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public void Remove(T node) + { +#if DEBUG + if(node == null) + { + throw new ArgumentNullException("node"); + } + if (node.Queue != null && !Equals(node.Queue)) + { + throw new InvalidOperationException("node.Remove was called on a node from another queue"); + } + 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 + T 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); + } + + /// + /// By default, nodes that have been previously added to one queue cannot be added to another queue. + /// If you need to do this, please call originalQueue.ResetNode(node) before attempting to add it in the new queue + /// If the node is currently in the queue or belongs to another queue, the result is undefined + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public void ResetNode(T node) + { +#if DEBUG + if (node == null) + { + throw new ArgumentNullException("node"); + } + if (node.Queue != null && !Equals(node.Queue)) + { + throw new InvalidOperationException("node.ResetNode was called on a node from another queue"); + } + if (Contains(node)) + { + throw new InvalidOperationException("node.ResetNode was called on a node that is still in the queue"); + } + + node.Queue = null; +#endif + + node.QueueIndex = 0; + } + + public IEnumerator GetEnumerator() + { +#if NET_VERSION_4_5 // ArraySegment does not implement IEnumerable before 4.5 + IEnumerable e = new ArraySegment(_nodes, 1, _numNodes); + return e.GetEnumerator(); +#else + for(int i = 1; i <= _numNodes; i++) + yield return _nodes[i]; +#endif + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + + /// + /// Should not be called in production code. + /// Checks to make sure the queue is still in a valid state. Used for testing/debugging the queue. + /// + 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; + } + } +} \ No newline at end of file diff --git a/Software/Visual_Studio/SideChains/Priority Queue/FastPriorityQueueNode.cs b/Software/Visual_Studio/SideChains/Priority Queue/FastPriorityQueueNode.cs new file mode 100644 index 000000000..54b0573ec --- /dev/null +++ b/Software/Visual_Studio/SideChains/Priority Queue/FastPriorityQueueNode.cs @@ -0,0 +1,25 @@ +using System; + +namespace Priority_Queue +{ + public class FastPriorityQueueNode + { + /// + /// The Priority to insert this node at. Must be set BEFORE adding a node to the queue (ideally just once, in the node's constructor). + /// Should not be manually edited once the node has been enqueued - use queue.UpdatePriority() instead + /// + public float Priority { get; protected internal set; } + + /// + /// Represents the current position in the queue + /// + public int QueueIndex { get; internal set; } + +#if DEBUG + /// + /// The queue this node is tied to. Used only for debug builds. + /// + public object Queue { get; internal set; } +#endif + } +} diff --git a/Software/Visual_Studio/SideChains/Priority Queue/GenericPriorityQueue.cs b/Software/Visual_Studio/SideChains/Priority Queue/GenericPriorityQueue.cs new file mode 100644 index 000000000..7687ba213 --- /dev/null +++ b/Software/Visual_Studio/SideChains/Priority Queue/GenericPriorityQueue.cs @@ -0,0 +1,602 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.Runtime.CompilerServices; + +namespace Priority_Queue +{ + /// + /// A copy of StablePriorityQueue which also has generic priority-type + /// + /// The values in the queue. Must extend the GenericPriorityQueueNode class + /// The priority-type. Must extend IComparable<TPriority> + public sealed class GenericPriorityQueue : IFixedSizePriorityQueue + where TItem : GenericPriorityQueueNode + where TPriority : IComparable + { + private int _numNodes; + private TItem[] _nodes; + private long _numNodesEverEnqueued; + private readonly Comparison _comparer; + + /// + /// Instantiate a new Priority Queue + /// + /// The max nodes ever allowed to be enqueued (going over this will cause undefined behavior) + public GenericPriorityQueue(int maxNodes) : this(maxNodes, Comparer.Default) { } + + /// + /// Instantiate a new Priority Queue + /// + /// The max nodes ever allowed to be enqueued (going over this will cause undefined behavior) + /// The comparer used to compare TPriority values. + public GenericPriorityQueue(int maxNodes, IComparer comparer) : this(maxNodes, comparer.Compare) { } + + /// + /// Instantiate a new Priority Queue + /// + /// The max nodes ever allowed to be enqueued (going over this will cause undefined behavior) + /// The comparison function to use to compare TPriority values + public GenericPriorityQueue(int maxNodes, Comparison comparer) + { +#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; + _comparer = comparer; + } + + /// + /// Returns the number of nodes in the queue. + /// O(1) + /// + public int Count + { + get + { + return _numNodes; + } + } + + /// + /// 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) + /// + public int MaxSize + { + get + { + return _nodes.Length - 1; + } + } + + /// + /// Removes every node from the queue. + /// O(n) (So, don't do this often!) + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public void Clear() + { + Array.Clear(_nodes, 1, _numNodes); + _numNodes = 0; + } + + /// + /// Returns (in O(1)!) whether the given node is in the queue. + /// If node is or has been previously added to another queue, the result is undefined unless oldQueue.ResetNode(node) has been called + /// O(1) + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public bool Contains(TItem node) + { +#if DEBUG + if(node == null) + { + throw new ArgumentNullException("node"); + } + if (node.Queue != null && !Equals(node.Queue)) + { + throw new InvalidOperationException("node.Contains was called on a node from another queue. Please call originalQueue.ResetNode() first"); + } + if (node.QueueIndex < 0 || node.QueueIndex >= _nodes.Length) + { + throw new InvalidOperationException("node.QueueIndex has been corrupted. Did you change it manually?"); + } +#endif + + return (_nodes[node.QueueIndex] == node); + } + + /// + /// 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. + /// If node is or has been previously added to another queue, the result is undefined unless oldQueue.ResetNode(node) has been called + /// O(log n) + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public void Enqueue(TItem node, TPriority priority) + { +#if DEBUG + if(node == null) + { + throw new ArgumentNullException("node"); + } + if(_numNodes >= _nodes.Length - 1) + { + throw new InvalidOperationException("Queue is full - node cannot be added: " + node); + } + if (node.Queue != null && !Equals(node.Queue)) + { + throw new InvalidOperationException("node.Enqueue was called on a node from another queue. Please call originalQueue.ResetNode() first"); + } + if (Contains(node)) + { + throw new InvalidOperationException("Node is already enqueued: " + node); + } + node.Queue = this; +#endif + + node.Priority = priority; + _numNodes++; + _nodes[_numNodes] = node; + node.QueueIndex = _numNodes; + node.InsertionIndex = _numNodesEverEnqueued++; + CascadeUp(node); + } + +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + private void CascadeUp(TItem node) + { + //aka Heapify-up + int parent; + if (node.QueueIndex > 1) + { + parent = node.QueueIndex >> 1; + TItem 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; + TItem 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; + } + +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + private void CascadeDown(TItem 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; + TItem 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 + TItem 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 + TItem 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 + TItem 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 + TItem 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; + } + } + } + } + + /// + /// 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 + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + private bool HasHigherPriority(TItem higher, TItem lower) + { + var cmp = _comparer(higher.Priority, lower.Priority); + return (cmp < 0 || (cmp == 0 && higher.InsertionIndex < lower.InsertionIndex)); + } + + /// + /// 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) + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public TItem Dequeue() + { +#if DEBUG + 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?)"); + } +#endif + + TItem 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 + TItem 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; + } + + /// + /// 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) + /// + 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); + Array.Copy(_nodes, newArray, highestIndexToCopy + 1); + _nodes = newArray; + } + + /// + /// Returns the head of the queue, without removing it (use Dequeue() for that). + /// If the queue is empty, behavior is undefined. + /// O(1) + /// + public TItem First + { + get + { +#if DEBUG + if(_numNodes <= 0) + { + throw new InvalidOperationException("Cannot call .First on an empty queue"); + } +#endif + + return _nodes[1]; + } + } + + /// + /// This method must be called on a node every time its priority changes while it is in the queue. + /// Forgetting to call this method will result in a corrupted queue! + /// Calling this method on a node not in the queue results in undefined behavior + /// O(log n) + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public void UpdatePriority(TItem node, TPriority priority) + { +#if DEBUG + if(node == null) + { + throw new ArgumentNullException("node"); + } + if (node.Queue != null && !Equals(node.Queue)) + { + throw new InvalidOperationException("node.UpdatePriority was called on a node from another queue"); + } + if (!Contains(node)) + { + throw new InvalidOperationException("Cannot call UpdatePriority() on a node which is not enqueued: " + node); + } +#endif + + node.Priority = priority; + OnNodeUpdated(node); + } + +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + private void OnNodeUpdated(TItem 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); + } + } + + /// + /// 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) + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public void Remove(TItem node) + { +#if DEBUG + if(node == null) + { + throw new ArgumentNullException("node"); + } + if (node.Queue != null && !Equals(node.Queue)) + { + throw new InvalidOperationException("node.Remove was called on a node from another queue"); + } + 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 + TItem 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); + } + + /// + /// By default, nodes that have been previously added to one queue cannot be added to another queue. + /// If you need to do this, please call originalQueue.ResetNode(node) before attempting to add it in the new queue + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public void ResetNode(TItem node) + { +#if DEBUG + if (node == null) + { + throw new ArgumentNullException("node"); + } + if (node.Queue != null && !Equals(node.Queue)) + { + throw new InvalidOperationException("node.ResetNode was called on a node from another queue"); + } + if (Contains(node)) + { + throw new InvalidOperationException("node.ResetNode was called on a node that is still in the queue"); + } + + node.Queue = null; +#endif + + node.QueueIndex = 0; + } + + + public IEnumerator GetEnumerator() + { +#if NET_VERSION_4_5 // ArraySegment does not implement IEnumerable before 4.5 + IEnumerable e = new ArraySegment(_nodes, 1, _numNodes); + return e.GetEnumerator(); +#else + for(int i = 1; i <= _numNodes; i++) + yield return _nodes[i]; +#endif + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + + /// + /// Should not be called in production code. + /// Checks to make sure the queue is still in a valid state. Used for testing/debugging the queue. + /// + 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; + } + } +} diff --git a/Software/Visual_Studio/SideChains/Priority Queue/GenericPriorityQueueNode.cs b/Software/Visual_Studio/SideChains/Priority Queue/GenericPriorityQueueNode.cs new file mode 100644 index 000000000..5a53ed24e --- /dev/null +++ b/Software/Visual_Studio/SideChains/Priority Queue/GenericPriorityQueueNode.cs @@ -0,0 +1,29 @@ +namespace Priority_Queue +{ + public class GenericPriorityQueueNode + { + /// + /// The Priority to insert this node at. Must be set BEFORE adding a node to the queue (ideally just once, in the node's constructor). + /// Should not be manually edited once the node has been enqueued - use queue.UpdatePriority() instead + /// + public TPriority Priority { get; protected internal set; } + + /// + /// Represents the current position in the queue + /// + public int QueueIndex { get; internal set; } + + /// + /// Represents the order the node was inserted in + /// + public long InsertionIndex { get; internal set; } + + +#if DEBUG + /// + /// The queue this node is tied to. Used only for debug builds. + /// + public object Queue { get; internal set; } +#endif + } +} diff --git a/Software/Visual_Studio/SideChains/Priority Queue/IFixedSizePriorityQueue.cs b/Software/Visual_Studio/SideChains/Priority Queue/IFixedSizePriorityQueue.cs new file mode 100644 index 000000000..e95f7c29b --- /dev/null +++ b/Software/Visual_Studio/SideChains/Priority Queue/IFixedSizePriorityQueue.cs @@ -0,0 +1,31 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace Priority_Queue +{ + /// + /// A helper-interface only needed to make writing unit tests a bit easier (hence the 'internal' access modifier) + /// + internal interface IFixedSizePriorityQueue : IPriorityQueue + where TPriority : IComparable + { + /// + /// 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 + /// + void Resize(int maxNodes); + + /// + /// 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. + /// + int MaxSize { get; } + + /// + /// By default, nodes that have been previously added to one queue cannot be added to another queue. + /// If you need to do this, please call originalQueue.ResetNode(node) before attempting to add it in the new queue + /// + void ResetNode(TItem node); + } +} diff --git a/Software/Visual_Studio/SideChains/Priority Queue/IPriorityQueue.cs b/Software/Visual_Studio/SideChains/Priority Queue/IPriorityQueue.cs new file mode 100644 index 000000000..a5053e955 --- /dev/null +++ b/Software/Visual_Studio/SideChains/Priority Queue/IPriorityQueue.cs @@ -0,0 +1,55 @@ +using System; +using System.Collections.Generic; + +namespace Priority_Queue +{ + /// + /// The IPriorityQueue interface. This is mainly here for purists, and in case I decide to add more implementations later. + /// For speed purposes, it is actually recommended that you *don't* access the priority queue through this interface, since the JIT can + /// (theoretically?) optimize method calls from concrete-types slightly better. + /// + public interface IPriorityQueue : IEnumerable + where TPriority : IComparable + { + /// + /// Enqueue a node to the priority queue. Lower values are placed in front. Ties are broken by first-in-first-out. + /// See implementation for how duplicates are handled. + /// + void Enqueue(TItem node, TPriority priority); + + /// + /// Removes the head of the queue (node with minimum priority; ties are broken by order of insertion), and returns it. + /// + TItem Dequeue(); + + /// + /// Removes every node from the queue. + /// + void Clear(); + + /// + /// Returns whether the given node is in the queue. + /// + bool Contains(TItem node); + + /// + /// Removes a node from the queue. The node does not need to be the head of the queue. + /// + void Remove(TItem node); + + /// + /// Call this method to change the priority of a node. + /// + void UpdatePriority(TItem node, TPriority priority); + + /// + /// Returns the head of the queue, without removing it (use Dequeue() for that). + /// + TItem First { get; } + + /// + /// Returns the number of nodes in the queue. + /// + int Count { get; } + } +} diff --git a/Software/Visual_Studio/SideChains/Priority Queue/Priority Queue.csproj b/Software/Visual_Studio/SideChains/Priority Queue/Priority Queue.csproj new file mode 100644 index 000000000..8b02157f1 --- /dev/null +++ b/Software/Visual_Studio/SideChains/Priority Queue/Priority Queue.csproj @@ -0,0 +1,89 @@ + + + + + Debug + AnyCPU + {1531C1EA-BD53-41D1-A34B-CFCDF79D2651} + Library + Properties + Priority_Queue + Priority Queue + v4.6.1 + 512 + NET_VERSION_4_5 + $(DefineConstants);$(CustomConstants) + + + + + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + NET_VERSION_4_5 + $(DefineConstants);$(CustomConstants) + false + + + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + NET_VERSION_4_5 + $(DefineConstants);$(CustomConstants) + false + + + pdbonly + true + TRACE;NET_VERSION_4_5 + prompt + 4 + false + bin\Release\net45\ + v4.5 + bin\Release\net45\Priority Queue.xml + + + pdbonly + true + TRACE + prompt + 4 + false + bin\Release\net20\ + v2.0 + bin\Release\net20\Priority Queue.xml + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Software/Visual_Studio/SideChains/Priority Queue/Priority Queue.nuspec b/Software/Visual_Studio/SideChains/Priority Queue/Priority Queue.nuspec new file mode 100644 index 000000000..94ab70f4d --- /dev/null +++ b/Software/Visual_Studio/SideChains/Priority Queue/Priority Queue.nuspec @@ -0,0 +1,42 @@ + + + + OptimizedPriorityQueue + 4.2.0 + Highly Optimized Priority Queue + BlueRaja + BlueRaja + https://github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp/blob/master/LICENSE.txt + https://github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp + false + A highly optimized Priority Queue for path-finding and related applications + Speed improvements; added ResetNode(); included IEqualityComparer in SimplePriorityQueue to avoid boxing + Copyright 2018 + C# priority-queue pathfinding optimized + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Software/Visual_Studio/SideChains/Priority Queue/Properties/AssemblyInfo.cs b/Software/Visual_Studio/SideChains/Priority Queue/Properties/AssemblyInfo.cs new file mode 100644 index 000000000..7143261f2 --- /dev/null +++ b/Software/Visual_Studio/SideChains/Priority Queue/Properties/AssemblyInfo.cs @@ -0,0 +1,38 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// General Information about an assembly is controlled through the following +// set of attributes. Change these attribute values to modify the information +// associated with an assembly. +[assembly: AssemblyTitle("Highly Optimized Priority Queue")] +[assembly: AssemblyDescription("A highly optimized Priority Queue for path-finding and related applications")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("Highly Optimized Priority Queue")] +[assembly: AssemblyCopyright("Copyright © BlueRaja 2017")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// Setting ComVisible to false makes the types in this assembly not visible +// to COM components. If you need to access a type in this assembly from +// COM, set the ComVisible attribute to true on that type. +[assembly: ComVisible(false)] + +// The following GUID is for the ID of the typelib if this project is exposed to COM +[assembly: Guid("3eee6b54-af8a-494b-9121-3d46ed09a58b")] + +[assembly: InternalsVisibleToAttribute("Priority Queue Tests")] + +// Version information for an assembly consists of the following four values: +// +// Major Version +// Minor Version +// Build Number +// Revision +// +// You can specify all the values or you can default the Build and Revision Numbers +// by using the '*' as shown below: +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("4.2.0")] +[assembly: AssemblyFileVersion("4.2.0")] \ No newline at end of file diff --git a/Software/Visual_Studio/SideChains/Priority Queue/SimplePriorityQueue.cs b/Software/Visual_Studio/SideChains/Priority Queue/SimplePriorityQueue.cs new file mode 100644 index 000000000..0d35ff1ac --- /dev/null +++ b/Software/Visual_Studio/SideChains/Priority Queue/SimplePriorityQueue.cs @@ -0,0 +1,588 @@ +using System; +using System.Collections; +using System.Collections.Generic; + +namespace Priority_Queue +{ + /// + /// A simplified priority queue implementation. Is stable, auto-resizes, and thread-safe, at the cost of being slightly slower than + /// FastPriorityQueue + /// Methods tagged as O(1) or O(log n) are assuming there are no duplicates. Duplicates may increase the algorithmic complexity. + /// + /// The type to enqueue + /// The priority-type to use for nodes. Must extend IComparable<TPriority> + public class SimplePriorityQueue : IPriorityQueue + where TPriority : IComparable + { + private class SimpleNode : GenericPriorityQueueNode + { + public TItem Data { get; private set; } + + public SimpleNode(TItem data) + { + Data = data; + } + } + + private const int INITIAL_QUEUE_SIZE = 10; + private readonly GenericPriorityQueue _queue; + private readonly Dictionary> _itemToNodesCache; + private readonly IList _nullNodesCache; + + #region Constructors + /// + /// Instantiate a new Priority Queue + /// + public SimplePriorityQueue() : this(Comparer.Default, EqualityComparer.Default) { } + + /// + /// Instantiate a new Priority Queue + /// + /// The comparer used to compare TPriority values. Defaults to Comparer<TPriority>.default + public SimplePriorityQueue(IComparer priorityComparer) : this(priorityComparer.Compare, EqualityComparer.Default) { } + + /// + /// Instantiate a new Priority Queue + /// + /// The comparison function to use to compare TPriority values + public SimplePriorityQueue(Comparison priorityComparer) : this(priorityComparer, EqualityComparer.Default) { } + + /// + /// Instantiate a new Priority Queue + /// + /// The equality comparison function to use to compare TItem values + public SimplePriorityQueue(IEqualityComparer itemEquality) : this(Comparer.Default, itemEquality) { } + + /// + /// Instantiate a new Priority Queue + /// + /// The comparer used to compare TPriority values. Defaults to Comparer<TPriority>.default + /// The equality comparison function to use to compare TItem values + public SimplePriorityQueue(IComparer priorityComparer, IEqualityComparer itemEquality) : this(priorityComparer.Compare, itemEquality) { } + + /// + /// Instantiate a new Priority Queue + /// + /// The comparison function to use to compare TPriority values + /// The equality comparison function to use to compare TItem values + public SimplePriorityQueue(Comparison priorityComparer, IEqualityComparer itemEquality) + { + _queue = new GenericPriorityQueue(INITIAL_QUEUE_SIZE, priorityComparer); + _itemToNodesCache = new Dictionary>(itemEquality); + _nullNodesCache = new List(); + } + #endregion + + /// + /// Given an item of type T, returns the existing SimpleNode in the queue + /// + private SimpleNode GetExistingNode(TItem item) + { + if (item == null) + { + return _nullNodesCache.Count > 0 ? _nullNodesCache[0] : null; + } + + IList nodes; + if (!_itemToNodesCache.TryGetValue(item, out nodes)) + { + return null; + } + return nodes[0]; + } + + /// + /// Adds an item to the Node-cache to allow for many methods to be O(1) or O(log n) + /// + private void AddToNodeCache(SimpleNode node) + { + if (node.Data == null) + { + _nullNodesCache.Add(node); + return; + } + + IList nodes; + if (!_itemToNodesCache.TryGetValue(node.Data, out nodes)) + { + nodes = new List(); + _itemToNodesCache[node.Data] = nodes; + } + nodes.Add(node); + } + + /// + /// Removes an item to the Node-cache to allow for many methods to be O(1) or O(log n) (assuming no duplicates) + /// + private void RemoveFromNodeCache(SimpleNode node) + { + if (node.Data == null) + { + _nullNodesCache.Remove(node); + return; + } + + IList nodes; + if (!_itemToNodesCache.TryGetValue(node.Data, out nodes)) + { + return; + } + nodes.Remove(node); + if (nodes.Count == 0) + { + _itemToNodesCache.Remove(node.Data); + } + } + + /// + /// Returns the number of nodes in the queue. + /// O(1) + /// + public int Count + { + get + { + lock(_queue) + { + return _queue.Count; + } + } + } + + /// + /// Returns the head of the queue, without removing it (use Dequeue() for that). + /// Throws an exception when the queue is empty. + /// O(1) + /// + public TItem First + { + get + { + lock(_queue) + { + if(_queue.Count <= 0) + { + throw new InvalidOperationException("Cannot call .First on an empty queue"); + } + + return _queue.First.Data; + } + } + } + + /// + /// Removes every node from the queue. + /// O(n) + /// + public void Clear() + { + lock(_queue) + { + _queue.Clear(); + _itemToNodesCache.Clear(); + _nullNodesCache.Clear(); + } + } + + /// + /// Returns whether the given item is in the queue. + /// O(1) + /// + public bool Contains(TItem item) + { + lock(_queue) + { + return item == null ? _nullNodesCache.Count > 0 : _itemToNodesCache.ContainsKey(item); + } + } + + /// + /// Removes the head of the queue (node with minimum priority; ties are broken by order of insertion), and returns it. + /// If queue is empty, throws an exception + /// O(log n) + /// + public TItem Dequeue() + { + lock(_queue) + { + if(_queue.Count <= 0) + { + throw new InvalidOperationException("Cannot call Dequeue() on an empty queue"); + } + + SimpleNode node =_queue.Dequeue(); + RemoveFromNodeCache(node); + return node.Data; + } + } + + /// + /// Enqueue the item with the given priority, without calling lock(_queue) or AddToNodeCache(node) + /// + /// + /// + /// + private SimpleNode EnqueueNoLockOrCache(TItem item, TPriority priority) + { + SimpleNode node = new SimpleNode(item); + if (_queue.Count == _queue.MaxSize) + { + _queue.Resize(_queue.MaxSize * 2 + 1); + } + _queue.Enqueue(node, priority); + return node; + } + + /// + /// Enqueue a node to the priority queue. Lower values are placed in front. Ties are broken by first-in-first-out. + /// This queue automatically resizes itself, so there's no concern of the queue becoming 'full'. + /// Duplicates and null-values are allowed. + /// O(log n) + /// + public void Enqueue(TItem item, TPriority priority) + { + lock(_queue) + { + IList nodes; + if (item == null) + { + nodes = _nullNodesCache; + } + else if (!_itemToNodesCache.TryGetValue(item, out nodes)) + { + nodes = new List(); + _itemToNodesCache[item] = nodes; + } + SimpleNode node = EnqueueNoLockOrCache(item, priority); + nodes.Add(node); + } + } + + /// + /// Enqueue a node to the priority queue if it doesn't already exist. Lower values are placed in front. Ties are broken by first-in-first-out. + /// This queue automatically resizes itself, so there's no concern of the queue becoming 'full'. Null values are allowed. + /// Returns true if the node was successfully enqueued; false if it already exists. + /// O(log n) + /// + public bool EnqueueWithoutDuplicates(TItem item, TPriority priority) + { + lock(_queue) + { + IList nodes; + if (item == null) + { + if (_nullNodesCache.Count > 0) + { + return false; + } + nodes = _nullNodesCache; + } + else if (_itemToNodesCache.ContainsKey(item)) + { + return false; + } + else + { + nodes = new List(); + _itemToNodesCache[item] = nodes; + } + SimpleNode node = EnqueueNoLockOrCache(item, priority); + nodes.Add(node); + return true; + } + } + + /// + /// Removes an item from the queue. The item does not need to be the head of the queue. + /// If the item is not in the queue, an exception is thrown. If unsure, check Contains() first. + /// If multiple copies of the item are enqueued, only the first one is removed. + /// O(log n) + /// + public void Remove(TItem item) + { + lock(_queue) + { + SimpleNode removeMe; + IList nodes; + if (item == null) + { + if (_nullNodesCache.Count == 0) + { + throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + item); + } + removeMe = _nullNodesCache[0]; + nodes = _nullNodesCache; + } + else + { + if (!_itemToNodesCache.TryGetValue(item, out nodes)) + { + throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + item); + } + removeMe = nodes[0]; + if (nodes.Count == 1) + { + _itemToNodesCache.Remove(item); + } + } + _queue.Remove(removeMe); + nodes.Remove(removeMe); + } + } + + /// + /// Call this method to change the priority of an item. + /// Calling this method on a item not in the queue will throw an exception. + /// If the item is enqueued multiple times, only the first one will be updated. + /// (If your requirements are complex enough that you need to enqueue the same item multiple times and be able + /// to update all of them, please wrap your items in a wrapper class so they can be distinguished). + /// O(log n) + /// + public void UpdatePriority(TItem item, TPriority priority) + { + lock (_queue) + { + SimpleNode updateMe = GetExistingNode(item); + if (updateMe == null) + { + throw new InvalidOperationException("Cannot call UpdatePriority() on a node which is not enqueued: " + item); + } + _queue.UpdatePriority(updateMe, priority); + } + } + + /// + /// Returns the priority of the given item. + /// Calling this method on a item not in the queue will throw an exception. + /// If the item is enqueued multiple times, only the priority of the first will be returned. + /// (If your requirements are complex enough that you need to enqueue the same item multiple times and be able + /// to query all their priorities, please wrap your items in a wrapper class so they can be distinguished). + /// O(1) + /// + public TPriority GetPriority(TItem item) + { + lock (_queue) + { + SimpleNode findMe = GetExistingNode(item); + if(findMe == null) + { + throw new InvalidOperationException("Cannot call GetPriority() on a node which is not enqueued: " + item); + } + return findMe.Priority; + } + } + + #region Try* methods for multithreading + /// Get the head of the queue, without removing it (use TryDequeue() for that). + /// Useful for multi-threading, where the queue may become empty between calls to Contains() and First + /// Returns true if successful, false otherwise + /// O(1) + public bool TryFirst(out TItem first) + { + if (_queue.Count > 0) + { + lock (_queue) + { + if (_queue.Count > 0) + { + first = _queue.First.Data; + return true; + } + } + } + + first = default(TItem); + return false; + } + + /// + /// Removes the head of the queue (node with minimum priority; ties are broken by order of insertion), and sets it to first. + /// Useful for multi-threading, where the queue may become empty between calls to Contains() and Dequeue() + /// Returns true if successful; false if queue was empty + /// O(log n) + /// + public bool TryDequeue(out TItem first) + { + if (_queue.Count > 0) + { + lock (_queue) + { + if (_queue.Count > 0) + { + SimpleNode node = _queue.Dequeue(); + first = node.Data; + RemoveFromNodeCache(node); + return true; + } + } + } + + first = default(TItem); + return false; + } + + /// + /// Attempts to remove an item from the queue. The item does not need to be the head of the queue. + /// Useful for multi-threading, where the queue may become empty between calls to Contains() and Remove() + /// Returns true if the item was successfully removed, false if it wasn't in the queue. + /// If multiple copies of the item are enqueued, only the first one is removed. + /// O(log n) + /// + public bool TryRemove(TItem item) + { + lock(_queue) + { + SimpleNode removeMe; + IList nodes; + if (item == null) + { + if (_nullNodesCache.Count == 0) + { + return false; + } + removeMe = _nullNodesCache[0]; + nodes = _nullNodesCache; + } + else + { + if (!_itemToNodesCache.TryGetValue(item, out nodes)) + { + return false; + } + removeMe = nodes[0]; + if (nodes.Count == 1) + { + _itemToNodesCache.Remove(item); + } + } + _queue.Remove(removeMe); + nodes.Remove(removeMe); + return true; + } + } + + /// + /// Call this method to change the priority of an item. + /// Useful for multi-threading, where the queue may become empty between calls to Contains() and UpdatePriority() + /// If the item is enqueued multiple times, only the first one will be updated. + /// (If your requirements are complex enough that you need to enqueue the same item multiple times and be able + /// to update all of them, please wrap your items in a wrapper class so they can be distinguished). + /// Returns true if the item priority was updated, false otherwise. + /// O(log n) + /// + public bool TryUpdatePriority(TItem item, TPriority priority) + { + lock(_queue) + { + SimpleNode updateMe = GetExistingNode(item); + if(updateMe == null) + { + return false; + } + _queue.UpdatePriority(updateMe, priority); + return true; + } + } + + /// + /// Attempt to get the priority of the given item. + /// Useful for multi-threading, where the queue may become empty between calls to Contains() and GetPriority() + /// If the item is enqueued multiple times, only the priority of the first will be returned. + /// (If your requirements are complex enough that you need to enqueue the same item multiple times and be able + /// to query all their priorities, please wrap your items in a wrapper class so they can be distinguished). + /// Returns true if the item was found in the queue, false otherwise + /// O(1) + /// + public bool TryGetPriority(TItem item, out TPriority priority) + { + lock(_queue) + { + SimpleNode findMe = GetExistingNode(item); + if(findMe == null) + { + priority = default(TPriority); + return false; + } + priority = findMe.Priority; + return true; + } + } + #endregion + + public IEnumerator GetEnumerator() + { + List queueData = new List(); + lock (_queue) + { + //Copy to a separate list because we don't want to 'yield return' inside a lock + foreach(var node in _queue) + { + queueData.Add(node.Data); + } + } + + return queueData.GetEnumerator(); + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + + public bool IsValidQueue() + { + lock(_queue) + { + // Check all items in cache are in the queue + foreach (IList nodes in _itemToNodesCache.Values) + { + foreach (SimpleNode node in nodes) + { + if (!_queue.Contains(node)) + { + return false; + } + } + } + + // Check all items in queue are in cache + foreach (SimpleNode node in _queue) + { + if (GetExistingNode(node.Data) == null) + { + return false; + } + } + + // Check queue structure itself + return _queue.IsValidQueue(); + } + } + } + + /// + /// A simplified priority queue implementation. Is stable, auto-resizes, and thread-safe, at the cost of being slightly slower than + /// FastPriorityQueue + /// This class is kept here for backwards compatibility. It's recommended you use SimplePriorityQueue<TItem, TPriority> + /// + /// The type to enqueue + public class SimplePriorityQueue : SimplePriorityQueue + { + /// + /// Instantiate a new Priority Queue + /// + public SimplePriorityQueue() { } + + /// + /// Instantiate a new Priority Queue + /// + /// The comparer used to compare priority values. Defaults to Comparer<float>.default + public SimplePriorityQueue(IComparer comparer) : base(comparer) { } + + /// + /// Instantiate a new Priority Queue + /// + /// The comparison function to use to compare priority values + public SimplePriorityQueue(Comparison comparer) : base(comparer) { } + } +} \ No newline at end of file diff --git a/Software/Visual_Studio/SideChains/Priority Queue/StablePriorityQueue.cs b/Software/Visual_Studio/SideChains/Priority Queue/StablePriorityQueue.cs new file mode 100644 index 000000000..442b2dbcc --- /dev/null +++ b/Software/Visual_Studio/SideChains/Priority Queue/StablePriorityQueue.cs @@ -0,0 +1,587 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.Runtime.CompilerServices; + +namespace Priority_Queue +{ + /// + /// A copy of FastPriorityQueue which is also stable - that is, when two nodes are enqueued with the same priority, they + /// are always dequeued in the same order. + /// See https://github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp/wiki/Getting-Started for more information + /// + /// The values in the queue. Must extend the StablePriorityQueueNode class + public sealed class StablePriorityQueue : IFixedSizePriorityQueue + where T : StablePriorityQueueNode + { + private int _numNodes; + private T[] _nodes; + private long _numNodesEverEnqueued; + + /// + /// Instantiate a new Priority Queue + /// + /// The max nodes ever allowed to be enqueued (going over this will cause undefined behavior) + public StablePriorityQueue(int maxNodes) + { + #if DEBUG + if (maxNodes <= 0) + { + throw new InvalidOperationException("New queue size cannot be smaller than 1"); + } + #endif + + _numNodes = 0; + _nodes = new T[maxNodes + 1]; + _numNodesEverEnqueued = 0; + } + + /// + /// Returns the number of nodes in the queue. + /// O(1) + /// + public int Count + { + get + { + return _numNodes; + } + } + + /// + /// 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) + /// + public int MaxSize + { + get + { + return _nodes.Length - 1; + } + } + + /// + /// Removes every node from the queue. + /// O(n) (So, don't do this often!) + /// + #if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] + #endif + public void Clear() + { + Array.Clear(_nodes, 1, _numNodes); + _numNodes = 0; + } + + /// + /// Returns (in O(1)!) whether the given node is in the queue. + /// If node is or has been previously added to another queue, the result is undefined unless oldQueue.ResetNode(node) has been called + /// O(1) + /// + #if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] + #endif + public bool Contains(T node) + { + #if DEBUG + if(node == null) + { + throw new ArgumentNullException("node"); + } + if (node.Queue != null && !Equals(node.Queue)) + { + throw new InvalidOperationException("node.Contains was called on a node from another queue. Please call originalQueue.ResetNode() first"); + } + if (node.QueueIndex < 0 || node.QueueIndex >= _nodes.Length) + { + throw new InvalidOperationException("node.QueueIndex has been corrupted. Did you change it manually?"); + } + #endif + + return (_nodes[node.QueueIndex] == node); + } + + /// + /// 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. + /// If node is or has been previously added to another queue, the result is undefined unless oldQueue.ResetNode(node) has been called + /// O(log n) + /// + #if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] + #endif + public void Enqueue(T node, float priority) + { + #if DEBUG + if(node == null) + { + throw new ArgumentNullException("node"); + } + if(_numNodes >= _nodes.Length - 1) + { + throw new InvalidOperationException("Queue is full - node cannot be added: " + node); + } + if (node.Queue != null && !Equals(node.Queue)) + { + throw new InvalidOperationException("node.Enqueue was called on a node from another queue. Please call originalQueue.ResetNode() first"); + } + if (Contains(node)) + { + throw new InvalidOperationException("Node is already enqueued: " + node); + } + node.Queue = this; + #endif + + node.Priority = priority; + _numNodes++; + _nodes[_numNodes] = node; + node.QueueIndex = _numNodes; + node.InsertionIndex = _numNodesEverEnqueued++; + CascadeUp(node); + } + + //Performance appears to be slightly better when this is NOT inlined o_O + #if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] + #endif + private void CascadeUp(T node) + { + //aka Heapify-up + int parent; + if(node.QueueIndex > 1) + { + parent = node.QueueIndex >> 1; + T 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; + T 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; + } + +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + private void CascadeDown(T 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; + T 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 + T 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 + T 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 + T 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 + T 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; + } + } + } + } + + /// + /// 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 + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] + #endif + private bool HasHigherPriority(T higher, T lower) + { + return (higher.Priority < lower.Priority || + (higher.Priority == lower.Priority && higher.InsertionIndex < lower.InsertionIndex)); + } + + /// + /// 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) + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public T Dequeue() + { + #if DEBUG + 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?)"); + } + #endif + + T 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 + T 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; + } + + /// + /// 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) + /// + 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 + + T[] newArray = new T[maxNodes + 1]; + int highestIndexToCopy = Math.Min(maxNodes, _numNodes); + Array.Copy(_nodes, newArray, highestIndexToCopy + 1); + _nodes = newArray; + } + + /// + /// Returns the head of the queue, without removing it (use Dequeue() for that). + /// If the queue is empty, behavior is undefined. + /// O(1) + /// + public T First + { + get + { + #if DEBUG + if(_numNodes <= 0) + { + throw new InvalidOperationException("Cannot call .First on an empty queue"); + } + #endif + + return _nodes[1]; + } + } + + /// + /// This method must be called on a node every time its priority changes while it is in the queue. + /// Forgetting to call this method will result in a corrupted queue! + /// Calling this method on a node not in the queue results in undefined behavior + /// O(log n) + /// + #if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] + #endif + public void UpdatePriority(T node, float priority) + { + #if DEBUG + if(node == null) + { + throw new ArgumentNullException("node"); + } + if (node.Queue != null && !Equals(node.Queue)) + { + throw new InvalidOperationException("node.UpdatePriority was called on a node from another queue"); + } + if (!Contains(node)) + { + throw new InvalidOperationException("Cannot call UpdatePriority() on a node which is not enqueued: " + node); + } + #endif + + node.Priority = priority; + OnNodeUpdated(node); + } + +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + private void OnNodeUpdated(T 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); + } + } + + /// + /// 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) + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public void Remove(T node) + { +#if DEBUG + if(node == null) + { + throw new ArgumentNullException("node"); + } + if (node.Queue != null && !Equals(node.Queue)) + { + throw new InvalidOperationException("node.Remove was called on a node from another queue"); + } + 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 + T 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); + } + + /// + /// By default, nodes that have been previously added to one queue cannot be added to another queue. + /// If you need to do this, please call originalQueue.ResetNode(node) before attempting to add it in the new queue + /// +#if NET_VERSION_4_5 + [MethodImpl(MethodImplOptions.AggressiveInlining)] +#endif + public void ResetNode(T node) + { +#if DEBUG + if (node == null) + { + throw new ArgumentNullException("node"); + } + if (node.Queue != null && !Equals(node.Queue)) + { + throw new InvalidOperationException("node.ResetNode was called on a node from another queue"); + } + if (Contains(node)) + { + throw new InvalidOperationException("node.ResetNode was called on a node that is still in the queue"); + } + + node.Queue = null; +#endif + + node.QueueIndex = 0; + } + + + public IEnumerator GetEnumerator() + { +#if NET_VERSION_4_5 // ArraySegment does not implement IEnumerable before 4.5 + IEnumerable e = new ArraySegment(_nodes, 1, _numNodes); + return e.GetEnumerator(); +#else + for(int i = 1; i <= _numNodes; i++) + yield return _nodes[i]; +#endif + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + + /// + /// Should not be called in production code. + /// Checks to make sure the queue is still in a valid state. Used for testing/debugging the queue. + /// + 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; + } + } +} \ No newline at end of file diff --git a/Software/Visual_Studio/SideChains/Priority Queue/StablePriorityQueueNode.cs b/Software/Visual_Studio/SideChains/Priority Queue/StablePriorityQueueNode.cs new file mode 100644 index 000000000..c794aa0d0 --- /dev/null +++ b/Software/Visual_Studio/SideChains/Priority Queue/StablePriorityQueueNode.cs @@ -0,0 +1,10 @@ +namespace Priority_Queue +{ + public class StablePriorityQueueNode : FastPriorityQueueNode + { + /// + /// Represents the order the node was inserted in + /// + public long InsertionIndex { get; internal set; } + } +} -- cgit v1.3.1