Implementing A* pathfinding. Needs to be optimized.

Assets/Scripts/NodalPathfinding/NodalPathfinding2D.cs

@@ -184,18 +184,73 @@ namespace BS
             return path;
         }
 
-        public List<Vector3> GetPathAStar(Vector3 from, Vector3 to)
+        public List<Vector3> GetPathAstar(Vector3 from, Vector3 to)
         {
-            
             List<Vector3> path = new List<Vector3>();
-
             if (WorldToIndex(from, to - from).x < 0 || WorldToIndex(to, from - to).x < 0)
             {
                 return path;
             }
-            
-            
+            Node start = nodes[WorldToIndex(from, to - from)];
+            Node goal = nodes[WorldToIndex(to, from - to)];
+            //path.Add(cur.worldPositon);
+
+            List<Node> queue = new List<Node>();
+            List<Node> closed = new List<Node>();
+
+            queue.Add(new Node(start, null));
+            while (queue.Count > 0)
+            {
+                Node cur = queue[0];
+                queue.RemoveAt(0);
+
+                if(cur.gridPosition == goal.gridPosition)
+                {
+                    closed.Add(cur);
+                    break;
+                }
+
+                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)
+                    {
+                        if (alreadyOpened.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;
+                        }
+                    }
+                    else if (alreadyClosed == null)
+                    {
+                        Node node = new Node(adjNode, cur, cur.cost + Vector2.Distance(cur.worldPositon, adjNode.worldPositon));
+                        node.CalculateScore(to);
+                        queue.Add(node);
+                    }
+
+                }
+
+                queue.Sort((a, b) => a.score >= b.score ? 1 : -1);
+                closed.Add(cur);
+            }
 
+            Node tmp = closed.Find(n => n.gridPosition == goal.gridPosition);
+            if (tmp == null)    //Path is blocked
+            {
+                return GetPathGreedy(from, to);
+            }
+            else
+            {
+                while (tmp.gridPosition != start.gridPosition)
+                {
+                    path.Add(tmp.worldPositon);
+                    tmp = tmp.parent;
+                }
+            }
+            path.Reverse();
             return path;
         }
 
@@ -259,25 +314,49 @@ namespace BS
         }
     }
 
-    struct Node
+    class Node
     {
         public Node(Vector2Int gridPosition, Vector3 worldPositon)
         {
             this.gridPosition = gridPosition;
             this.worldPositon = worldPositon;
             adjacencies = null;
+            cost = 0;
+            score = 0;
+            parent = null;
         }
+        public Node(Node node, Node parent, float cost = 0)
+        {
+            this.gridPosition = node.gridPosition;
+            this.worldPositon = node.worldPositon;
+            this.adjacencies = node.adjacencies;
+            this.cost = cost;
+            this.parent = parent;
+            score = 0;
+        }
+
         public Vector2Int gridPosition;
         public Vector3 worldPositon;
         public List<Vector2Int> adjacencies;
+        public float cost;
+        public float score;
+        public Node parent;
+        public void CalculateScore(Vector3 destination)
+        {
+            score = cost + GetHeuristic(destination);
+        }
+
+        private float GetHeuristic(Vector3 destination)
+        {
+            return Vector2.Distance(worldPositon, destination);
+        }
     }
 
     class NodeComparer : IComparer<Node>
     {
-        private Vector3 destination;
-        NodeComparer(Vector3 destination)
+        public int Compare(Node x, Node y)
         {
-            this.destination = destination;
+            throw new System.NotImplementedException();
         }
     }
 }
\ No newline at end of file

Assets/Scripts/NodalPathfinding/NodalPathfinding2DAgent.cs

@@ -12,9 +12,12 @@ namespace BS {
 
         float t = 0.5f;
 
+        public bool isGizmos = true;
 
         private void OnDrawGizmos()
         {
+            if (!isGizmos)
+                return;
             Gizmos.color = Color.white;
             if (path.Count > 0)
             {
@@ -24,12 +27,13 @@ namespace BS {
                     Gizmos.DrawLine(path[i] + new Vector3(0,0,1), path[i + 1] + new Vector3(0, 0, 1));
                 }
                 Gizmos.color = Color.red;
-                Gizmos.DrawSphere(destination, 0.5f);
+                Gizmos.DrawSphere(destination + new Vector3(0, 0, 1), 0.5f);
             }
         }
 
         private void Update()
         {
+
             t -= Time.deltaTime;
             if (t < 0)
             {
@@ -37,6 +41,7 @@ namespace BS {
                 t = 0.5f;
             }
 
+
             if (Input.GetKeyDown(KeyCode.Space))
             {
                 transform.position = new Vector2(Random.Range(pathFinder.bounds.min.x, pathFinder.bounds.max.x), Random.Range(pathFinder.bounds.min.y, pathFinder.bounds.max.y));
@@ -51,7 +56,8 @@ namespace BS {
         public void MoveTo(Vector3 destination)
         {
             this.destination = destination;
-            path = pathFinder.GetPathGreedy(transform.position, destination);
+            //path = pathFinder.GetPathGreedy(transform.position, destination);
+            path = pathFinder.GetPathAstar(transform.position, destination);
         }
     }
 }
\ No newline at end of file