New calculators and some minor improvements

CMGTGraph/Algorithms/Algorithms-AStar.cs

@@ -27,52 +27,56 @@ public AStarNode(T data, T predecessor = default, float currentPathLength = 0f,
         /// Get a path between two points in the graph using the A* algorithm.
         /// If no path can be found, an empty list is returned.
         /// </summary>
-        public static List<T> AStarSolve<T>(this IReadOnlyGraph<T> graph, T start, T end)
+        public static List<T> AStarSolve<T>(this IReadOnlyGraph<T> graph, T start, T end, ICalculator<T> calculator = null)
             where T : IEquatable<T>
         {
             graph.ThrowOnInvalidInput(start, end);
 
-            return graph.AStarSolveWithInfo(start, end).Path;
+            return graph.AStarSolveWithInfo(start, end, calculator).Path;
         }
 
         /// <summary>
         /// Get a path between two points in the graph using the A* algorithm.
         /// A list of the visited nodes is also returned. 
         /// If no path can be found, both lists in the result are empty.
         /// </summary>
-        public static PathFindingResult<T> AStarSolveWithInfo<T>(this IReadOnlyGraph<T> g, T start, T end)
+        public static PathFindingResult<T> AStarSolveWithInfo<T>(this IReadOnlyGraph<T> g, T start, T end, ICalculator<T> calculator = null)
             where T : IEquatable<T>
         {
             g.ThrowOnInvalidInput(start, end);
 
+            calculator = calculator ?? g.Calculator;
+
             var closed = new HashSet<AStarNode<T>>();
             var open = new HashSet<AStarNode<T>>
                 {new AStarNode<T>(start, distanceToFinish: g.Calculator.Distance(start, end))};
 
             while (open.Count > 0)
             {
                 // get most promising node
-                var recordHolder = open.First();
-                var record = recordHolder.EstimatedCompletePathLength;
+                AStarNode<T> recordHolder = null;
+                var record = float.MaxValue;
                 foreach (var node in open)
                 {
                     if (node.EstimatedCompletePathLength >= record) continue;
                     record = node.EstimatedCompletePathLength;
                     recordHolder = node;
                 }
-
-                open.Remove(recordHolder);
-
+                
+                if(recordHolder == null) continue;
+                
                 if (recordHolder.Data.Equals(end))
                 {
                     return new PathFindingResult<T>(BuildPath(recordHolder, closed),
                         new HashSet<T>(open.Select(x => x.Data)),
                         new HashSet<T>(closed.Select(x => x.Data)));
                 }
+
+                open.Remove(recordHolder);
 
                 closed.Add(recordHolder);
                 AStarExpandNode(recordHolder, end, g.GetPassableConnections(recordHolder.Data), open, closed,
-                    g.Calculator);
+                    calculator);
             }
 
             return PathFindingResult<T>.Empty;
@@ -84,26 +88,37 @@ private static void AStarExpandNode<T>(DijkstraNode<T> node, T finish, IEnumerab
             foreach (var neighbor in neighbors)
             {
                 var n = new AStarNode<T>(neighbor);
-                if(closed.Contains(n)) continue;
+                // if (closed.Contains(n))
+                // {
+                //     Logger.Spam("Node in closed");
+                //     continue;
+                // }
 
-                // TODO spam also F G and H when changing them
-
                 // TODO i suspect this is not calculated correctly (diagonal is sometimes chosen over direct)
                 var currentPathLength = node.CurrentPathLength + calculator.Distance(node.Data, neighbor);
-                if (!open.Contains(n))
+
+                var inOpen = open.Contains(n);
+                var inClosed = closed.Contains(n);
+
+                if((inOpen || inClosed) && currentPathLength >= n.CurrentPathLength) continue;
+
+                n.Predecessor = node.Data;
+                n.CurrentPathLength = currentPathLength;
+                n.DistanceToFinish = calculator.Distance(neighbor, finish);
+
+                var changed = "Changed";
+                if (inClosed)
                 {
-                    n.Predecessor = node.Data;
-                    n.CurrentPathLength = currentPathLength;
-                    n.DistanceToFinish = calculator.Distance(neighbor, finish);
+                    changed = "Re-added";
                     open.Add(n);
-                    Logger.Spam($"Added new node {n.Data.ToString()} F {n.EstimatedCompletePathLength.ToString(CultureInfo.InvariantCulture)} G {n.CurrentPathLength.ToString(CultureInfo.InvariantCulture)} H {n.DistanceToFinish.ToString(CultureInfo.InvariantCulture)}");
                 }
-                else if(currentPathLength < n.CurrentPathLength)
+                else if (!inOpen)
                 {
-                    n.CurrentPathLength = currentPathLength;
-                    n.Predecessor = node.Data;
-                    Logger.Spam($"Reevaluated {n.Data.ToString()}");
+                    changed = "Added";
+                    open.Add(n);
                 }
+
+                Logger.Spam($"{changed} node: {n.Data.ToString()} F {n.EstimatedCompletePathLength.ToString(CultureInfo.InvariantCulture)} G {n.CurrentPathLength.ToString(CultureInfo.InvariantCulture)} H {n.DistanceToFinish.ToString(CultureInfo.InvariantCulture)}");
             }
         }
     }

