using UnityEngine; using System; using System.Collections; using System.Collections.Generic; namespace DigitalOpus.MB.Core { /// From https://www.codeproject.com/Articles/126751/Priority-queue-in-Csharp-with-help-of-heap-data-st.aspx /// /// Priority queue based on binary heap, /// Elements with minimum priority dequeued first /// /// Type of priorities /// Type of values public class PriorityQueue : ICollection> { public List> _baseHeap; private IComparer _comparer; #region Constructors /// /// Initializes a new instance of priority queue with default initial capacity and default priority comparer /// public PriorityQueue() : this(Comparer.Default) { } /// /// Initializes a new instance of priority queue with specified initial capacity and default priority comparer /// /// initial capacity public PriorityQueue(int capacity) : this(capacity, Comparer.Default) { } /// /// Initializes a new instance of priority queue with specified initial capacity and specified priority comparer /// /// initial capacity /// priority comparer public PriorityQueue(int capacity, IComparer comparer) { if (comparer == null) throw new ArgumentNullException(); _baseHeap = new List>(capacity); _comparer = comparer; } /// /// Initializes a new instance of priority queue with default initial capacity and specified priority comparer /// /// priority comparer public PriorityQueue(IComparer comparer) { if (comparer == null) throw new ArgumentNullException(); _baseHeap = new List>(); _comparer = comparer; } /// /// Initializes a new instance of priority queue with specified data and default priority comparer /// /// data to be inserted into priority queue public PriorityQueue(IEnumerable> data) : this(data, Comparer.Default) { } /// /// Initializes a new instance of priority queue with specified data and specified priority comparer /// /// data to be inserted into priority queue /// priority comparer public PriorityQueue(IEnumerable> data, IComparer comparer) { if (data == null || comparer == null) throw new ArgumentNullException(); _comparer = comparer; _baseHeap = new List>(data); // heapify data for (int pos = _baseHeap.Count / 2 - 1; pos >= 0; pos--) HeapifyFromBeginningToEnd(pos); } #endregion #region Merging /// /// Merges two priority queues /// /// first priority queue /// second priority queue /// resultant priority queue /// /// source priority queues must have equal comparers, /// otherwise will be thrown /// public static PriorityQueue MergeQueues(PriorityQueue pq1, PriorityQueue pq2) { if (pq1 == null || pq2 == null) throw new ArgumentNullException(); if (pq1._comparer != pq2._comparer) throw new InvalidOperationException("Priority queues to be merged must have equal comparers"); return MergeQueues(pq1, pq2, pq1._comparer); } /// /// Merges two priority queues and sets specified comparer for resultant priority queue /// /// first priority queue /// second priority queue /// comparer for resultant priority queue /// resultant priority queue public static PriorityQueue MergeQueues(PriorityQueue pq1, PriorityQueue pq2, IComparer comparer) { if (pq1 == null || pq2 == null || comparer == null) throw new ArgumentNullException(); // merge data PriorityQueue result = new PriorityQueue(pq1.Count + pq2.Count, pq1._comparer); result._baseHeap.AddRange(pq1._baseHeap); result._baseHeap.AddRange(pq2._baseHeap); // heapify data for (int pos = result._baseHeap.Count / 2 - 1; pos >= 0; pos--) result.HeapifyFromBeginningToEnd(pos); return result; } #endregion #region Priority queue operations /// /// Enqueues element into priority queue /// /// element priority /// element value public void Enqueue(TPriority priority, TValue value) { Insert(priority, value); } /// /// Dequeues element with minimum priority and return its priority and value as /// /// priority and value of the dequeued element /// /// Method throws if priority queue is empty /// public KeyValuePair Dequeue() { if (!IsEmpty) { KeyValuePair result = _baseHeap[0]; DeleteRoot(); return result; } else throw new InvalidOperationException("Priority queue is empty"); } /// /// Dequeues element with minimum priority and return its value /// /// value of the dequeued element /// /// Method throws if priority queue is empty /// public TValue DequeueValue() { return Dequeue().Value; } /// /// Returns priority and value of the element with minimun priority, without removing it from the queue /// /// priority and value of the element with minimum priority /// /// Method throws if priority queue is empty /// public KeyValuePair Peek() { if (!IsEmpty) return _baseHeap[0]; else throw new InvalidOperationException("Priority queue is empty"); } /// /// Returns value of the element with minimun priority, without removing it from the queue /// /// value of the element with minimum priority /// /// Method throws if priority queue is empty /// public TValue PeekValue() { return Peek().Value; } /// /// Gets whether priority queue is empty /// public bool IsEmpty { get { return _baseHeap.Count == 0; } } #endregion #region Heap operations private void ExchangeElements(int pos1, int pos2) { KeyValuePair val = _baseHeap[pos1]; _baseHeap[pos1] = _baseHeap[pos2]; _baseHeap[pos2] = val; } private void Insert(TPriority priority, TValue value) { KeyValuePair val = new KeyValuePair(priority, value); _baseHeap.Add(val); // heap[i] have children heap[2*i + 1] and heap[2*i + 2] and parent heap[(i-1)/ 2]; // heapify after insert, from end to beginning HeapifyFromEndToBeginning(_baseHeap.Count - 1); } private int HeapifyFromEndToBeginning(int pos) { if (pos >= _baseHeap.Count) return -1; while (pos > 0) { int parentPos = (pos - 1) / 2; if (_comparer.Compare(_baseHeap[parentPos].Key, _baseHeap[pos].Key) > 0) { ExchangeElements(parentPos, pos); pos = parentPos; } else break; } return pos; } private void DeleteRoot() { if (_baseHeap.Count <= 1) { _baseHeap.Clear(); return; } _baseHeap[0] = _baseHeap[_baseHeap.Count - 1]; _baseHeap.RemoveAt(_baseHeap.Count - 1); // heapify HeapifyFromBeginningToEnd(0); } private void HeapifyFromBeginningToEnd(int pos) { if (pos >= _baseHeap.Count) return; // heap[i] have children heap[2*i + 1] and heap[2*i + 2] and parent heap[(i-1)/ 2]; while (true) { // on each iteration exchange element with its smallest child int smallest = pos; int left = 2 * pos + 1; int right = 2 * pos + 2; if (left < _baseHeap.Count && _comparer.Compare(_baseHeap[smallest].Key, _baseHeap[left].Key) > 0) smallest = left; if (right < _baseHeap.Count && _comparer.Compare(_baseHeap[smallest].Key, _baseHeap[right].Key) > 0) smallest = right; if (smallest != pos) { ExchangeElements(smallest, pos); pos = smallest; } else break; } } #endregion #region ICollection> implementation /// /// Enqueus element into priority queue /// /// element to add public void Add(KeyValuePair item) { Enqueue(item.Key, item.Value); } /// /// Clears the collection /// public void Clear() { _baseHeap.Clear(); } /// /// Determines whether the priority queue contains a specific element /// /// The object to locate in the priority queue /// true if item is found in the priority queue; otherwise, false. public bool Contains(KeyValuePair item) { return _baseHeap.Contains(item); } public bool TryFindValue(TPriority item,out TValue foundVersion){ for (int i = 0; i < _baseHeap.Count; i++) { if (_comparer.Compare(item,_baseHeap[i].Key) == 0){ foundVersion = _baseHeap[i].Value; return true; } } foundVersion = default(TValue); return false; } /// /// Gets number of elements in the priority queue /// public int Count { get { return _baseHeap.Count; } } /// /// Copies the elements of the priority queue to an Array, starting at a particular Array index. /// /// The one-dimensional Array that is the destination of the elements copied from the priority queue. The Array must have zero-based indexing. /// The zero-based index in array at which copying begins. /// /// It is not guaranteed that items will be copied in the sorted order. /// public void CopyTo(KeyValuePair[] array, int arrayIndex) { _baseHeap.CopyTo(array, arrayIndex); } /// /// Gets a value indicating whether the collection is read-only. /// /// /// For priority queue this property returns false. /// public bool IsReadOnly { get { return false; } } /// /// Removes the first occurrence of a specific object from the priority queue. /// /// The object to remove from the ICollection <(Of <(T >)>). /// true if item was successfully removed from the priority queue. /// This method returns false if item is not found in the collection. public bool Remove(KeyValuePair item) { // find element in the collection and remove it int elementIdx = _baseHeap.IndexOf(item); if (elementIdx < 0) return false; //remove element _baseHeap[elementIdx] = _baseHeap[_baseHeap.Count - 1]; _baseHeap.RemoveAt(_baseHeap.Count - 1); // heapify int newPos = HeapifyFromEndToBeginning(elementIdx); if (newPos == elementIdx) HeapifyFromBeginningToEnd(elementIdx); return true; } /// /// Returns an enumerator that iterates through the collection. /// /// Enumerator /// /// Returned enumerator does not iterate elements in sorted order. public IEnumerator> GetEnumerator() { return _baseHeap.GetEnumerator(); } /// /// Returns an enumerator that iterates through the collection. /// /// Enumerator /// /// Returned enumerator does not iterate elements in sorted order. IEnumerator IEnumerable.GetEnumerator() { return this.GetEnumerator(); } #endregion } }