Implementing A* with heap

Assets/Scenes/Pathfinding.unity

@@ -406,7 +406,7 @@ GameObject:
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 1
+  m_IsActive: 0
 --- !u!114 &620200134
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -422,6 +422,7 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
+  moveSpeed: 0
   isGizmos: 1
 --- !u!61 &620200135
 BoxCollider2D:
@@ -531,7 +532,7 @@ GameObject:
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 1
+  m_IsActive: 0
 --- !u!114 &852157121
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -547,6 +548,7 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
+  moveSpeed: 0
   isGizmos: 1
 --- !u!61 &852157122
 BoxCollider2D:
@@ -872,7 +874,7 @@ GameObject:
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 1
+  m_IsActive: 0
 --- !u!114 &1119483105
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -888,6 +890,7 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
+  moveSpeed: 0
   isGizmos: 1
 --- !u!61 &1119483106
 BoxCollider2D:
@@ -997,7 +1000,7 @@ GameObject:
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 1
+  m_IsActive: 0
 --- !u!114 &1304602415
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -1013,6 +1016,7 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
+  moveSpeed: 0
   isGizmos: 1
 --- !u!61 &1304602416
 BoxCollider2D:
@@ -1230,7 +1234,7 @@ GameObject:
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 1
+  m_IsActive: 0
 --- !u!114 &1353820626
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -1246,6 +1250,7 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
+  moveSpeed: 0
   isGizmos: 1
 --- !u!61 &1353820627
 BoxCollider2D:
@@ -1355,7 +1360,7 @@ GameObject:
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 1
+  m_IsActive: 0
 --- !u!114 &1546514103
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -1371,6 +1376,7 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
+  moveSpeed: 0
   isGizmos: 1
 --- !u!61 &1546514104
 BoxCollider2D:
@@ -1496,6 +1502,7 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
+  moveSpeed: 0
   isGizmos: 1
 --- !u!61 &1592952602
 BoxCollider2D:
@@ -1688,7 +1695,7 @@ GameObject:
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 1
+  m_IsActive: 0
 --- !u!114 &1651584784
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -1704,6 +1711,7 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
+  moveSpeed: 0
   isGizmos: 1
 --- !u!61 &1651584785
 BoxCollider2D:
@@ -1813,7 +1821,7 @@ GameObject:
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 1
+  m_IsActive: 0
 --- !u!114 &1669406768
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -1829,6 +1837,7 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
+  moveSpeed: 0
   isGizmos: 1
 --- !u!61 &1669406769
 BoxCollider2D:
@@ -1938,7 +1947,7 @@ GameObject:
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
   m_StaticEditorFlags: 0
-  m_IsActive: 1
+  m_IsActive: 0
 --- !u!114 &2003659734
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -1954,6 +1963,7 @@ MonoBehaviour:
   pathFinder: {fileID: 178212234}
   destination: {x: 0, y: 0, z: 0}
   path: []
+  moveSpeed: 0
   isGizmos: 1
 --- !u!61 &2003659735
 BoxCollider2D:

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;
+    private Heap<T> heap;
+	
+	public int Count { get { return heap.Count; } }
 
-    public PriorityQueue(IComparer<T> comparer, SortMode sortMode = SortMode.ASCENDING)
+    public PriorityQueue(System.Comparison<T> comparison)
     {
-        this.comparer = comparer;
-        this.sortMode = sortMode;
+		heap = new Heap<T>(comparison);
     }
 
     public void Enqueue(T item)
     {
-        InsertHeap(item);
+		heap.Insert(item);
     }
 
     public T Dequeue()
     {
-        T first = heap[0];
-        heap[0] = heap[heap.Count - 1];
-        heap.RemoveAt(heap.Count - 1);
-        Heapify();
-        return first;
+        return heap.Pop();
     }
 
-    private void InsertHeap(T item)
+	public bool Contains(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)
+		return heap.Contains(item);
+	}
+}
+
+class Heap<T>
+{
+	private List<T> list = new List<T>();
+	private System.Comparison<T> comparison;
+
+	public int Count { get { return list.Count; } }
+
+	public Heap(System.Comparison<T> comparison)
 	{
-            T tmp = heap[index];
-            heap[index] = heap[index / 2];
-            heap[index / 2] = tmp;
+		this.comparison = comparison;
+	}
+
+	public bool Contains(T item)
+	{
+		return list.Contains(item);
+	}
+
+	public void Insert(T item)
+	{
+		list.Add(item);
+		int index = list.Count - 1;
+		while (comparison.Invoke(list[index], list[index / 2]) > 0)
+		{
+			T tmp = list[index];
+			list[index] = list[index / 2];
+			list[index / 2] = tmp;
 			index /= 2;
 		}
 	}
 
+	public T Pop()
+	{
+		T tmp = list[0];
+		list[0] = list[list.Count - 1];
+		list.RemoveAt(list.Count - 1);
+		Heapify();
+		return tmp;
+	}
+
 	private void Heapify()
 	{
 		int index = 0;
@@ -54,19 +76,19 @@ public class PriorityQueue<T>
 			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))
