//////////////////////////////////////////////////////////////////////////
public void Init(FindPathOptions options, iExplorer <T> exlorer, T start, T[] goal)
{
if (Result == FindPathResult.None)
{
// save data
Options = options;
Explorer = exlorer;
Start = start;
Goal = goal;
// set running state
Result = FindPathResult.Running;
// allocate collections
Path = new LinkedList <T>();
OpenSet = new FastPriorityQueue <PathNode <T> >(Pathfinder.c_AdaptiveBufferExperiance.Average);
ClosedSet = new HashSet <T>();
ClosedNodes = new LinkedList <PathNode <T> >();
// create start node, add to open set
var startNode = new PathNode <T>(start)
{
СameFrom = null,
PathCost = 0.0f,
PathCostEstimated = 0.0f,
Cost = 0.0f
};
OpenSet.Enqueue(startNode, startNode.Cost);
}
}
public static FindPathProcess <T> FindPath <T>(this iExplorer <T> explorer, FindPathOptions options, T start, params T[] goal)
{
// set default options
if (options == null)
{
options = FindPathOptions.c_Default;
}
// create result
var result = new FindPathProcess <T>();
// common sense check
if (start == null || goal.Length == 0 || explorer == null)
{
result.Complete(FindPathResult.BadArguments);
return(result);
}
// find only reachable goals
goal = goal
.Where(n => explorer.Reachable(start, n))
.ToArray();
// is reachable
if (goal.Length == 0)
{
result.Complete(FindPathResult.NotReachable);
return(result);
}
// init result inner data
result.Init(options, explorer, start, goal);
return(result);
}
public static FindPathResult <T> FindPath <T>(iExplorer <T> explorer, T start, List <T> goal)
where T : PathNodeFPQN
{
var result = new FindPathResult <T>(c_MaxElementsInQueue, c_MaxElementsInQueue);
if (start == null || goal == null || explorer == null)
{
return(result);
}
var closedSet = result.m_ClosedSet;
var openSet = result.m_OpenSet;
start.PathLengthFromStart = 0.0f;
start.PathLengthEstimated = implGetShortestPossiblePath(explorer, start, goal);
openSet.Enqueue(start, 0);
while (closedSet.Count < c_MaxElementsInQueue && openSet.Count > 0)
{
var currentNode = openSet.Dequeue();
foreach (var n in goal)
{
if (currentNode == n)
{
implGetPathForNode <T>(currentNode, result.m_Path);
return(result);
}
}
closedSet.Enqueue(currentNode, currentNode.PathLengthEstimated + currentNode.PathLengthFromStart);
foreach (var neighbourNode in explorer.iGetNeighbours(currentNode as T))
{
if (closedSet.Contains(neighbourNode)) // skip if already checked
{
continue;
}
var pathLengthFromStart = currentNode.PathLengthFromStart + explorer.iGetPathCost(currentNode as T, neighbourNode);
if (openSet.Contains(neighbourNode))
{ // if presented and part is shorter then reset his parent and cost
if (neighbourNode.PathLengthFromStart > pathLengthFromStart)
{
neighbourNode.СameFrom = currentNode;
neighbourNode.PathLengthFromStart = pathLengthFromStart;
openSet.UpdatePriority(neighbourNode, neighbourNode.PathLengthEstimated + neighbourNode.PathLengthFromStart);
}
}
else
{ // if not presented add as wariant
neighbourNode.СameFrom = currentNode;
neighbourNode.PathLengthFromStart = pathLengthFromStart;
neighbourNode.PathLengthEstimated = implGetShortestPossiblePath(explorer, neighbourNode as T, goal);
openSet.Enqueue(neighbourNode, neighbourNode.PathLengthEstimated + neighbourNode.PathLengthFromStart);
}
}
}
return(null);
}
private static float implGetShortestPossiblePath <T>(iExplorer <T> explorer, T start, List <T> goal)
{
var shortestPath = float.MaxValue;
foreach (var n in goal)
{
var currentShortestPath = explorer.iGetShortestPossiblePath(start, n);
if (shortestPath > currentShortestPath)
{
shortestPath = currentShortestPath;
}
}
return(shortestPath);
}
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 FindPathProcess <T> FindPath <T>(this iExplorer <T> explorer, out FindPathProcess <T> findPathProcess, FindPathOptions options, T start, params T[] goal)
{
findPathProcess = explorer.FindPath(options, start, goal);
return(findPathProcess);
}
//////////////////////////////////////////////////////////////////////////
public static FindPathProcess <T> FindPath <T>(this iExplorer <T> explorer, T start, params T[] goal)
{
return(explorer.FindPath(FindPathOptions.c_Default, start, goal));
}
