public static FindPathResult <T> FindPath <T>(this iExplorer <T> explorer, T start, T goal, CancellationToken cancellationToken, int maxChecks = c_MaxChecks, int bufferSize = c_BufferSize)
{
var result = new FindPathResult <T>();
var startNode = new PathNode <T>(start);
if (start == null || goal == null || explorer == null || explorer.iReachable(start, goal) == false)
{
return(result);
}
var closedSet = result.m_ClosedSet;
var openSet = new FastPriorityQueue <PathNode <T> >(bufferSize);
result.m_OpenSet = openSet;
startNode.СameFrom = null;
startNode.PathCost = 0.0f;
startNode.PathCostEstimated = explorer.iGetShortestPossiblePath(start, goal);
startNode.Cost = 0.0f;
openSet.Enqueue(startNode, startNode.Cost);
// do while has variants
while (openSet.Count > 0 && closedSet.Count < maxChecks && openSet.Count < bufferSize)
{
// cancellation check
if (cancellationToken.IsCancellationRequested)
{
return(result);
}
// get next node
var currentNode = openSet.First();
openSet.Remove(currentNode);
// goal check
if (currentNode.Master.Equals(goal))
{
implGetPathForNode(currentNode, result.m_Path);
return(result);
}
// close current
closedSet.Add(currentNode.Master);
// proceed connections
foreach (var neighborNode in explorer.iGetNeighbours(currentNode.Master))
{
if (closedSet.Contains(neighborNode)) // skip if already checked
{
continue;
}
var pathCost = currentNode.PathCost + explorer.iGetPathCost(currentNode.Master, neighborNode);
// can use Dictionary instead FirstOrDefault
var openNode = openSet.FirstOrDefault(n => n.Master.Equals(neighborNode));
if (openNode != null)
{ // if presented and part is shorter then reset his parent and cost
if (openNode.PathCost > pathCost)
{
openNode.СameFrom = currentNode;
openNode.PathCost = pathCost;
// update priority
openNode.Cost = openNode.PathCostEstimated + openNode.PathCost;
openSet.UpdatePriority(openNode, openNode.Cost);
}
}
else
{ // if not presented add as variant
var pathNode = new PathNode <T>(neighborNode);
pathNode.СameFrom = currentNode;
pathNode.PathCost = pathCost;
pathNode.PathCostEstimated = explorer.iGetShortestPossiblePath(pathNode.Master, goal);
pathNode.Cost = pathNode.PathCostEstimated + pathNode.PathCost;
openSet.Enqueue(pathNode, pathNode.Cost);
}
}
}
return(result);
}
public static IEnumerable <TNode> FindPath(
TNode start,
TNode end,
Func <TNode, IEnumerable <TNode> > getNeighbors,
Func <TNode, TNode, float> getCost,
Func <TNode, TNode, float> getHeuristicCost,
IEqualityComparer <TNode> equalityComparer,
int maxNodes = 1024)
{
var openList = new FastPriorityQueue <PriorityNode>(maxNodes);
var closedList = new HashSet <PriorityNode>(new PriorityNodeEqualityComparer(equalityComparer));
var startNode = new PriorityNode(start, getHeuristicCost(start, end));
openList.Enqueue(startNode, startNode.Priority);
while (openList.Any())
{
var currentNode = openList.Dequeue();
if (equalityComparer.Equals(currentNode.Node, end))
{
var path = new List <TNode>();
while (currentNode.Parent != null)
{
path.Add(currentNode.Node);
currentNode = currentNode.Parent;
}
return(((IEnumerable <TNode>)path).Reverse());
}
closedList.Add(currentNode);
foreach (var neighbor in getNeighbors(currentNode.Node))
{
var cost = currentNode.RealCost + getCost(currentNode.Node, neighbor);
if (float.IsInfinity(cost))
{
continue;
}
var openListNeighbor = openList.FirstOrDefault(x => equalityComparer.Equals(x.Node, neighbor));
if (openListNeighbor != null && cost < openListNeighbor.RealCost)
{
openList.Remove(openListNeighbor);
openListNeighbor = null;
}
var closedListNeighbor = closedList.FirstOrDefault(x => equalityComparer.Equals(x.Node, neighbor));
if (closedListNeighbor != null && cost < closedListNeighbor.RealCost)
{
closedList.Remove(closedListNeighbor);
closedListNeighbor = null;
}
if (openListNeighbor == null && closedListNeighbor == null)
{
if (openList.Count == openList.MaxSize)
{
continue;
}
var realCost = currentNode.RealCost + cost;
var heuristicCost = getHeuristicCost(neighbor, end);
var newNode = new PriorityNode(currentNode, neighbor, realCost, heuristicCost);
openList.Enqueue(newNode, newNode.Priority);
}
}
}
return(new List <TNode>());
}