+			if (left < list.Count && comparison.Invoke(list[left], list[index]) > 0)
 			{
 				target = left;
 			}
-            if (right < heap.Count && (sortMode == SortMode.ASCENDING ? comparer.Compare(heap[index], heap[right]) <= 0 : comparer.Compare(heap[index], heap[right]) > 0))
+			if (right < list.Count && comparison.Invoke(list[right], list[index]) > 0)
 			{
 				target = right;
 			}
 			if (index != target)
 			{
-                T tmp = heap[index];
-                heap[index] = heap[target];
-                heap[target] = tmp;
+				T tmp = list[index];
+				list[index] = list[target];
+				list[target] = tmp;
 				index = target;
 			}
 			else

Assets/Scripts/NodalPathfinding/NodalPathfinding2D.cs

@@ -195,14 +195,13 @@ namespace BS
             Node goal = nodes[WorldToIndex(to, from - to)];
 			//path.Add(cur.worldPositon);
 
-            List<Node> queue = new List<Node>();
+			PriorityQueue<Node> queue = new PriorityQueue<Node>((a, b) => a.score < b.score ? 1 : a.score == b.score ? 0 : -1);
             List<Node> closed = new List<Node>();
 
-            queue.Add(new Node(start, null));
+            queue.Enqueue(start);
             while (queue.Count > 0)
             {
-                Node cur = queue[0];
-                queue.RemoveAt(0);
+                Node cur = queue.Dequeue();
 
                 if(cur.gridPosition == goal.gridPosition)
                 {
@@ -213,27 +212,27 @@ namespace BS
                 foreach (var adj in cur.adjacencies)
                 {
                     Node adjNode = nodes[adj];
-                    Node alreadyOpened = queue.Find(n => n.gridPosition == adj);
-                    Node alreadyClosed = closed.Find(n => n.gridPosition == adj);
-                    if (alreadyOpened != null)
+					bool isOpened = queue.Contains(nodes[adj]);
+					bool isClosed = closed.Contains(nodes[adj]);
+
+                    if (isOpened)
                     {
-                        if (alreadyOpened.cost > cur.cost + Vector2.Distance(cur.worldPositon, adjNode.worldPositon))
+                        if (adjNode.cost > cur.cost + Vector2.Distance(cur.worldPositon, adjNode.worldPositon))
                         {
-                            alreadyOpened.cost = cur.cost + Vector2.Distance(cur.worldPositon, adjNode.worldPositon);
-                            alreadyOpened.CalculateScore(to);
-                            alreadyOpened.parent = cur;
+                            adjNode.cost = cur.cost + Vector2.Distance(cur.worldPositon, adjNode.worldPositon);
+                            adjNode.CalculateScore(to);
+                            adjNode.parent = cur;
                         }
                     }
-                    else if (alreadyClosed == null)
+                    else if (!isClosed)
                     {
-                        Node node = new Node(adjNode, cur, cur.cost + Vector2.Distance(cur.worldPositon, adjNode.worldPositon));
-                        node.CalculateScore(to);
-                        queue.Add(node);
+						adjNode.parent = cur;
+						adjNode.cost = cur.cost + Vector2.Distance(cur.worldPositon, adjNode.worldPositon);
+						adjNode.CalculateScore(to);
+                        queue.Enqueue(adjNode);
                     }
 
                 }
-
-                queue.Sort((a, b) => a.score >= b.score ? 1 : -1);
                 closed.Add(cur);
             }
 
@@ -251,6 +250,13 @@ namespace BS
                 }
             }
             path.Reverse();
+
+			foreach(var node in nodes.Values)
+			{
+				node.cost = 0;
+				node.parent = null;
+				node.score = 0;
+			}
             return path;
         }
 

Assets/Scripts/NodalPathfinding/NodalPathfinding2DAgent.cs

@@ -9,6 +9,7 @@ namespace BS {
 
         public Vector3 destination;
         public List<Vector3> path = new List<Vector3>();
+		public float moveSpeed;
 
         float t = 0.5f;
 
@@ -33,13 +34,14 @@ namespace BS {
 
         private void Update()
         {
-
+			/*
             t -= Time.deltaTime;
             if (t < 0)
             {
                 MoveTo(new Vector2(Random.Range(pathFinder.bounds.min.x, pathFinder.bounds.max.x), Random.Range(pathFinder.bounds.min.y, pathFinder.bounds.max.y)));
                 t = 0.5f;
             }
+			*/
 
 
             if (Input.GetKeyDown(KeyCode.Space))
@@ -58,6 +60,11 @@ namespace BS {
             this.destination = destination;
             //path = pathFinder.GetPathGreedy(transform.position, destination);
             path = pathFinder.GetPathAstar(transform.position, destination);
+        }
+
+		public void Move(Vector2 direction)
+		{
+
 		}
 	}
 }
\ No newline at end of file