protected override void Prepare()
{
nnConstraint.tags = enabledTags;
var startNNInfo = PathFindHelper.GetNearest(startPoint, nnConstraint);
startPoint = startNNInfo.position;
endPoint = startPoint;
startIntPoint = (Int3)startPoint;
hTarget = (Int3)aim; //startIntPoint;
startNode = startNNInfo.node;
endNode = startNode;
#if ASTARDEBUG
Debug.DrawLine((Vector3)startNode.position, startPoint, Color.blue);
Debug.DrawLine((Vector3)endNode.position, endPoint, Color.blue);
#endif
if (startNode == null || endNode == null)
{
FailWithError("Couldn't find close nodes to the start point");
return;
}
if (!CanTraverse(startNode))
{
FailWithError("The node closest to the start point could not be traversed");
return;
}
heuristicScale = aimStrength;
}
protected override void Prepare()
{
if (startNode == null)
{
//Initialize the NNConstraint
nnConstraint.tags = enabledTags;
var startNNInfo = PathFindHelper.GetNearest(originalStartPoint, nnConstraint);
startPoint = startNNInfo.position;
startNode = startNNInfo.node;
}
else
{
startPoint = (Vector3)startNode.position;
}
#if ASTARDEBUG
Debug.DrawLine((Vector3)startNode.position, startPoint, Color.blue);
#endif
if (startNode == null)
{
FailWithError("Couldn't find a close node to the start point");
return;
}
if (!CanTraverse(startNode))
{
FailWithError("The node closest to the start point could not be traversed");
return;
}
}
/** 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;
}
}
}
});
}
}
protected override void Prepare()
{
nnConstraint.tags = enabledTags;
var startNNInfo = PathFindHelper.GetNearest(startPoint, nnConstraint);
startNode = startNNInfo.node;
if (startNode == null)
{
FailWithError("Could not find close node to the start point");
return;
}
}
/** 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);
}
/** Reset all values to their default values.
*
* \note All inheriting path types (e.g ConstantPath, RandomPath, etc.) which declare their own variables need to
* override this function, resetting ALL their variables to enable pooling of paths.
* If this is not done, trying to use that path type for pooling could result in weird behaviour.
* The best way is to reset to default values the variables declared in the extended path type and then
* call the base function in inheriting types with base.Reset().
*/
protected virtual void Reset()
{
#if ASTAR_POOL_DEBUG
pathTraceInfo = "This path was got from the pool or created from here (stacktrace):\n";
pathTraceInfo += System.Environment.StackTrace;
#endif
hasBeenReset = true;
PipelineState = (int)PathState.Created;
releasedNotSilent = false;
pathHandler = null;
callback = null;
immediateCallback = null;
errorLog = "";
completeState = PathCompleteState.NotCalculated;
path = ListPool <GraphNode> .Claim();
vectorPath = ListPool <Vector3> .Claim();
currentR = null;
duration = 0;
searchedNodes = 0;
nnConstraint = PathNNConstraint.Default;
next = null;
heuristic = PathFindHelper.heuristic;
heuristicScale = PathFindHelper.heuristicScale;
enabledTags = -1;
tagPenalties = null;
pathID = PathFindHelper.GetNextPathID();
hTarget = Int3.zero;
hTargetNode = null;
traversalProvider = null;
}
/** 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);
}
/** Prepares the path. Searches for start and end nodes and does some simple checking if a path is at all possible */
protected override void Prepare()
{
//Initialize the NNConstraint
nnConstraint.tags = enabledTags;
var startNNInfo = PathFindHelper.GetNearest(startPoint, nnConstraint);
//Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint
var pathNNConstraint = nnConstraint as PathNNConstraint;
if (pathNNConstraint != null)
{
pathNNConstraint.SetStart(startNNInfo.node);
}
startPoint = startNNInfo.position;
startIntPoint = (Int3)startPoint;
startNode = startNNInfo.node;
if (startNode == null)
{
FailWithError("Couldn't find a node close to the start point");
return;
}
if (!CanTraverse(startNode))
{
FailWithError("The node closest to the start point could not be traversed");
return;
}
// If it is declared that this path type has an end point
// Some path types might want to use most of the ABPath code, but will not have an explicit end point at this stage
if (hasEndPoint)
{
var endNNInfo = PathFindHelper.GetNearest(endPoint, nnConstraint);
endPoint = endNNInfo.position;
endNode = endNNInfo.node;
if (endNode == null)
{
FailWithError("Couldn't find a node close to the end point");
return;
}
// This should not trigger unless the user has modified the NNConstraint
if (!CanTraverse(endNode))
{
FailWithError("The node closest to the end point could not be traversed");
return;
}
// This should not trigger unless the user has modified the NNConstraint
if (startNode.Area != endNode.Area)
{
FailWithError("There is no valid path to the target");
return;
}
{
// Note, other methods assume hTarget is (Int3)endPoint
hTarget = (Int3)endPoint;
hTargetNode = endNode;
// Mark end node with flag1 to mark it as a target point
pathHandler.GetPathNode(endNode).flag1 = true;
}
}
}
/** 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;
}
}
}
protected override void Prepare()
{
nnConstraint.tags = enabledTags;
var startNNInfo = PathFindHelper.GetNearest(startPoint, nnConstraint);
startNode = startNNInfo.node;
if (startNode == null)
{
FailWithError("Could not find start node for multi target path");
return;
}
if (!CanTraverse(startNode))
{
FailWithError("The node closest to the start point could not be traversed");
return;
}
// Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint
var pathNNConstraint = nnConstraint as PathNNConstraint;
if (pathNNConstraint != null)
{
pathNNConstraint.SetStart(startNNInfo.node);
}
vectorPaths = new List <Vector3> [targetPoints.Length];
nodePaths = new List <GraphNode> [targetPoints.Length];
targetNodes = new GraphNode[targetPoints.Length];
targetsFound = new bool[targetPoints.Length];
targetNodeCount = targetPoints.Length;
bool anyWalkable = false;
bool anySameArea = false;
bool anyNotNull = false;
for (int i = 0; i < targetPoints.Length; i++)
{
var endNNInfo = PathFindHelper.GetNearest(targetPoints[i], nnConstraint);
targetNodes[i] = endNNInfo.node;
targetPoints[i] = endNNInfo.position;
if (targetNodes[i] != null)
{
anyNotNull = true;
endNode = targetNodes[i];
}
bool notReachable = false;
if (endNNInfo.node != null && CanTraverse(endNNInfo.node))
{
anyWalkable = true;
}
else
{
notReachable = true;
}
if (endNNInfo.node != null && endNNInfo.node.Area == startNode.Area)
{
anySameArea = true;
}
else
{
notReachable = true;
}
if (notReachable)
{
// Signal that the pathfinder should not look for this node because we have already found it
targetsFound[i] = true;
targetNodeCount--;
}
}
startPoint = startNNInfo.position;
startIntPoint = (Int3)startPoint;
if (!anyNotNull)
{
FailWithError("Couldn't find nodes close to the all of the end points");
return;
}
if (!anyWalkable)
{
FailWithError("No target nodes could be traversed");
return;
}
if (!anySameArea)
{
FailWithError("There are no valid paths to the targets");
return;
}
RecalculateHTarget(true);
}
/** Constructor for a graph node. */
protected GraphNode()
{
this.nodeIndex = PathFindHelper.GetNewNodeIndex();
PathFindHelper.InitializeNode(this);
}