AStar add

Author: mitchellri

src/Dijkstra.NET/ShortestPath/AStar.cs

@@ -0,0 +1,60 @@
+using System;
+using System.Collections.Generic;
+using Dijkstra.NET.Graph;
+
+namespace Dijkstra.NET.ShortestPath
+{
+    internal static class AStar
+    {
+        public static ShortestPathResult GetShortestPath(IDijkstraGraph graph, uint from, uint to, Func<uint,uint,int> heuristic, int depth)
+        {
+            var path = new Dictionary<uint, uint>();
+            var distance = new Dictionary<uint, int> {[from] = 0};
+            var d = new Dictionary<uint, int> {[from] = 0};
+            var q = new SortedSet<uint>(new[] {from}, new NodeComparer(distance, heuristic));
+            var current = new HashSet<uint>();
+
+            int Distance(uint key)
+            {
+                return distance.ContainsKey(key) ? distance[key] : Int32.MaxValue;
+            }
+
+            do
+            {
+                uint u = q.Deque();
+
+                if (u == to)
+                {
+                    return new ShortestPathResult(from, to, distance[u], path);
+                }
+
+                current.Remove(u);
+
+                if (depth == d[u])
+                {
+                    continue;
+                }
+
+                graph[u]((node, cost) =>
+                {
+                    if (Distance(node) > Distance(u) + cost)
+                    {
+                        if (current.Contains(node))
+                        {
+                            q.Remove(node);
+                        }
+
+                        distance[node] = Distance(u) + cost;
+                        q.Add(node);
+                        current.Add(node);
+                        path[node] = u;
+                        d[node] = d[u] + 1;
+                    }
+                });
+
+            } while (q.Count > 0 && depth > 0);
+
+            return new ShortestPathResult(from, to);
+        }
+    }
+}

src/Dijkstra.NET/ShortestPath/DijkstraExtensions.cs

@@ -17,6 +17,16 @@ public static class DijkstraExtensions
         public static ShortestPathResult Dijkstra(this IDijkstraGraph graph, uint from, uint to)
             => Dijkstra(graph, from, to, Int32.MaxValue);
 
+        /// <summary>
+        /// Get path from @from to @to
+        /// </summary>
+        /// <param name="graph">Source graph</param>
+        /// <param name="from">Start node</param>
+        /// <param name="to">End node</param>
+        /// <returns>Value with path</returns>
+        public static ShortestPathResult AStar(this IDijkstraGraph graph, uint from, uint to, Func<uint,uint,int> heuristic)
+            => AStar(graph, from, to, heuristic, Int32.MaxValue);
+
         /// <summary>
         /// Get path from @from to @to
         /// </summary>
@@ -29,5 +39,18 @@ public static ShortestPathResult Dijkstra(this IDijkstraGraph graph, uint from,
         {
             return ShortestPath.Dijkstra.GetShortestPath(graph, from, to, depth);
         }
+        
+        /// <summary>
+        /// Get path from @from to @to
+        /// </summary>
+        /// <param name="graph">Source graph</param>
+        /// <param name="from">Start node</param>
+        /// <param name="to">End node</param>
+        /// <param name="depth">Depth of path</param>
+        /// <returns>Value with path</returns>
+        public static ShortestPathResult AStar(this IDijkstraGraph graph, uint from, uint to, Func<uint, uint, int> heuristic, int depth)
+        {
+            return ShortestPath.AStar.GetShortestPath(graph, from, to, heuristic, depth);
+        }
     }
 }

src/Dijkstra.NET/ShortestPath/NodeComparer.cs

@@ -6,16 +6,27 @@ namespace Dijkstra.NET.ShortestPath
     internal class NodeComparer : IComparer<uint>
     {
         private readonly IDictionary<uint, int> _distance;
+        private readonly Func<uint, uint, int> _heuristic;
 
         public NodeComparer(IDictionary<uint, int> distance)
         {
             _distance = distance;
+            _heuristic = null;
+        }
+        public NodeComparer(IDictionary<uint, int> distance, Func<uint, uint, int> heuristic)
+        {
+            _distance = distance;
+            _heuristic = heuristic;
         }
 
         public int Compare(uint x, uint y)
         {
-            int xDistance = _distance.ContainsKey(x) ? _distance[x] : Int32.MaxValue;
-            int yDistance = _distance.ContainsKey(y) ? _distance[y] : Int32.MaxValue;
+            int xDistance = _distance.ContainsKey(x) ?
+                _heuristic is null ? _distance[x] : _distance[x] + _heuristic(x, y)
+                : Int32.MaxValue;
+            int yDistance = _distance.ContainsKey(y) ?
+                _heuristic is null ? _distance[y] : _distance[y] + _heuristic(y, x)
+                : Int32.MaxValue;
 
             int comparer = xDistance.CompareTo(yDistance);