/// <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);
}