/// <summary>
/// Create a solver object for the given IPathGraph. The PathSolver is reusable - it assumes you'll
/// potentially want to find multiple paths using the same worldGraph.
/// </summary>
/// <param name="worldGraph">Object that knows how to describe your world in pathfinding terms.</param>
public PathSolver(IPathGraph <TNode, TPassthrough> worldGraph)
{
_worldGraph = worldGraph;
_infoGraph = new Dictionary <TNode, NodeInfo>();
_seqNum = 0;
_lifetimeTimer = new System.Diagnostics.Stopwatch();
}
/// <summary>
/// Initialize a new search.
/// </summary>
/// <param name="graph">Graph over which the search is conducted.</param>
/// <param name="heuristic">Provides an estimation of the distance between the given cell and the target.</param>
/// <param name="heuristicWeightPercentage">
/// The search will aim for the shortest path when given a weight of 100%.
/// We can allow the search to find paths that aren't optimal by changing the weight.
/// The weight limits the worst case length of the path,
/// e.g. a weight of 110% will find a path no more than 10% longer than the shortest possible.
/// The benefit of allowing the search to return suboptimal paths is faster computation time.
/// The search can skip some areas of the search space, meaning it has less work to do.
/// </param>
/// <param name="targetPredicate">Determines if the given cell is the target.</param>
PathSearch(IPathGraph graph, Func <CPos, int> heuristic, int heuristicWeightPercentage, Func <CPos, bool> targetPredicate)
{
Graph = graph;
this.heuristic = heuristic;
this.heuristicWeightPercentage = heuristicWeightPercentage;
TargetPredicate = targetPredicate;
openQueue = new PriorityQueue <GraphConnection>(GraphConnection.ConnectionCostComparer);
}
int GetFrontierNodeIndex(IPathGraph <TKey, TNode> path, PriorityList <float, TNode> frontier, TKey key)
{
for (int n = 0; n < frontier.Count; n++)
{
if (frontier[n].Key.Equals(key))
{
return(n);
}
}
return(-1);
}
// Build the path from the destination.
// When we find a node that has the same previous position than itself, that node is the source node.
static List <CPos> MakePath(IPathGraph graph, CPos destination)
{
var ret = new List <CPos>();
var currentNode = destination;
while (graph[currentNode].PreviousNode != currentNode)
{
ret.Add(currentNode);
currentNode = graph[currentNode].PreviousNode;
}
ret.Add(currentNode);
return(ret);
}
private void ExploreSurroundingNodes(
Func <TNode, bool> pathingBlocked,
TNode currentNode,
PriorityList <float, TNode> frontier,
IEnumerable <TKey> surroundingNodes,
IPathMaker <TKey, TNode> pathMaker,
IPathGraph <TKey, TNode> explored)
{
foreach (TKey key in surroundingNodes)
{
TNode newNode = pathMaker.MakeNode(key, currentNode);
if (pathingBlocked(newNode))
{
continue;
}
AddNodeToFrontier(explored, newNode, frontier);
}
}
void AddNodeToFrontier(IPathGraph <TKey, TNode> path, TNode newNode, PriorityList <float, TNode> frontier)
{
var key = newNode.Key;
var travelCost = newNode.TraveledCost;
var lastNode = newNode.Previous;
var n = GetFrontierNodeIndex(path, frontier, key);
if (n >= 0)
{
if (frontier[n].TraveledCost > travelCost)
{
var node = frontier[n];
frontier[n].TraveledCost = travelCost;
frontier[n].Previous = lastNode;
frontier.RemoveAt(n);
frontier.Insert(travelCost, node);
}
}
else
{
frontier.Insert(travelCost, newNode);
}
}
private IEnumerable <TKey> GetSurroundingNodes(IGraph <TKey> graph, IPathNode <TKey, TNode> currentNode, IPathGraph <TKey, TNode> explored)
{
var key = currentNode.Key;
var surrounding = graph.GetSurroundingNodes(key, 1);
if (surrounding.Length < 2)
{
return(null);
}
return(from coordinate in graph.GetSurroundingNodes(key, 1)[1]
where explored.GetNode(coordinate) == null
select coordinate);
}