Free cookie consent management tool by TermsFeed Policy Generator

source: stable/HeuristicLab.ExtLibs/HeuristicLab.SimSharp/3.1.1/SimSharp-3.1.1/Collections/GenericPriorityQueue.cs @ 17764

Last change on this file since 17764 was 17053, checked in by abeham, 5 years ago

#2975: merged to stable

File size: 33.8 KB
RevLine 
[16779]1#region License
2/*
3The MIT License (MIT)
4
5Copyright (c) 2013 Daniel "BlueRaja" Pflughoeft
6
7Permission is hereby granted, free of charge, to any person obtaining a copy
8of this software and associated documentation files (the "Software"), to deal
9in the Software without restriction, including without limitation the rights
10to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11copies of the Software, and to permit persons to whom the Software is
12furnished to do so, subject to the following conditions:
13
14The above copyright notice and this permission notice shall be included in
15all copies or substantial portions of the Software.
16
17THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
18IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
19FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
20AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
21LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
22OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
23THE SOFTWARE.
24 */
25#endregion
26
27using System;
28using System.Collections;
29using System.Collections.Generic;
30using System.Runtime.CompilerServices;
31
32namespace SimSharp {
33  /// <summary>
34  /// A copy of StablePriorityQueue which also has generic explicit priority-type
35  /// </summary>
36  /// <remarks>
37  /// Original sources from https://github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp
38  /// </remarks>
39  /// <typeparam name="TItem">The values in the queue.  Must extend the GenericPriorityQueue class</typeparam>
40  /// <typeparam name="TPriority">The priority-type.  Must extend IComparable&lt;TPriority&gt;</typeparam>
41  public sealed class GenericPriorityQueue<TItem, TPriority> : IEnumerable<TItem>
42      where TItem : GenericPriorityQueueNode<TPriority>
43      where TPriority : IComparable<TPriority> {
44    private int _numNodes;
45    private TItem[] _nodes;
46    private long _numNodesEverEnqueued;
47    private readonly Comparison<TPriority> _comparer;
48
49    /// <summary>
50    /// Instantiate a new Priority Queue
51    /// </summary>
52    /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
53    public GenericPriorityQueue(int maxNodes) : this(maxNodes, Comparer<TPriority>.Default) { }
54
55    /// <summary>
56    /// Instantiate a new Priority Queue
57    /// </summary>
58    /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
59    /// <param name="comparer">The comparer used to compare TPriority values.</param>
60    public GenericPriorityQueue(int maxNodes, IComparer<TPriority> comparer) : this(maxNodes, comparer.Compare) { }
61
62    /// <summary>
63    /// Instantiate a new Priority Queue
64    /// </summary>
65    /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
66    /// <param name="comparer">The comparison function to use to compare TPriority values</param>
67    public GenericPriorityQueue(int maxNodes, Comparison<TPriority> comparer) {
68#if DEBUG
69      if (maxNodes <= 0) {
70        throw new InvalidOperationException("New queue size cannot be smaller than 1");
71      }
72#endif
73
74      _numNodes = 0;
75      _nodes = new TItem[maxNodes + 1];
76      _numNodesEverEnqueued = 0;
77      _comparer = comparer;
78    }
79
80    /// <summary>
81    /// Returns the number of nodes in the queue.
82    /// O(1)
83    /// </summary>
84    public int Count {
85      get {
86        return _numNodes;
87      }
88    }
89
90    /// <summary>
91    /// Returns the maximum number of items that can be enqueued at once in this queue.  Once you hit this number (ie. once Count == MaxSize),
92    /// attempting to enqueue another item will cause undefined behavior.  O(1)
93    /// </summary>
94    public int MaxSize {
95      get {
96        return _nodes.Length - 1;
97      }
98    }
99
100    /// <summary>
101    /// Removes every node from the queue.
102    /// O(n) (So, don't do this often!)
103    /// </summary>
104    [MethodImpl(MethodImplOptions.AggressiveInlining)]
105    public void Clear() {
106      Array.Clear(_nodes, 1, _numNodes);
107      _numNodes = 0;
108    }
109
110    /// <summary>
111    /// Returns (in O(1)!) whether the given node is in the queue.  O(1)
112    /// </summary>
113    [MethodImpl(MethodImplOptions.AggressiveInlining)]
114    public bool Contains(TItem node) {
115#if DEBUG
116      if (node == null) {
117        throw new ArgumentNullException("node");
118      }
119      if (node.QueueIndex < 0 || node.QueueIndex >= _nodes.Length) {
120        throw new InvalidOperationException("node.QueueIndex has been corrupted. Did you change it manually? Or add this node to another queue?");
121      }
122#endif
123
124      return (_nodes[node.QueueIndex] == node);
125    }
126
127    /// <summary>
128    /// Enqueue a node to the priority queue.  Lower values are placed in front. Ties are broken by first-in-first-out.
129    /// If the queue is full, the result is undefined.
130    /// If the node is already enqueued, the result is undefined.
131    /// O(log n)
132    /// </summary>
133    [MethodImpl(MethodImplOptions.AggressiveInlining)]
134    public void Enqueue(TItem node, TPriority priority) {
135#if DEBUG
136      if (node == null) {
137        throw new ArgumentNullException("node");
138      }
139      if (_numNodes >= _nodes.Length - 1) {
140        throw new InvalidOperationException("Queue is full - node cannot be added: " + node);
141      }
142      if (Contains(node)) {
143        throw new InvalidOperationException("Node is already enqueued: " + node);
144      }
145#endif
146
147      node.Priority = priority;
148      _numNodes++;
149      _nodes[_numNodes] = node;
150      node.QueueIndex = _numNodes;
151      node.InsertionIndex = _numNodesEverEnqueued++;
152      CascadeUp(node);
153    }
154
155    [MethodImpl(MethodImplOptions.AggressiveInlining)]
156    private void CascadeUp(TItem node) {
157      //aka Heapify-up
158      int parent;
159      if (node.QueueIndex > 1) {
160        parent = node.QueueIndex >> 1;
161        TItem parentNode = _nodes[parent];
162        if (HasHigherPriority(parentNode, node))
163          return;
164
165        //Node has lower priority value, so move parent down the heap to make room
166        _nodes[node.QueueIndex] = parentNode;
167        parentNode.QueueIndex = node.QueueIndex;
168
169        node.QueueIndex = parent;
170      } else {
171        return;
172      }
173      while (parent > 1) {
174        parent >>= 1;
175        TItem parentNode = _nodes[parent];
176        if (HasHigherPriority(parentNode, node))
177          break;
178
179        //Node has lower priority value, so move parent down the heap to make room
180        _nodes[node.QueueIndex] = parentNode;
181        parentNode.QueueIndex = node.QueueIndex;
182
183        node.QueueIndex = parent;
184      }
185      _nodes[node.QueueIndex] = node;
186    }
187
188    [MethodImpl(MethodImplOptions.AggressiveInlining)]
189    private void CascadeDown(TItem node) {
190      //aka Heapify-down
191      int finalQueueIndex = node.QueueIndex;
192      int childLeftIndex = 2 * finalQueueIndex;
193
194      // If leaf node, we're done
195      if (childLeftIndex > _numNodes) {
196        return;
197      }
198
199      // Check if the left-child is higher-priority than the current node
200      int childRightIndex = childLeftIndex + 1;
201      TItem childLeft = _nodes[childLeftIndex];
202      if (HasHigherPriority(childLeft, node)) {
203        // Check if there is a right child. If not, swap and finish.
204        if (childRightIndex > _numNodes) {
205          node.QueueIndex = childLeftIndex;
206          childLeft.QueueIndex = finalQueueIndex;
207          _nodes[finalQueueIndex] = childLeft;
208          _nodes[childLeftIndex] = node;
209          return;
210        }
211        // Check if the left-child is higher-priority than the right-child
212        TItem childRight = _nodes[childRightIndex];
213        if (HasHigherPriority(childLeft, childRight)) {
214          // left is highest, move it up and continue
215          childLeft.QueueIndex = finalQueueIndex;
216          _nodes[finalQueueIndex] = childLeft;
217          finalQueueIndex = childLeftIndex;
218        } else {
219          // right is even higher, move it up and continue
220          childRight.QueueIndex = finalQueueIndex;
221          _nodes[finalQueueIndex] = childRight;
222          finalQueueIndex = childRightIndex;
223        }
224      }
225      // Not swapping with left-child, does right-child exist?
226      else if (childRightIndex > _numNodes) {
227        return;
228      } else {
229        // Check if the right-child is higher-priority than the current node
230        TItem childRight = _nodes[childRightIndex];
231        if (HasHigherPriority(childRight, node)) {
232          childRight.QueueIndex = finalQueueIndex;
233          _nodes[finalQueueIndex] = childRight;
234          finalQueueIndex = childRightIndex;
235        }
236        // Neither child is higher-priority than current, so finish and stop.
237        else {
238          return;
239        }
240      }
241
242      while (true) {
243        childLeftIndex = 2 * finalQueueIndex;
244
245        // If leaf node, we're done
246        if (childLeftIndex > _numNodes) {
247          node.QueueIndex = finalQueueIndex;
248          _nodes[finalQueueIndex] = node;
249          break;
250        }
251
252        // Check if the left-child is higher-priority than the current node
253        childRightIndex = childLeftIndex + 1;
254        childLeft = _nodes[childLeftIndex];
255        if (HasHigherPriority(childLeft, node)) {
256          // Check if there is a right child. If not, swap and finish.
257          if (childRightIndex > _numNodes) {
258            node.QueueIndex = childLeftIndex;
259            childLeft.QueueIndex = finalQueueIndex;
260            _nodes[finalQueueIndex] = childLeft;
261            _nodes[childLeftIndex] = node;
262            break;
263          }
264          // Check if the left-child is higher-priority than the right-child
265          TItem childRight = _nodes[childRightIndex];
266          if (HasHigherPriority(childLeft, childRight)) {
267            // left is highest, move it up and continue
268            childLeft.QueueIndex = finalQueueIndex;
269            _nodes[finalQueueIndex] = childLeft;
270            finalQueueIndex = childLeftIndex;
271          } else {
272            // right is even higher, move it up and continue
273            childRight.QueueIndex = finalQueueIndex;
274            _nodes[finalQueueIndex] = childRight;
275            finalQueueIndex = childRightIndex;
276          }
277        }
278        // Not swapping with left-child, does right-child exist?
279        else if (childRightIndex > _numNodes) {
280          node.QueueIndex = finalQueueIndex;
281          _nodes[finalQueueIndex] = node;
282          break;
283        } else {
284          // Check if the right-child is higher-priority than the current node
285          TItem childRight = _nodes[childRightIndex];
286          if (HasHigherPriority(childRight, node)) {
287            childRight.QueueIndex = finalQueueIndex;
288            _nodes[finalQueueIndex] = childRight;
289            finalQueueIndex = childRightIndex;
290          }
291          // Neither child is higher-priority than current, so finish and stop.
292          else {
293            node.QueueIndex = finalQueueIndex;
294            _nodes[finalQueueIndex] = node;
295            break;
296          }
297        }
298      }
299    }
300
301    /// <summary>
302    /// Returns true if 'higher' has higher priority than 'lower', false otherwise.
303    /// Note that calling HasHigherPriority(node, node) (ie. both arguments the same node) will return false
304    /// </summary>
305    [MethodImpl(MethodImplOptions.AggressiveInlining)]
306    private bool HasHigherPriority(TItem higher, TItem lower) {
307      var cmp = _comparer(higher.Priority, lower.Priority);
308      return (cmp < 0 || (cmp == 0 && higher.InsertionIndex < lower.InsertionIndex));
309    }
310
311    /// <summary>
312    /// Removes the head of the queue (node with minimum priority; ties are broken by order of insertion), and returns it.
313    /// If queue is empty, result is undefined
314    /// O(log n)
315    /// </summary>
316    [MethodImpl(MethodImplOptions.AggressiveInlining)]
317    public TItem Dequeue() {
318#if DEBUG
319      if (_numNodes <= 0) {
320        throw new InvalidOperationException("Cannot call Dequeue() on an empty queue");
321      }
322
323      if (!IsValidQueue()) {
324        throw new InvalidOperationException("Queue has been corrupted (Did you update a node priority manually instead of calling UpdatePriority()?" +
325                                            "Or add the same node to two different queues?)");
326      }
327#endif
328
329      TItem returnMe = _nodes[1];
330      //If the node is already the last node, we can remove it immediately
331      if (_numNodes == 1) {
332        _nodes[1] = null;
333        _numNodes = 0;
334        return returnMe;
335      }
336
337      //Swap the node with the last node
338      TItem formerLastNode = _nodes[_numNodes];
339      _nodes[1] = formerLastNode;
340      formerLastNode.QueueIndex = 1;
341      _nodes[_numNodes] = null;
342      _numNodes--;
343
344      //Now bubble formerLastNode (which is no longer the last node) down
345      CascadeDown(formerLastNode);
346      return returnMe;
347    }
348
349    /// <summary>
350    /// Resize the queue so it can accept more nodes.  All currently enqueued nodes are remain.
351    /// Attempting to decrease the queue size to a size too small to hold the existing nodes results in undefined behavior
352    /// O(n)
353    /// </summary>
354    public void Resize(int maxNodes) {
355#if DEBUG
356      if (maxNodes <= 0) {
357        throw new InvalidOperationException("Queue size cannot be smaller than 1");
358      }
359
360      if (maxNodes < _numNodes) {
361        throw new InvalidOperationException("Called Resize(" + maxNodes + "), but current queue contains " + _numNodes + " nodes");
362      }
363#endif
364
365      TItem[] newArray = new TItem[maxNodes + 1];
366      int highestIndexToCopy = Math.Min(maxNodes, _numNodes);
367      Array.Copy(_nodes, newArray, highestIndexToCopy + 1);
368      _nodes = newArray;
369    }
370
371    /// <summary>
372    /// Returns the head of the queue, without removing it (use Dequeue() for that).
373    /// If the queue is empty, behavior is undefined.
374    /// O(1)
375    /// </summary>
376    public TItem First {
377      get {
378#if DEBUG
379        if (_numNodes <= 0) {
380          throw new InvalidOperationException("Cannot call .First on an empty queue");
381        }
382#endif
383
384        return _nodes[1];
385      }
386    }
387
388    /// <summary>
389    /// This method must be called on a node every time its priority changes while it is in the queue. 
390    /// <b>Forgetting to call this method will result in a corrupted queue!</b>
391    /// Calling this method on a node not in the queue results in undefined behavior
392    /// O(log n)
393    /// </summary>
394    [MethodImpl(MethodImplOptions.AggressiveInlining)]
395    public void UpdatePriority(TItem node, TPriority priority) {
396#if DEBUG
397      if (node == null) {
398        throw new ArgumentNullException("node");
399      }
400      if (!Contains(node)) {
401        throw new InvalidOperationException("Cannot call UpdatePriority() on a node which is not enqueued: " + node);
402      }
403#endif
404
405      node.Priority = priority;
406      OnNodeUpdated(node);
407    }
408
409    [MethodImpl(MethodImplOptions.AggressiveInlining)]
410    private void OnNodeUpdated(TItem node) {
411      //Bubble the updated node up or down as appropriate
412      int parentIndex = node.QueueIndex >> 1;
413
414      if (parentIndex > 0 && HasHigherPriority(node, _nodes[parentIndex])) {
415        CascadeUp(node);
416      } else {
417        //Note that CascadeDown will be called if parentNode == node (that is, node is the root)
418        CascadeDown(node);
419      }
420    }
421
422    /// <summary>
423    /// Removes a node from the queue.  The node does not need to be the head of the queue. 
424    /// If the node is not in the queue, the result is undefined.  If unsure, check Contains() first
425    /// O(log n)
426    /// </summary>
427    [MethodImpl(MethodImplOptions.AggressiveInlining)]
428    public void Remove(TItem node) {
429#if DEBUG
430      if (node == null) {
431        throw new ArgumentNullException("node");
432      }
433      if (!Contains(node)) {
434        throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + node);
435      }
436#endif
437
438      //If the node is already the last node, we can remove it immediately
439      if (node.QueueIndex == _numNodes) {
440        _nodes[_numNodes] = null;
441        _numNodes--;
442        return;
443      }
444
445      //Swap the node with the last node
446      TItem formerLastNode = _nodes[_numNodes];
447      _nodes[node.QueueIndex] = formerLastNode;
448      formerLastNode.QueueIndex = node.QueueIndex;
449      _nodes[_numNodes] = null;
450      _numNodes--;
451
452      //Now bubble formerLastNode (which is no longer the last node) up or down as appropriate
453      OnNodeUpdated(formerLastNode);
454    }
455
456    public IEnumerator<TItem> GetEnumerator() {
457      IEnumerable<TItem> e = new ArraySegment<TItem>(_nodes, 1, _numNodes);
458      return e.GetEnumerator();
459    }
460
461    IEnumerator IEnumerable.GetEnumerator() {
462      return GetEnumerator();
463    }
464
465    /// <summary>
466    /// <b>Should not be called in production code.</b>
467    /// Checks to make sure the queue is still in a valid state.  Used for testing/debugging the queue.
468    /// </summary>
469    public bool IsValidQueue() {
470      for (int i = 1; i < _nodes.Length; i++) {
471        if (_nodes[i] != null) {
472          int childLeftIndex = 2 * i;
473          if (childLeftIndex < _nodes.Length && _nodes[childLeftIndex] != null && HasHigherPriority(_nodes[childLeftIndex], _nodes[i]))
474            return false;
475
476          int childRightIndex = childLeftIndex + 1;
477          if (childRightIndex < _nodes.Length && _nodes[childRightIndex] != null && HasHigherPriority(_nodes[childRightIndex], _nodes[i]))
478            return false;
479        }
480      }
481      return true;
482    }
483  }
484
485  /// <summary>
486  /// A copy of StablePriorityQueue where the items themselves contain an implicit priority.
487  /// </summary>
488  /// <remarks>
489  /// Either <typeparamref name="TItem"/> implements IComparable&lt;typeparamref name="TItem"/>&gt; or
490  /// a custom comparer must be provided.
491  /// </remarks>
492  /// <typeparam name="TItem">The values in the queue. Must extend the GenericPriorityQueueNode class</typeparam>
493  public sealed class GenericPriorityQueue<TItem> : IEnumerable<TItem>
494      where TItem : GenericPriorityQueueNode {
495    private int _numNodes;
496    private TItem[] _nodes;
497    private long _numNodesEverEnqueued;
498    private readonly Comparison<TItem> _comparer;
499
500    /// <summary>
501    /// Instantiate a new Priority Queue
502    /// </summary>
503    /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
504    public GenericPriorityQueue(int maxNodes) : this(maxNodes, Comparer<TItem>.Default) { }
505
506    /// <summary>
507    /// Instantiate a new Priority Queue
508    /// </summary>
509    /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
510    /// <param name="comparer">The comparer used to compare TPriority values.</param>
511    public GenericPriorityQueue(int maxNodes, IComparer<TItem> comparer) : this(maxNodes, comparer.Compare) { }
512
513    /// <summary>
514    /// Instantiate a new Priority Queue
515    /// </summary>
516    /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
517    /// <param name="comparer">The comparison function to use to compare TPriority values</param>
518    public GenericPriorityQueue(int maxNodes, Comparison<TItem> comparer) {
519#if DEBUG
520      if (maxNodes <= 0) {
521        throw new InvalidOperationException("New queue size cannot be smaller than 1");
522      }
523#endif
524
525      _numNodes = 0;
526      _nodes = new TItem[maxNodes + 1];
527      _numNodesEverEnqueued = 0;
528      _comparer = comparer;
529    }
530
531    /// <summary>
532    /// Returns the number of nodes in the queue.
533    /// O(1)
534    /// </summary>
535    public int Count {
536      get {
537        return _numNodes;
538      }
539    }
540
541    /// <summary>
542    /// Returns the maximum number of items that can be enqueued at once in this queue.  Once you hit this number (ie. once Count == MaxSize),
543    /// attempting to enqueue another item will cause undefined behavior.  O(1)
544    /// </summary>
545    public int MaxSize {
546      get {
547        return _nodes.Length - 1;
548      }
549    }
550
551    /// <summary>
552    /// Removes every node from the queue.
553    /// O(n) (So, don't do this often!)
554    /// </summary>
555    [MethodImpl(MethodImplOptions.AggressiveInlining)]
556    public void Clear() {
557      Array.Clear(_nodes, 1, _numNodes);
558      _numNodes = 0;
559    }
560
561    /// <summary>
562    /// Returns (in O(1)!) whether the given node is in the queue.  O(1)
563    /// </summary>
564    [MethodImpl(MethodImplOptions.AggressiveInlining)]
565    public bool Contains(TItem node) {
566#if DEBUG
567      if (node == null) {
568        throw new ArgumentNullException("node");
569      }
570      if (node.QueueIndex < 0 || node.QueueIndex >= _nodes.Length) {
571        throw new InvalidOperationException("node.QueueIndex has been corrupted. Did you change it manually? Or add this node to another queue?");
572      }
573#endif
574
575      return (_nodes[node.QueueIndex] == node);
576    }
577
578    /// <summary>
579    /// Enqueue a node to the priority queue.  Lower values are placed in front. Ties are broken by first-in-first-out.
580    /// If the queue is full, the result is undefined.
581    /// If the node is already enqueued, the result is undefined.
582    /// O(log n)
583    /// </summary>
584    [MethodImpl(MethodImplOptions.AggressiveInlining)]
585    public void Enqueue(TItem node) {
586#if DEBUG
587      if (node == null) {
588        throw new ArgumentNullException("node");
589      }
590      if (_numNodes >= _nodes.Length - 1) {
591        throw new InvalidOperationException("Queue is full - node cannot be added: " + node);
592      }
593      if (Contains(node)) {
594        throw new InvalidOperationException("Node is already enqueued: " + node);
595      }
596#endif
597     
598      _numNodes++;
599      _nodes[_numNodes] = node;
600      node.QueueIndex = _numNodes;
601      node.InsertionIndex = _numNodesEverEnqueued++;
602      CascadeUp(node);
603    }
604
605    [MethodImpl(MethodImplOptions.AggressiveInlining)]
606    private void CascadeUp(TItem node) {
607      //aka Heapify-up
608      int parent;
609      if (node.QueueIndex > 1) {
610        parent = node.QueueIndex >> 1;
611        TItem parentNode = _nodes[parent];
612        if (HasHigherPriority(parentNode, node))
613          return;
614
615        //Node has lower priority value, so move parent down the heap to make room
616        _nodes[node.QueueIndex] = parentNode;
617        parentNode.QueueIndex = node.QueueIndex;
618
619        node.QueueIndex = parent;
620      } else {
621        return;
622      }
623      while (parent > 1) {
624        parent >>= 1;
625        TItem parentNode = _nodes[parent];
626        if (HasHigherPriority(parentNode, node))
627          break;
628
629        //Node has lower priority value, so move parent down the heap to make room
630        _nodes[node.QueueIndex] = parentNode;
631        parentNode.QueueIndex = node.QueueIndex;
632
633        node.QueueIndex = parent;
634      }
635      _nodes[node.QueueIndex] = node;
636    }
637
638    [MethodImpl(MethodImplOptions.AggressiveInlining)]
639    private void CascadeDown(TItem node) {
640      //aka Heapify-down
641      int finalQueueIndex = node.QueueIndex;
642      int childLeftIndex = 2 * finalQueueIndex;
643
644      // If leaf node, we're done
645      if (childLeftIndex > _numNodes) {
646        return;
647      }
648
649      // Check if the left-child is higher-priority than the current node
650      int childRightIndex = childLeftIndex + 1;
651      TItem childLeft = _nodes[childLeftIndex];
652      if (HasHigherPriority(childLeft, node)) {
653        // Check if there is a right child. If not, swap and finish.
654        if (childRightIndex > _numNodes) {
655          node.QueueIndex = childLeftIndex;
656          childLeft.QueueIndex = finalQueueIndex;
657          _nodes[finalQueueIndex] = childLeft;
658          _nodes[childLeftIndex] = node;
659          return;
660        }
661        // Check if the left-child is higher-priority than the right-child
662        TItem childRight = _nodes[childRightIndex];
663        if (HasHigherPriority(childLeft, childRight)) {
664          // left is highest, move it up and continue
665          childLeft.QueueIndex = finalQueueIndex;
666          _nodes[finalQueueIndex] = childLeft;
667          finalQueueIndex = childLeftIndex;
668        } else {
669          // right is even higher, move it up and continue
670          childRight.QueueIndex = finalQueueIndex;
671          _nodes[finalQueueIndex] = childRight;
672          finalQueueIndex = childRightIndex;
673        }
674      }
675      // Not swapping with left-child, does right-child exist?
676      else if (childRightIndex > _numNodes) {
677        return;
678      } else {
679        // Check if the right-child is higher-priority than the current node
680        TItem childRight = _nodes[childRightIndex];
681        if (HasHigherPriority(childRight, node)) {
682          childRight.QueueIndex = finalQueueIndex;
683          _nodes[finalQueueIndex] = childRight;
684          finalQueueIndex = childRightIndex;
685        }
686        // Neither child is higher-priority than current, so finish and stop.
687        else {
688          return;
689        }
690      }
691
692      while (true) {
693        childLeftIndex = 2 * finalQueueIndex;
694
695        // If leaf node, we're done
696        if (childLeftIndex > _numNodes) {
697          node.QueueIndex = finalQueueIndex;
698          _nodes[finalQueueIndex] = node;
699          break;
700        }
701
702        // Check if the left-child is higher-priority than the current node
703        childRightIndex = childLeftIndex + 1;
704        childLeft = _nodes[childLeftIndex];
705        if (HasHigherPriority(childLeft, node)) {
706          // Check if there is a right child. If not, swap and finish.
707          if (childRightIndex > _numNodes) {
708            node.QueueIndex = childLeftIndex;
709            childLeft.QueueIndex = finalQueueIndex;
710            _nodes[finalQueueIndex] = childLeft;
711            _nodes[childLeftIndex] = node;
712            break;
713          }
714          // Check if the left-child is higher-priority than the right-child
715          TItem childRight = _nodes[childRightIndex];
716          if (HasHigherPriority(childLeft, childRight)) {
717            // left is highest, move it up and continue
718            childLeft.QueueIndex = finalQueueIndex;
719            _nodes[finalQueueIndex] = childLeft;
720            finalQueueIndex = childLeftIndex;
721          } else {
722            // right is even higher, move it up and continue
723            childRight.QueueIndex = finalQueueIndex;
724            _nodes[finalQueueIndex] = childRight;
725            finalQueueIndex = childRightIndex;
726          }
727        }
728        // Not swapping with left-child, does right-child exist?
729        else if (childRightIndex > _numNodes) {
730          node.QueueIndex = finalQueueIndex;
731          _nodes[finalQueueIndex] = node;
732          break;
733        } else {
734          // Check if the right-child is higher-priority than the current node
735          TItem childRight = _nodes[childRightIndex];
736          if (HasHigherPriority(childRight, node)) {
737            childRight.QueueIndex = finalQueueIndex;
738            _nodes[finalQueueIndex] = childRight;
739            finalQueueIndex = childRightIndex;
740          }
741          // Neither child is higher-priority than current, so finish and stop.
742          else {
743            node.QueueIndex = finalQueueIndex;
744            _nodes[finalQueueIndex] = node;
745            break;
746          }
747        }
748      }
749    }
750
751    /// <summary>
752    /// Returns true if 'higher' has higher priority than 'lower', false otherwise.
753    /// Note that calling HasHigherPriority(node, node) (ie. both arguments the same node) will return false
754    /// </summary>
755    [MethodImpl(MethodImplOptions.AggressiveInlining)]
756    private bool HasHigherPriority(TItem higher, TItem lower) {
757      var cmp = _comparer(higher, lower);
758      return (cmp < 0 || (cmp == 0 && higher.InsertionIndex < lower.InsertionIndex));
759    }
760
761    /// <summary>
762    /// Removes the head of the queue (node with minimum priority; ties are broken by order of insertion), and returns it.
763    /// If queue is empty, result is undefined
764    /// O(log n)
765    /// </summary>
766    [MethodImpl(MethodImplOptions.AggressiveInlining)]
767    public TItem Dequeue() {
768#if DEBUG
769      if (_numNodes <= 0) {
770        throw new InvalidOperationException("Cannot call Dequeue() on an empty queue");
771      }
772
773      if (!IsValidQueue()) {
774        throw new InvalidOperationException("Queue has been corrupted (Did you update a node priority manually instead of calling UpdatePriority()?" +
775                                            "Or add the same node to two different queues?)");
776      }
777#endif
778
779      TItem returnMe = _nodes[1];
780      //If the node is already the last node, we can remove it immediately
781      if (_numNodes == 1) {
782        _nodes[1] = null;
783        _numNodes = 0;
784        return returnMe;
785      }
786
787      //Swap the node with the last node
788      TItem formerLastNode = _nodes[_numNodes];
789      _nodes[1] = formerLastNode;
790      formerLastNode.QueueIndex = 1;
791      _nodes[_numNodes] = null;
792      _numNodes--;
793
794      //Now bubble formerLastNode (which is no longer the last node) down
795      CascadeDown(formerLastNode);
796      return returnMe;
797    }
798
799    /// <summary>
800    /// Resize the queue so it can accept more nodes.  All currently enqueued nodes are remain.
801    /// Attempting to decrease the queue size to a size too small to hold the existing nodes results in undefined behavior
802    /// O(n)
803    /// </summary>
804    public void Resize(int maxNodes) {
805#if DEBUG
806      if (maxNodes <= 0) {
807        throw new InvalidOperationException("Queue size cannot be smaller than 1");
808      }
809
810      if (maxNodes < _numNodes) {
811        throw new InvalidOperationException("Called Resize(" + maxNodes + "), but current queue contains " + _numNodes + " nodes");
812      }
813#endif
814
815      TItem[] newArray = new TItem[maxNodes + 1];
816      int highestIndexToCopy = Math.Min(maxNodes, _numNodes);
817      Array.Copy(_nodes, newArray, highestIndexToCopy + 1);
818      _nodes = newArray;
819    }
820
821    /// <summary>
822    /// Returns the head of the queue, without removing it (use Dequeue() for that).
823    /// If the queue is empty, behavior is undefined.
824    /// O(1)
825    /// </summary>
826    public TItem First {
827      get {
828#if DEBUG
829        if (_numNodes <= 0) {
830          throw new InvalidOperationException("Cannot call .First on an empty queue");
831        }
832#endif
833
834        return _nodes[1];
835      }
836    }
837
838    /// <summary>
839    /// This method must be called on a node every time its priority changes while it is in the queue. 
840    /// <b>Forgetting to call this method will result in a corrupted queue!</b>
841    /// Calling this method on a node not in the queue results in undefined behavior
842    /// O(log n)
843    /// </summary>
844    [MethodImpl(MethodImplOptions.AggressiveInlining)]
845    public void UpdatePriority(TItem node) {
846#if DEBUG
847      if (node == null) {
848        throw new ArgumentNullException("node");
849      }
850      if (!Contains(node)) {
851        throw new InvalidOperationException("Cannot call UpdatePriority() on a node which is not enqueued: " + node);
852      }
853#endif
854     
855      OnNodeUpdated(node);
856    }
857
858    [MethodImpl(MethodImplOptions.AggressiveInlining)]
859    private void OnNodeUpdated(TItem node) {
860      //Bubble the updated node up or down as appropriate
861      int parentIndex = node.QueueIndex >> 1;
862
863      if (parentIndex > 0 && HasHigherPriority(node, _nodes[parentIndex])) {
864        CascadeUp(node);
865      } else {
866        //Note that CascadeDown will be called if parentNode == node (that is, node is the root)
867        CascadeDown(node);
868      }
869    }
870
871    /// <summary>
872    /// Removes a node from the queue.  The node does not need to be the head of the queue. 
873    /// If the node is not in the queue, the result is undefined.  If unsure, check Contains() first
874    /// O(log n)
875    /// </summary>
876    [MethodImpl(MethodImplOptions.AggressiveInlining)]
877    public void Remove(TItem node) {
878#if DEBUG
879      if (node == null) {
880        throw new ArgumentNullException("node");
881      }
882      if (!Contains(node)) {
883        throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + node);
884      }
885#endif
886
887      //If the node is already the last node, we can remove it immediately
888      if (node.QueueIndex == _numNodes) {
889        _nodes[_numNodes] = null;
890        _numNodes--;
891        return;
892      }
893
894      //Swap the node with the last node
895      TItem formerLastNode = _nodes[_numNodes];
896      _nodes[node.QueueIndex] = formerLastNode;
897      formerLastNode.QueueIndex = node.QueueIndex;
898      _nodes[_numNodes] = null;
899      _numNodes--;
900
901      //Now bubble formerLastNode (which is no longer the last node) up or down as appropriate
902      OnNodeUpdated(formerLastNode);
903    }
904
905    public IEnumerator<TItem> GetEnumerator() {
906      IEnumerable<TItem> e = new ArraySegment<TItem>(_nodes, 1, _numNodes);
907      return e.GetEnumerator();
908    }
909
910    IEnumerator IEnumerable.GetEnumerator() {
911      return GetEnumerator();
912    }
913
914    /// <summary>
915    /// <b>Should not be called in production code.</b>
916    /// Checks to make sure the queue is still in a valid state.  Used for testing/debugging the queue.
917    /// </summary>
918    public bool IsValidQueue() {
919      for (int i = 1; i < _nodes.Length; i++) {
920        if (_nodes[i] != null) {
921          int childLeftIndex = 2 * i;
922          if (childLeftIndex < _nodes.Length && _nodes[childLeftIndex] != null && HasHigherPriority(_nodes[childLeftIndex], _nodes[i]))
923            return false;
924
925          int childRightIndex = childLeftIndex + 1;
926          if (childRightIndex < _nodes.Length && _nodes[childRightIndex] != null && HasHigherPriority(_nodes[childRightIndex], _nodes[i]))
927            return false;
928        }
929      }
930      return true;
931    }
932  }
933}
Note: See TracBrowser for help on using the repository browser.