diff --git a/src/Dijkstra.NET/ShortestPath/AStar.cs b/src/Dijkstra.NET/ShortestPath/AStar.cs new file mode 100644 index 0000000..e1b7d5a --- /dev/null +++ b/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 heuristic, int depth) + { + var path = new Dictionary(); + var distance = new Dictionary {[from] = 0}; + var d = new Dictionary {[from] = 0}; + var q = new SortedSet(new[] {from}, new HeuristicNodeComparer(distance, heuristic)); + var current = new HashSet(); + + 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); + } + } +} \ No newline at end of file diff --git a/src/Dijkstra.NET/ShortestPath/DijkstraExtensions.cs b/src/Dijkstra.NET/ShortestPath/DijkstraExtensions.cs index 3fc200a..ba1730b 100644 --- a/src/Dijkstra.NET/ShortestPath/DijkstraExtensions.cs +++ b/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); + /// + /// Get path from @from to @to + /// + /// Source graph + /// Start node + /// End node + /// Value with path + public static ShortestPathResult AStar(this IDijkstraGraph graph, uint from, uint to, Func heuristic) + => AStar(graph, from, to, heuristic, Int32.MaxValue); + /// /// Get path from @from to @to /// @@ -29,5 +39,18 @@ public static ShortestPathResult Dijkstra(this IDijkstraGraph graph, uint from, { return ShortestPath.Dijkstra.GetShortestPath(graph, from, to, depth); } + + /// + /// Get path from @from to @to + /// + /// Source graph + /// Start node + /// End node + /// Depth of path + /// Value with path + public static ShortestPathResult AStar(this IDijkstraGraph graph, uint from, uint to, Func heuristic, int depth) + { + return ShortestPath.AStar.GetShortestPath(graph, from, to, heuristic, depth); + } } } \ No newline at end of file diff --git a/src/Dijkstra.NET/ShortestPath/HeuristicNodeComparer.cs b/src/Dijkstra.NET/ShortestPath/HeuristicNodeComparer.cs new file mode 100644 index 0000000..03ca151 --- /dev/null +++ b/src/Dijkstra.NET/ShortestPath/HeuristicNodeComparer.cs @@ -0,0 +1,30 @@ +using System; +using System.Collections.Generic; + +namespace Dijkstra.NET.ShortestPath +{ + internal class HeuristicNodeComparer : NodeComparer + { + private readonly Func _heuristic; + + public HeuristicNodeComparer(IDictionary distance, Func heuristic) : base(distance) + { + _heuristic = heuristic; + } + + public override int Compare(uint x, uint y) + { + int xDistance = _distance.ContainsKey(x) ? _distance[x] + _heuristic(x, y) : Int32.MaxValue; + int yDistance = _distance.ContainsKey(y) ? _distance[y] + _heuristic(y, x) : Int32.MaxValue; + + int comparer = xDistance.CompareTo(yDistance); + + if (comparer == 0) + { + return x.CompareTo(y); + } + + return comparer; + } + } +} diff --git a/src/Dijkstra.NET/ShortestPath/NodeComparer.cs b/src/Dijkstra.NET/ShortestPath/NodeComparer.cs index 57de565..6400311 100644 --- a/src/Dijkstra.NET/ShortestPath/NodeComparer.cs +++ b/src/Dijkstra.NET/ShortestPath/NodeComparer.cs @@ -5,14 +5,14 @@ namespace Dijkstra.NET.ShortestPath { internal class NodeComparer : IComparer { - private readonly IDictionary _distance; + protected readonly IDictionary _distance; public NodeComparer(IDictionary distance) { _distance = distance; } - public int Compare(uint x, uint y) + public virtual int Compare(uint x, uint y) { int xDistance = _distance.ContainsKey(x) ? _distance[x] : Int32.MaxValue; int yDistance = _distance.ContainsKey(y) ? _distance[y] : Int32.MaxValue; @@ -27,4 +27,4 @@ public int Compare(uint x, uint y) return comparer; } } -} +} \ No newline at end of file