Implementing PriorityQueue for A*

Assets/Scripts/Generals.meta

+fileFormatVersion: 2
+guid: 6b7073261dd8e5d4284df86af077644d
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/Scripts/Generals/PriorityQueue.cs

+using System.Collections;
+using System.Collections.Generic;
+
+public enum SortMode
+{
+    ASCENDING,
+    DECENDING
+}
+public class PriorityQueue<T>
+{
+    private List<T> heap = new List<T>();
+    private IComparer<T> comparer;
+    private SortMode sortMode;
+
+    public PriorityQueue(IComparer<T> comparer, SortMode sortMode = SortMode.ASCENDING)
+    {
+        this.comparer = comparer;
+        this.sortMode = sortMode;
+    }
+
+    public void Enqueue(T item)
+    {
+        InsertHeap(item);
+    }
+
+    public T Dequeue()
+    {
+        T first = heap[0];
+        heap[0] = heap[heap.Count - 1];
+        heap.RemoveAt(heap.Count - 1);
+        Heapify();
+        return first;
+    }
+
+    private void InsertHeap(T item)
+    {
+        heap.Add(item);
+        int index = heap.Count - 1;
+        while (sortMode == SortMode.ASCENDING ? comparer.Compare(heap[index], heap[index / 2]) <= 0 :comparer.Compare(heap[index], heap[index / 2]) > 0)
+        {
+            T tmp = heap[index];
+            heap[index] = heap[index / 2];
+            heap[index / 2] = tmp;
+            index /= 2;
+        }
+    }
+
+    private void Heapify()
+    {
+        int index = 0;
+        while (true)
+        {
+            int left = 2 * index + 1;
+            int right = 2 * index + 2;
+
+            int target = index;
+            if(left < heap.Count && (sortMode == SortMode.ASCENDING?comparer.Compare(heap[index],heap[left]) <= 0 :comparer.Compare(heap[index], heap[left]) > 0))
+            {
+                target = left;
+            }
+            if (right < heap.Count && (sortMode == SortMode.ASCENDING ? comparer.Compare(heap[index], heap[right]) <= 0 : comparer.Compare(heap[index], heap[right]) > 0))
+            {
+                target = right;
+            }
+            if (index != target)
+            {
+                T tmp = heap[index];
+                heap[index] = heap[target];
+                heap[target] = tmp;
+                index = target;
+            }
+            else
+            {
+                break;
+            }
+        }
+    }
+}
\ No newline at end of file

Assets/Scripts/Generals/PriorityQueue.cs.meta

+fileFormatVersion: 2
+guid: e72f9fd752085534aa021681d36b4d5d
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/Scripts/NodalPathfinding/NodalPathfinding2D.cs

@@ -272,16 +272,12 @@ namespace BS
         public List<Vector2Int> adjacencies;
     }
 
-    public class PriorityQueue<T>
+    class NodeComparer : IComparer<Node>
     {
-
-        public PriorityQueue()
+        private Vector3 destination;
+        NodeComparer(Vector3 destination)
         {
-        }
-
-        public void Enqueue(T item)
-        {
-            
+            this.destination = destination;
         }
     }
 }
\ No newline at end of file