/// <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, the <see cref="PathFindingResult{T}"/> will be empty, but no members will be null. /// <br/><see cref="PathFindingResult{T}"/> will contain the found path, the nodes that were queued to be evaluated /// (in <see cref="PathFindingResult{T}.OpenNodes"/>) and the nodes that were finally evaluated /// (in <see cref="PathFindingResult{T}.ClosedNodes"/>) /// </summary> 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); if (start.Equals(end)) { return(PathFindingResult <T> .Empty); } // check if another calculator has been provided, if not choose the one from the graph calculator = calculator ?? g.Calculator; // initialize the open and closed list // the closed list contains all nodes that we don't want to evaluate ever again, they are done var closed = new HashSet <AStarNode <T> >(); // the open list contains the nodes that are queued up for evaluation at some point var open = new HashSet <AStarNode <T> > // first we enqueue the provided start node, with an estimated distance of start to end // it should be possible to make this float.maxValue instead { new AStarNode <T>(start, distanceToFinish: g.Calculator.Distance(start, end)) }; // we have nodes to evaluate while (open.Count > 0) { // get most promising node var recordHolder = AStarGetMostPromisingNode(open); if (recordHolder == null) { throw new InvalidOperationException("The most promising node was null. This should never ever happen!"); } // check if the record node is the finish and return the corresponding data if it is 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)))); } // if we chose a node, we are kind of done with evaluating it again // (there are scenarios where that might be a good idea) open.Remove(recordHolder); closed.Add(recordHolder); // expand the record holder. AStarExpandNode(recordHolder, end, g.GetPassableConnections(recordHolder.Data), open, closed, calculator); } // if we leave the loop (we have no nodes to evaluate), we have no path return(PathFindingResult <T> .Empty); }
/// <summary> /// The actual recursive method that is used to calculate the path. /// <br/>This method has an "artificial" recursion anchor at <see cref="MaxDepth"/> to /// prevent a <see cref="StackOverflowException"/>. /// It will simply stop recursing deeper after a depth higher than <see cref="MaxDepth"/>. /// <br/>Use cautiously :) it can take a while. /// </summary> /// <param name="graph">The graph to perform the operation on.</param> /// <param name="start">The start node of the problem</param> /// <param name="end">The end node.</param> /// <param name="pathTo">The current path to the start node.</param> /// <param name="depth">The current depth we are at.</param> /// <typeparam name="T"></typeparam> /// <returns>Returns the reverse path from <see cref="start"/> to <see cref="end"/></returns> private static List <T> RecursiveSolve <T>(IReadOnlyGraph <T> graph, T start, T end, List <T> pathTo, int depth = 0) where T : IEquatable <T> { if (depth == MaxDepth) { return(new List <T>()); } List <T> path = null; // In this case LINQ made it way more readable :) so we keep it // we only want to ask neighbors for a path, if they aren't in the path towards us // otherwise there would be some nasty infinite loops making this bad algorithm even worse foreach (var nb in graph.GetPassableConnections(start).Where(nb => !pathTo.Contains(nb))) { // end and start have to be flipped because the path is built in reverse order if (nb.Equals(end)) { return new List <T> { end, start } } ; // hand the task of finding a path off to the neighbor var newPath = RecursiveSolve(graph, nb, end, new List <T>(pathTo) { start }, depth + 1); // the new path is only better if it exists, if there was no previous path, or // if it is shorter than the previous path if (newPath.Count > 0 && (path == null || newPath.Count < path.Count)) { path = newPath; } } // If there is a path we want to add start to it (living off our neighbors work) path?.Add(start); return(path ?? new List <T>()); } }
public static PathFindingResult <T> DijkstraSolveWithInfo <T>(this IReadOnlyGraph <T> g, T start, T end) where T : IEquatable <T> { g.ThrowOnInvalidInput(start, end); if (start.Equals(end)) { return(PathFindingResult <T> .Empty); } var startNode = new DijkstraNode <T>(start, currentPathLength: 0f); // we have visited the start node already and thus don't need to again var visited = new HashSet <DijkstraNode <T> > { startNode }; // we also want to add it to the open set though, to have a node to evaluate the neighbors from var open = new HashSet <DijkstraNode <T> > { startNode }; while (open.Count > 0) { // get the node that has the shortest saved path to the start node // TODO refactor into function (Finding best node) var curr = GetNearestNode(open); // if the best node is actually the finish: hooray lucky day, return it if (curr.Data.Equals(end)) { return(new PathFindingResult <T>(BuildPath(curr, visited), new HashSet <T>(open.Select(x => x.Data)), new HashSet <T>(visited.Select(x => x.Data)))); } // the just evaluated node isn't open for evaluation again, we were to pushy :( open.Remove(curr); // we have in fact just visited it visited.Add(curr); // expand this node DijkstraExpandNode(curr, g.GetPassableConnections(curr.Data), open, visited, g.Calculator); } // we haven't found a path return(PathFindingResult <T> .Empty); }
/// <summary> /// Use iterative BFS to find a path between <paramref name="start"/> and <paramref name="end"/>. /// This method will also return the visited nodes in the process. /// <br/>The returned <see cref="PathFindingResult{T}"/> will contain the path /// (in <see cref="PathFindingResult{T}.Path"/>), and the visited nodes /// (in <see cref="PathFindingResult{T}.OpenNodes"/>). /// <see cref="PathFindingResult{T}.ClosedNodes"/> will be empty as that is not applicable here. /// </summary> /// <param name="graph">The graph to operate on</param> /// <param name="start">The node to start searching from</param> /// <param name="end">The node that is the finish</param> public static PathFindingResult <T> IterativeBfsSolveWithInfo <T>(this IReadOnlyGraph <T> graph, T start, T end) where T : IEquatable <T> { // in BFS nodes are evaluated in order of appearance, so a Queue makes sense var nodesToCheck = new Queue <Node <T> >(); nodesToCheck.Enqueue(new Node <T>(start)); // These will be the visited nodes so we don't visit stuff multiple times (no loops either) var visited = new HashSet <Node <T> > { new Node <T>(start) }; while (nodesToCheck.Count > 0) { var currentNode = nodesToCheck.Dequeue(); var neighbors = graph.GetPassableConnections(currentNode.Data); foreach (var nb in neighbors) { // create a dummy node to check if it is the end and if we need to add it. // This works (also Contains()), because we have overriden Equals() and GetHashCode() accordingly var node = new Node <T>(nb, currentNode.Data); if (nb.Equals(end)) { return(new PathFindingResult <T>(BuildPath(node, visited), new HashSet <T>(visited.Select(x => x.Data)), new HashSet <T>())); } // Add() returns true, if the element is successfully added (no duplicates because hashset) :) if (visited.Add(node)) { nodesToCheck.Enqueue(node); } } } // TODO should we warn? Logger.Warn("No path found!"); return(PathFindingResult <T> .Empty); }