/** Pick N random walkable nodes from all nodes in all graphs and add them to the buffer.
*
* Here we select N random nodes from a stream of nodes.
* Probability of choosing the first N nodes is 1
* Probability of choosing node i is min(N/i,1)
* A selected node will replace a random node of the previously
* selected ones.
*
* \see https://en.wikipedia.org/wiki/Reservoir_sampling
*/
void PickNRandomNodes(int count, List <GraphNode> buffer)
{
int n = 0;
var graphs = PathFindHelper.GetConfig().graphs;
// Loop through all graphs
for (int j = 0; j < graphs.Length; j++)
{
// Loop through all nodes in the graph
graphs[j].GetNodes(node => {
if (!node.Destroyed && node.Walkable)
{
n++;
if ((GetRandom() % n) < count)
{
if (buffer.Count < count)
{
buffer.Add(node);
}
else
{
buffer[(int)(GetRandom() % buffer.Count)] = node;
}
}
}
});
}
}
/** Returns the nearest node to a position using the specified NNConstraint.
* \param position The position to try to find a close node to
* \param hint Can be passed to enable some graph generators to find the nearest node faster.
* \param constraint Can for example tell the function to try to return a walkable node. If you do not get a good node back, consider calling GetNearestForce.
*/
public virtual NNInfoInternal GetNearest(Vector3 position, NNConstraint constraint, GraphNode hint)
{
// This is a default implementation and it is pretty slow
// Graphs usually override this to provide faster and more specialised implementations
float maxDistSqr = constraint == null || constraint.constrainDistance ? PathFindHelper.GetConfig().maxNearestNodeDistanceSqr : float.PositiveInfinity;
float minDist = float.PositiveInfinity;
GraphNode minNode = null;
float minConstDist = float.PositiveInfinity;
GraphNode minConstNode = null;
// Loop through all nodes and find the closest suitable node
GetNodes(node => {
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist)
{
minDist = dist;
minNode = node;
}
if (dist < minConstDist && dist < maxDistSqr && (constraint == null || constraint.Suitable(node)))
{
minConstDist = dist;
minConstNode = node;
}
});
var nnInfo = new NNInfoInternal(minNode);
nnInfo.constrainedNode = minConstNode;
if (minConstNode != null)
{
nnInfo.constClampedPosition = (Vector3)minConstNode.position;
}
else if (minNode != null)
{
nnInfo.constrainedNode = minNode;
nnInfo.constClampedPosition = (Vector3)minNode.position;
}
return(nnInfo);
}
GraphNode PickAnyWalkableNode()
{
var graphs = PathFindHelper.GetConfig().graphs;
GraphNode first = null;
// Find any node in the graphs
for (int j = 0; j < graphs.Length; j++)
{
graphs[j].GetNodes(node => {
if (node != null && node.Walkable && first == null)
{
first = node;
}
});
}
return(first);
}
/** Estimated cost from the specified node to the target.
* \see https://en.wikipedia.org/wiki/A*_search_algorithm
*/
internal uint CalculateHScore(GraphNode node)
{
uint h;
switch (heuristic)
{
case Heuristic.Euclidean:
h = (uint)(((GetHTarget() - node.position).costMagnitude) * heuristicScale);
// Inlining this check and the return
// for each case saves an extra jump.
// This code is pretty hot
if (hTargetNode != null)
{
h = System.Math.Max(h, PathFindHelper.euclideanEmbedding.GetHeuristic(node.NodeIndex, hTargetNode.NodeIndex));
}
return(h);
case Heuristic.Manhattan:
Int3 p2 = node.position;
h = (uint)((System.Math.Abs(hTarget.x - p2.x) + System.Math.Abs(hTarget.y - p2.y) + System.Math.Abs(hTarget.z - p2.z)) * heuristicScale);
if (hTargetNode != null)
{
h = System.Math.Max(h, PathFindHelper.GetConfig().euclideanEmbedding.GetHeuristic(node.NodeIndex, hTargetNode.NodeIndex));
}
return(h);
case Heuristic.DiagonalManhattan:
Int3 p = GetHTarget() - node.position;
p.x = System.Math.Abs(p.x);
p.y = System.Math.Abs(p.y);
p.z = System.Math.Abs(p.z);
int diag = System.Math.Min(p.x, p.z);
int diag2 = System.Math.Max(p.x, p.z);
h = (uint)((((14 * diag) / 10) + (diag2 - diag) + p.y) * heuristicScale);
if (hTargetNode != null)
{
h = System.Math.Max(h, PathFindHelper.euclideanEmbedding.GetHeuristic(node.NodeIndex, hTargetNode.NodeIndex));
}
return(h);
}
return(0U);
}
/** Main pathfinding method.
* This method will calculate the paths in the pathfinding queue.
*
* \see CalculatePathsThreaded
* \see StartPath
*/
IEnumerator CalculatePaths(PathHandler pathHandler)
{
// Max number of ticks before yielding/sleeping
long maxTicks = (long)(PathFindHelper.GetConfig().maxFrameTime *10000);
long targetTick = System.DateTime.UtcNow.Ticks + maxTicks;
while (true)
{
// The path we are currently calculating
Path p = null;
while (p == null)
{
try {
p = queue.PopNoBlock(false);
} catch (Exception e) {
yield break;
}
if (p == null)
{
yield return(null);
}
}
IPathInternals ip = (IPathInternals)p;
// Max number of ticks we are allowed to continue working in one run
// One tick is 1/10000 of a millisecond
maxTicks = (long)(PathFindHelper.GetConfig().maxFrameTime *10000);
ip.PrepareBase(pathHandler);
// Now processing the path
// Will advance to Processing
ip.AdvanceState(PathState.Processing);
// Call some callbacks
// It needs to be stored in a local variable to avoid race conditions
var tmpOnPathPreSearch = OnPathPreSearch;
if (tmpOnPathPreSearch != null)
{
tmpOnPathPreSearch(p);
}
// Tick for when the path started, used for calculating how long time the calculation took
long startTicks = System.DateTime.UtcNow.Ticks;
long totalTicks = 0;
ip.Prepare();
// Check if the Prepare call caused the path to complete
// If this happens the path usually failed
if (!p.IsDone())
{
// For debug uses, we set the last computed path to p, so we can view debug info on it in the editor (scene view).
PathFindHelper.debugPathData = ip.PathHandler;
PathFindHelper.debugPathID = p.pathID;
// Initialize the path, now ready to begin search
ip.Initialize();
// The error can turn up in the Init function
while (!p.IsDone())
{
// Do some work on the path calculation.
// The function will return when it has taken too much time
// or when it has finished calculation
ip.CalculateStep(targetTick);
// If the path has finished calculation, we can break here directly instead of sleeping
// Improves latency
if (p.IsDone())
{
break;
}
totalTicks += System.DateTime.UtcNow.Ticks - startTicks;
// Yield/sleep so other threads can work
yield return(null);
startTicks = System.DateTime.UtcNow.Ticks;
targetTick = System.DateTime.UtcNow.Ticks + maxTicks;
}
totalTicks += System.DateTime.UtcNow.Ticks - startTicks;
p.duration = totalTicks * 0.0001F;
#if ProfileAstar
System.Threading.Interlocked.Increment(ref AstarPath.PathsCompleted);
#endif
}
// Cleans up node tagging and other things
ip.Cleanup();
// Call the immediate callback
// It needs to be stored in a local variable to avoid race conditions
var tmpImmediateCallback = p.immediateCallback;
if (tmpImmediateCallback != null)
{
tmpImmediateCallback(p);
}
// It needs to be stored in a local variable to avoid race conditions
var tmpOnPathPostSearch = OnPathPostSearch;
if (tmpOnPathPostSearch != null)
{
tmpOnPathPostSearch(p);
}
// Push the path onto the return stack
// It will be detected by the main Unity thread and returned as fast as possible (the next late update)
returnQueue.Enqueue(p);
ip.AdvanceState(PathState.ReturnQueue);
// Wait a bit if we have calculated a lot of paths
if (System.DateTime.UtcNow.Ticks > targetTick)
{
yield return(null);
targetTick = System.DateTime.UtcNow.Ticks + maxTicks;
}
}
}
