remove costly List calls via State attribute, use sorted lists + binary search for open list & IComparable in Node

also refactor Manhattan overloading and add SignedManhattan func

Paths/Assets/Scripts/Algorithms/AStar.cs

@@ -37,28 +37,36 @@ namespace Algorithms {
             // add start node to Open List
             _openList.Add(startNode);
 
-            while (_openList.Count != 0 && !_closedList.Exists(x => x.Position == endNode.Position)) {
+            while (_openList.Count != 0) {
-                current = _openList[0];
+                current = _openList.First();
                 _openList.Remove(current);
+
+                current.State = NodeState.Closed;
                 _closedList.Add(current);
+                
+                if (current.Position == endNode.Position)
+                    break;
+                
                 RecordState();
 
                 Node[] adjacentNodes = this._nodeGrid.GetAdjacentNodesArray(current);
-                if (true) {
+                for (int i = 0; i < adjacentNodes.Length; i++) {
-                    for (int i = 0; i < adjacentNodes.Length; i++) {
+                    Node node = adjacentNodes[i];
-                        Node n = adjacentNodes[i];
+                    if (node != null && node.State == NodeState.None && node.Walkable) {
-                        if (n == null) continue;
+                        // Setup node & calculate new costs
+                        node.Parent = current;
+                        node.DistanceToTarget = NodeGrid.Manhattan(node, endNode);
+                        node.Cost = node.Weight + node.Parent.Cost;
                         
-                        if (!_closedList.Contains(n) && n.Walkable) {
+                        // Set to open and add to open list (sorted)
-                            if (!_openList.Contains(n)) {
+                        node.State = NodeState.Open;
-                                n.Parent = current;
+                        // _openList.Add(node);
-                                n.DistanceToTarget = NodeGrid.Manhattan(n, endNode);
-                                n.Cost = n.Weight + n.Parent.Cost;
 
-                                _openList.Add(n);
+                        int index = _openList.BinarySearch(node);
-                                _openList = _openList.OrderBy(node => node.F).ToList();
+                        if (index < 0) index = ~index;
-                            }
+                        _openList.Insert(index, node);
-                        }
+                        // _openList = _openList.OrderBy(n => n.F).ToList();
+                        // _openList.Sort((n, o) => n.F.CompareTo(o.F));
                     }
                 }
             }
@@ -86,7 +94,7 @@ namespace Algorithms {
         public void RecordState() {
             // TODO: Record pathfinding state information (stages, heuristic, statistical info)
             this._states.Add(
-                new GridState(this._nodeGrid, this._openList, this._closedList, Start, End, _path)
+                new GridState(this._nodeGrid, this._openList.ToList(), this._closedList, Start, End, _path)
             );
         }
 

Paths/Assets/Scripts/Algorithms/Node.cs

@@ -1,20 +1,27 @@
-using UnityEngine;
+using System;
+using UnityEngine;
 
 namespace Algorithms {
-    public class Node {
+    public enum NodeState {
+        None, Open, Closed
+    }
+    
+    public class Node : IComparable<Node> {
         // Change this depending on what the desired size is for each element in the grid
         public Node Parent;
-        public Vector2Int Position;
+        public readonly Vector2Int Position;
 
         // A* Algorithm variables
-        public float DistanceToTarget;
+        public float? DistanceToTarget;
-        public float Cost;
+        public float? Cost;
         public float Weight;
 
+        public NodeState State = NodeState.None;
+
         public float F {
             get {
-                if (DistanceToTarget != -1 && Cost != -1)
+                if (DistanceToTarget.HasValue && Cost.HasValue)
-                    return DistanceToTarget + Cost;
+                    return DistanceToTarget.Value + Cost.Value;
                 return -1;
             }
         }
@@ -24,14 +31,34 @@ namespace Algorithms {
         public Node(Vector2Int pos, bool walkable, float weight = 1) {
             Parent = null;
             Position = pos;
-            DistanceToTarget = -1;
+            DistanceToTarget = null;
             Cost = 1;
             Weight = weight;
             Walkable = walkable;
         }
 
+        public override bool Equals(object obj) {
+            return obj is Node node && Position.Equals(node.Position);
+        }
+
+        public override int GetHashCode() {
+            return Position.GetHashCode();
+        }
+
+        public int CompareTo(Node other) {
+            int diff = (int) (this.F - other.F);
+            return diff;
+            // return diff != 0 ? diff : NodeGrid.SignedManhattan(Position, other.Position);
+        }
+
         public override string ToString() {
-            return $"Node({Position.x}, {Position.y}, {Walkable})";
+            return string.Format(
+                    "Node ({0:00}, {1:00}, {2}, {3})",
+                    Position.x,
+                    Position.y,
+                    Walkable ? "Walkable" : "Not Walkable",
+                    State == NodeState.None ? (Walkable ? "Openable" : "Wall") : State.ToString()
+                );
         }
     }
 }

Paths/Assets/Scripts/Algorithms/NodeGrid.cs

@@ -55,7 +55,7 @@ namespace Algorithms {
             int col = node.Position.x;
             int row = node.Position.y;
 
-            return new Node[] {
+            return new [] {
                 row + 1 < Height ? Grid[col, row + 1] : null,
                 row - 1 >= 0 ? Grid[col, row - 1] : null,
                 col - 1 >= 0 ? Grid[col - 1, row] : null,
@@ -67,12 +67,16 @@ namespace Algorithms {
             return Grid[position.x, position.y];
         }
 
-        public static float Manhattan(Node a, Node b) {
+        public static int Manhattan(Node a, Node b) {
-            return Math.Abs(a.Position.x - b.Position.x) + Math.Abs(a.Position.y - b.Position.y);
+            return Manhattan(a.Position, b.Position);
         }
 
-        public static float Manhattan(Vector2Int a, Vector2Int b) {
+        public static int SignedManhattan(Vector2Int a, Vector2Int b) {
-            return Manhattan(new Node(a, false), new Node(b, false));
+            return a.x - b.x + (a.y - b.y);
+        }
+
+        public static int Manhattan(Vector2Int a, Vector2Int b) {
+            return Math.Abs(a.x - b.x) + Math.Abs(a.y - b.y);
         }
 
         /// <summary>

Paths/Assets/Scripts/Manager.cs

@@ -67,7 +67,7 @@ public class Manager : MonoBehaviour {
         // Vector2Int start = new Vector2Int(30, 30);
         Vector2Int end = nodeGrid.RandomPosition();
 
-        nodeGrid.ApplyGenerator(new RandomPlacement(0.25f, true));
+        nodeGrid.ApplyGenerator(new RandomPlacement(0.20f, true, true));
 
         nodeGrid.GetNode(start).Walkable = true;
         nodeGrid.GetNode(end).Walkable = true;