/// <summary>
/// Retrieves the first path found between two points using depth
/// first search (DFS)
/// </summary>
/// <typeparam name="T"></typeparam>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="GetNeighbors"></param>
/// <param name="GetEdgeCost"></param>
/// <returns></returns>
public static PathResult <T> GetDFSPath <T>(
T start,
T end,
Func <T, IList <T> > GetNeighbors,
Func <T, T, int> GetEdgeCost)
{
// Each entry represents a path under construction
// The first item is the head of the path
// The second item is the set of all nodes already visited in the path
var openPaths = new Stack <Tuple <PathNode <T>, HashSet <T> > >();
var startNode = new PathNode <T>(start, 0);
var rootPath = new Tuple <PathNode <T>, HashSet <T> >(startNode, new HashSet <T>()
{
start
});
openPaths.Push(rootPath);
while (openPaths.Count > 0)
{
var path = openPaths.Pop();
if (end.Equals(path.Item1.Node))
{
var pathResult = PathResult <T> .ReconstructPath(path.Item1);
return(pathResult);
}
var neighbors = GetNeighbors(path.Item1.Node);
foreach (var neighbor in neighbors)
{
if (path.Item2.Contains(neighbor))
{
continue;
}
int gScore = path.Item1.GScore + GetEdgeCost(path.Item1.Node, neighbor);
var neighborNode = new PathNode <T>(neighbor, gScore)
{
Parent = path.Item1
};
var alreadyVisited = path.Item2.ToHashSet();
alreadyVisited.Add(path.Item1.Node);
var newPath = new Tuple <PathNode <T>, HashSet <T> >(neighborNode, alreadyVisited);
openPaths.Push(newPath);
}
}
return(new PathResult <T>(new List <T>(), 0));
}
public static PathResult <T> ReconstructPath(T endNode, Dictionary <T, T> cameFrom, Dictionary <T, int> gScores)
{
var currentNode = endNode;
var path = new List <T>()
{
currentNode
};
while (cameFrom.ContainsKey(currentNode))
{
currentNode = cameFrom[currentNode];
path.Insert(0, currentNode);
}
var result = new PathResult <T>(path, gScores[endNode]);
return(result);
}
public static PathResult <T> ReconstructPath(PathNode <T> endNode)
{
// Starting with the last node in the path, work backwards through
// the parents to reconstruct the full path
var currentNode = endNode;
var path = new List <T>()
{
currentNode.Node
};
while (currentNode.Parent != null)
{
currentNode = currentNode.Parent;
path.Insert(0, currentNode.Node);
}
var result = new PathResult <T>(path, endNode.GScore);
return(result);
}
/// <summary>
/// Uses the A* pathfinding algorithm to find a path between the
/// <paramref name="startPoint"/> and <paramref name="endPoint"/>.
/// </summary>
/// <typeparam name="T"></typeparam>
/// <param name="startPoint"></param>
/// <param name="endPoint"></param>
/// <param name="heuristic"></param>
/// <returns></returns>
public static PathResult <T> GetPath <T>(
T startPoint,
T endPoint,
Func <T, int> Heuristic,
Func <T, IList <T> > GetNeighbors,
Func <T, T, int> GetEdgeCost)
{
// Pseudocode found here:
// https://brilliant.org/wiki/a-star-search/
// make an openlist containing only the starting node
// make an empty closed list
// while (the destination node has not been reached):
// consider the node with the lowest f score in the open list
// if (this node is our destination node) :
// we are finished
// if not:
// put the current node in the closed list and look at all of its neighbors
// for (each neighbor of the current node):
// if (neighbor has lower g value than current and is in the closed list) :
// replace the neighbor with the new, lower, g value
// current node is now the neighbor's parent
// else if (current g value is lower and this neighbor is in the open list ) :
// replace the neighbor with the new, lower, g value
// change the neighbor's parent to our current node
//
// else if this neighbor is not in both lists:
// add it to the open list and set its g
var startNode = new AStarNode <T>(startPoint, 0, Heuristic(startPoint));
int startPointHScore = Heuristic(startPoint);
var openSet = new SortedDictionary <int, HashSet <T> >();
openSet.Add(startPointHScore, new HashSet <T>()
{
startPoint
});
var currentFScores = new Dictionary <T, int>()
{
{ startPoint, startPointHScore }
};
var cameFrom = new Dictionary <T, T>();
var gScore = new Dictionary <T, int>()
{
{ startPoint, 0 }
};
while (openSet.Count > 0)
{
var lowestFScore = openSet.Keys.First();
var current = openSet[lowestFScore].First();
if (endPoint.Equals(current))
{
var result = PathResult <T> .ReconstructPath(current, cameFrom, gScore);
return(result);
}
openSet[lowestFScore].Remove(current);
if (openSet[lowestFScore].Count == 0)
{
openSet.Remove(lowestFScore);
}
currentFScores.Remove(current);
var neighbors = GetNeighbors(current);
foreach (var neighbor in neighbors)
{
if (!gScore.ContainsKey(neighbor))
{
gScore.Add(neighbor, int.MaxValue);
}
var neighborGScore = gScore[current] + GetEdgeCost(current, neighbor);
if (neighborGScore < gScore[neighbor])
{
if (!cameFrom.ContainsKey(neighbor))
{
cameFrom.Add(neighbor, current);
}
cameFrom[neighbor] = current;
gScore[neighbor] = neighborGScore;
var neighborFScore = gScore[neighbor] + Heuristic(neighbor);
if (currentFScores.ContainsKey(neighbor))
{
int currentNeighborFScore = currentFScores[neighbor];
if (currentNeighborFScore != neighborFScore)
{
openSet[currentNeighborFScore].Remove(neighbor);
if (openSet[currentNeighborFScore].Count == 0)
{
openSet.Remove(currentNeighborFScore);
}
}
currentFScores[neighbor] = neighborFScore;
}
else
{
currentFScores.Add(neighbor, neighborFScore);
}
if (!openSet.ContainsKey(neighborFScore))
{
openSet.Add(neighborFScore, new HashSet <T>());
}
if (!openSet[neighborFScore].Contains(neighbor))
{
openSet[neighborFScore].Add(neighbor);
}
if (!currentFScores.ContainsKey(neighbor))
{
currentFScores.Add(neighbor, neighborFScore);
}
}
}
}
return(new PathResult <T>(new List <T>(), 0));
}
/// <summary>
/// Gets all paths between two points. WARNING: The number of paths
/// between two points grows exponentially with the graph size. Use
/// at your own risk.
/// </summary>
/// <typeparam name="T"></typeparam>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="GetNeighbors"></param>
/// <param name="GetEdgeCost"></param>
/// <returns></returns>
public static IList <PathResult <T> > GetAllPaths <T>(
T start,
T end,
Func <T, IList <T> > GetNeighbors,
Func <T, T, int> GetEdgeCost)
{
// Each entry represents a path
// The first item is the head of the path
// The second item is the set of all nodes already visited in the path
var result = new List <PathResult <T> >();
var foundPaths = new List <PathNode <T> >();
var openPaths = new Queue <Tuple <PathNode <T>, HashSet <T> > >();
var startNode = new PathNode <T>(start, 0);
var rootPath = new Tuple <PathNode <T>, HashSet <T> >(startNode, new HashSet <T>()
{
start
});
openPaths.Enqueue(rootPath);
while (openPaths.Count > 0)
{
var pathsToAddToOpenPath = new Queue <Tuple <PathNode <T>, HashSet <T> > >();
int processCount = openPaths.Count;
var doneEvent = new ManualResetEvent(false);
while (openPaths.Count > 0)
{
ThreadPool.QueueUserWorkItem(new WaitCallback((obj) =>
{
var path = (Tuple <PathNode <T>, HashSet <T> >)obj;
if (end.Equals(path.Item1.Node))
{
lock (foundPaths)
{
foundPaths.Add(path.Item1);
}
if (Interlocked.Decrement(ref processCount) == 0)
{
doneEvent.Set();
}
return;
}
var neighbors = GetNeighbors(path.Item1.Node);
foreach (var neighbor in neighbors)
{
if (path.Item2.Contains(neighbor))
{
continue;
}
int gScore = path.Item1.GScore + GetEdgeCost(path.Item1.Node, neighbor);
var neighborNode = new PathNode <T>(neighbor, gScore)
{
Parent = path.Item1
};
var alreadyVisited = path.Item2.ToHashSet();
alreadyVisited.Add(path.Item1.Node);
var newPath = new Tuple <PathNode <T>, HashSet <T> >(neighborNode, alreadyVisited);
lock (pathsToAddToOpenPath)
{
pathsToAddToOpenPath.Enqueue(newPath);
}
}
if (Interlocked.Decrement(ref processCount) == 0)
{
doneEvent.Set();
}
}), openPaths.Dequeue());
}
doneEvent.WaitOne();
openPaths = pathsToAddToOpenPath;
}
foreach (var foundPath in foundPaths)
{
var pathResult = PathResult <T> .ReconstructPath(foundPath);
result.Add(pathResult);
}
return(result);
}