From a555a701afb414fcaa1ce6ab08a9bdc864a609b6 Mon Sep 17 00:00:00 2001 From: Xevion Date: Tue, 24 Nov 2020 09:45:36 -0600 Subject: [PATCH] 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 | 42 +++++++++++-------- Paths/Assets/Scripts/Algorithms/Node.cs | 45 ++++++++++++++++----- Paths/Assets/Scripts/Algorithms/NodeGrid.cs | 14 ++++--- Paths/Assets/Scripts/Manager.cs | 2 +- 4 files changed, 71 insertions(+), 32 deletions(-) diff --git a/Paths/Assets/Scripts/Algorithms/AStar.cs b/Paths/Assets/Scripts/Algorithms/AStar.cs index 1a61c6a..b4a1aea 100644 --- a/Paths/Assets/Scripts/Algorithms/AStar.cs +++ b/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)) { - current = _openList[0]; + while (_openList.Count != 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++) { - Node n = adjacentNodes[i]; - if (n == null) continue; + for (int i = 0; i < adjacentNodes.Length; i++) { + Node node = adjacentNodes[i]; + if (node != null && node.State == NodeState.None && node.Walkable) { + // Setup node & calculate new costs + node.Parent = current; + node.DistanceToTarget = NodeGrid.Manhattan(node, endNode); + node.Cost = node.Weight + node.Parent.Cost; + + // Set to open and add to open list (sorted) + node.State = NodeState.Open; + // _openList.Add(node); - if (!_closedList.Contains(n) && n.Walkable) { - if (!_openList.Contains(n)) { - n.Parent = current; - n.DistanceToTarget = NodeGrid.Manhattan(n, endNode); - n.Cost = n.Weight + n.Parent.Cost; - - _openList.Add(n); - _openList = _openList.OrderBy(node => node.F).ToList(); - } - } + int index = _openList.BinarySearch(node); + 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) ); } diff --git a/Paths/Assets/Scripts/Algorithms/Node.cs b/Paths/Assets/Scripts/Algorithms/Node.cs index b611f22..faab057 100644 --- a/Paths/Assets/Scripts/Algorithms/Node.cs +++ b/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 { // 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 Cost; + public float? DistanceToTarget; + public float? Cost; public float Weight; + public NodeState State = NodeState.None; + public float F { get { - if (DistanceToTarget != -1 && Cost != -1) - return DistanceToTarget + Cost; + if (DistanceToTarget.HasValue && Cost.HasValue) + 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() + ); } } } \ No newline at end of file diff --git a/Paths/Assets/Scripts/Algorithms/NodeGrid.cs b/Paths/Assets/Scripts/Algorithms/NodeGrid.cs index d98b090..25600fe 100644 --- a/Paths/Assets/Scripts/Algorithms/NodeGrid.cs +++ b/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) { - return Math.Abs(a.Position.x - b.Position.x) + Math.Abs(a.Position.y - b.Position.y); + public static int Manhattan(Node a, Node b) { + return Manhattan(a.Position, b.Position); } - public static float Manhattan(Vector2Int a, Vector2Int b) { - return Manhattan(new Node(a, false), new Node(b, false)); + public static int SignedManhattan(Vector2Int a, Vector2Int b) { + 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); } /// diff --git a/Paths/Assets/Scripts/Manager.cs b/Paths/Assets/Scripts/Manager.cs index 0dd99e5..f97dad1 100644 --- a/Paths/Assets/Scripts/Manager.cs +++ b/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;