CMGTGraph/Algorithms/Algorithms-Helpers.cs

@@ -123,6 +123,7 @@ private static List<T> BuildRecursiveReversePath<T>(Node<T> from, IReadOnlyDicti
 
         public struct PathFindingResult<T> where T : IEquatable<T>
         {
+            // TODO maybe make OPen and Closed IEnumerable
             public List<T> Path;
             public HashSet<T> OpenNodes;
             public HashSet<T> ClosedNodes;

CMGTGraph/Calculators/PointCalculator.cs

@@ -5,6 +5,12 @@ namespace CMGTGraph.Calculators
 {
     public class PointCalculator : ICalculator<Point>
     {
+        private PointCalculator(){}
+
+        public static readonly PointCalculator This;
+
+        static PointCalculator() => This = new PointCalculator();
+
         /// <inheritdoc />
         public float SqrDistance(Point a, Point b)
         {

CMGTGraph/Calculators/PointDiagonalCalculator.cs

@@ -0,0 +1,49 @@
+using System;
+using CMGTGraph.Types;
+
+namespace CMGTGraph.Calculators
+{
+    /// <summary>
+    /// This uses Diagonal distance. Use this if you can move in 8 directions.
+    /// </summary>
+    public class PointDiagonalCalculator : ICalculator<Point>
+    {
+        private readonly float _straight;
+
+        private readonly float _diagonal;
+
+        private PointDiagonalCalculator(float straight = 1f, float diagonal = 1.41421356237f)
+        {
+            _straight = straight;
+            _diagonal = diagonal;
+        }
+
+        public static readonly PointDiagonalCalculator Octile;
+
+        /// <summary>
+        /// Diagonal calculator which treats diagonal and straight edges equally in the heuristic.
+        /// </summary>
+        public static readonly PointDiagonalCalculator Chebyshev;
+
+        static PointDiagonalCalculator()
+        {
+            Octile = new PointDiagonalCalculator(1f, (float) Math.Sqrt(2f));
+            Chebyshev = new PointDiagonalCalculator(1f, 1f);
+        }
+
+        /// <inheritdoc />
+        public float SqrDistance(Point a, Point b)
+        {
+            var d = Distance(a, b);
+            return d * d;
+        }
+
+        /// <inheritdoc />
+        public float Distance(Point a, Point b)
+        {
+            var dx = Math.Abs(a.X - b.X);
+            var dy = Math.Abs(a.Y - b.Y);
+            return _straight * (dx + dy) + (_diagonal - 2 * _straight) * Math.Min(dx, dy);
+        }
+    }
+}

CMGTGraph/Calculators/PointFCalculator.cs

@@ -6,6 +6,14 @@ namespace CMGTGraph.Calculators
 {
     public class PointFCalculator : ICalculator<PointF>
     {
+        private PointFCalculator()
+        {
+        }
+
+        public static readonly PointFCalculator This;
+
+        static PointFCalculator() => This = new PointFCalculator();
+
         /// <inheritdoc />
         public float SqrDistance(PointF a, PointF b)
         {

CMGTGraph/Calculators/PointFDiagonalCalculator.cs

@@ -0,0 +1,46 @@
+using System;
+using System.Runtime.CompilerServices;
+using CMGTGraph.Types;
+
+namespace CMGTGraph.Calculators
+{
+    public class PointFDiagonalCalculator : ICalculator<PointF>
+    {
+        private readonly float _straight;
+        // SQRT 2
+        private readonly float _diagonal;
+
+        private PointFDiagonalCalculator(float straight = 1f, float diagonal = 1.41421356237f)
+        {
+            _straight = straight;
+            _diagonal = diagonal;
+        }
+
+        public static readonly PointFDiagonalCalculator Octile;
+        /// <summary>
+        /// Diagonal calculator which treats diagonal and straight edges equally in the heuristic.
+        /// </summary>
+        public static readonly PointFDiagonalCalculator Chebyshev;
+
+        static PointFDiagonalCalculator()
+        {
+            Octile = new PointFDiagonalCalculator();
+            Chebyshev = new PointFDiagonalCalculator(1f, 1f);
+        }
+
+        /// <inheritdoc />
+        public float SqrDistance(PointF a, PointF b)
+        {
+            var d = Distance(a, b);
+            return d * d;
+        }
+
+        /// <inheritdoc />
+        public float Distance(PointF a, PointF b)
+        {
+            var dx = Math.Abs(a.X - b.X);
+            var dy = Math.Abs(a.Y - b.Y);
+            return _straight * (dx + dy) + (_diagonal - 2 * _straight) * Math.Min(dx, dy);
+        }
+    }
+}

CMGTGraph/Calculators/PointFManhattanCalculator.cs

@@ -0,0 +1,32 @@
+using System;
+using CMGTGraph.Types;
+
+namespace CMGTGraph.Calculators
+{
+    /// <summary>
+    /// 
+    /// </summary>
+    public class PointFManhattanCalculator : ICalculator<PointF>
+    {
+        public static readonly PointFManhattanCalculator This;
+
+        private PointFManhattanCalculator()
+        {
+        }
+
+        static PointFManhattanCalculator() => This = new PointFManhattanCalculator();
+
+        /// <inheritdoc />
+        public float SqrDistance(PointF a, PointF b)
+        {
+            var d = Distance(a, b);
+            return d * d;
+        }
+
+        /// <inheritdoc />
+        public float Distance(PointF a, PointF b)
+        {
+            return Math.Abs(a.X - b.X) + Math.Abs(a.Y - b.Y);
+        }
+    }
+}

CMGTGraph/Calculators/PointManhattanCalculator.cs

@@ -0,0 +1,32 @@
+using System;
+using CMGTGraph.Types;
+
+namespace CMGTGraph.Calculators
+{
+    /// <summary>
+    /// This uses Manhattan square distance as the distance calculation.
+    /// Use this when you can move in 4 directions
+    /// </summary>
+    public class PointManhattanCalculator : ICalculator<Point>
+    {
+        public static readonly PointManhattanCalculator This;
+
+        private PointManhattanCalculator()
+        { }
+
+        static PointManhattanCalculator() => This = new PointManhattanCalculator();
+
+        /// <inheritdoc />
+        public float SqrDistance(Point a, Point b)
+        {
+            var d = Distance(a, b);
+            return d * d;
+        }
+
+        /// <inheritdoc />
+        public float Distance(Point a, Point b)
+        {
+            return Math.Abs(a.X - b.X) + Math.Abs(a.Y - b.Y);
+        }
+    }
+}

CMGTGraph/Graph.cs

@@ -1,13 +1,13 @@
 using System;
 using System.Collections.Generic;
-using System.Collections.ObjectModel;
 using System.Linq;
-using System.Security.Cryptography;
 using System.Text;
 using CMGTGraph.Calculators;
 
 namespace CMGTGraph
 {
+	// TODO look into ALT A* (https://www.microsoft.com/en-us/research/wp-content/uploads/2004/07/tr-2004-24.pdf)
+
     /// <summary>
     /// The representation of a graph.
     /// </summary>
@@ -16,8 +16,6 @@ public class Graph<T> : IReadOnlyGraph<T> where T : IEquatable<T>
     {
         public sealed class NodeNotFoundException : KeyNotFoundException
         {
-            public NodeNotFoundException()
-            { }
             public NodeNotFoundException(T value)
             {
                 Data.Add("ToString", value.Equals(null) ? value.ToString() : "null");
@@ -33,7 +31,7 @@ public NodeNotFoundException(T value)
         public HashSet<T> Nodes => new HashSet<T>(_connections.Keys);
 
         /// <inheritdoc />
-        public ICalculator<T> Calculator { get; }
+        public ICalculator<T> Calculator { get; set; }
 
         private readonly HashSet<T> _impassable;
 
@@ -59,11 +57,14 @@ public void Add(T node)
 
         /// <summary>
         /// Add a connection between two nodes. If one or both of the nodes don't/doesn't
-        /// exist in the graph, it/they will be added to the graph. 
+        /// exist in the graph, it/they will be added to the graph.
+        /// If you pass the same node twice, it will be added to the graph but no connections will be added
+        /// as to not create loops. 
         /// </summary>
         public void AddConnection(T nodeA, T nodeB)
         {
             Add(nodeA);
+            if (nodeA.Equals(nodeB)) return;
             Add(nodeB);
             _connections[nodeA].Add(nodeB);
             _connections[nodeB].Add(nodeA);

CMGTGraph/Types/Point.cs

@@ -9,13 +9,6 @@ public class Point : IEquatable<Point>
         public readonly int X;
         public readonly int Y;
 
-        public static PointCalculator Calculator { get; }
-
-        static Point()
-        {
-            Calculator = new PointCalculator();
-        }
-
         public Point(int x, int y)
         {
             X = x;

CMGTGraph/Types/PointF.cs

@@ -9,13 +9,6 @@ public class PointF : IEquatable<PointF>
         public readonly float X;
         public readonly float Y;
 
-        public static PointFCalculator Calculator { get; }
-
-        static PointF()
-        {
-            Calculator = new PointFCalculator();
-        }
-
         public PointF(float x, float y)
         {
             X = x;