/**
* Returns the nearest node to a position using the specified NNConstraint.
* Searches through all graphs for their nearest nodes to the specified position and picks the closest one.
* The NNConstraint can be used to specify constraints on which nodes can be chosen such as only picking walkable nodes.
* \see Pathfinding.NNConstraint
*/
public static NNInfo GetNearest(Vector3 position, NNConstraint constraint, GraphNode hint)
{
// Cache property lookup
var localGraphs = GetConfig().graphs;
float minDist = float.PositiveInfinity;
NNInfoInternal nearestNode = new NNInfoInternal();
int nearestGraph = -1;
if (localGraphs != null)
{
for (int i = 0; i < localGraphs.Length; i++)
{
NavGraph graph = localGraphs[i];
// Check if this graph should be searched
if (graph == null || !constraint.SuitableGraph(i, graph))
{
continue;
}
NNInfoInternal nnInfo;
if (GetConfig().fullGetNearestSearch)
{
// Slower nearest node search
// this will try to find a node which is suitable according to the constraint
nnInfo = graph.GetNearestForce(position, constraint);
}
else
{
// Fast nearest node search
// just find a node close to the position without using the constraint that much
// (unless that comes essentially 'for free')
nnInfo = graph.GetNearest(position, constraint);
}
GraphNode node = nnInfo.node;
// No node found in this graph
if (node == null)
{
continue;
}
// Distance to the closest point on the node from the requested position
float dist = ((Vector3)nnInfo.clampedPosition - position).magnitude;
if (GetConfig().prioritizeGraphs&& dist < GetConfig().prioritizeGraphsLimit)
{
// The node is close enough, choose this graph and discard all others
minDist = dist;
nearestNode = nnInfo;
nearestGraph = i;
break;
}
else
{
// Choose the best node found so far
if (dist < minDist)
{
minDist = dist;
nearestNode = nnInfo;
nearestGraph = i;
}
}
}
}
// No matches found
if (nearestGraph == -1)
{
return(new NNInfo());
}
// Check if a constrained node has already been set
if (nearestNode.constrainedNode != null)
{
nearestNode.node = nearestNode.constrainedNode;
nearestNode.clampedPosition = nearestNode.constClampedPosition;
}
if (!GetConfig().fullGetNearestSearch&& nearestNode.node != null && !constraint.Suitable(nearestNode.node))
{
// Otherwise, perform a check to force the graphs to check for a suitable node
NNInfoInternal nnInfo = localGraphs[nearestGraph].GetNearestForce(position, constraint);
if (nnInfo.node != null)
{
nearestNode = nnInfo;
}
}
if (!constraint.Suitable(nearestNode.node) || (constraint.constrainDistance &&
(nearestNode.clampedPosition - position).sqrMagnitude > GetConfig().maxNearestNodeDistanceSqr))
{
return(new NNInfo());
}
// Convert to NNInfo which doesn't have all the internal fields
return(new NNInfo(nearestNode));
}
/** Queries the tree for the closest node to \a p constrained by the NNConstraint trying to improve an existing solution.
* Note that this function will only fill in the constrained node.
* If you want a node not constrained by any NNConstraint, do an additional search with constraint = NNConstraint.None
*
* \param p Point to search around
* \param constraint Optionally set to constrain which nodes to return
* \param distance The best distance for the \a previous solution. Will be updated with the best distance
* after this search. Will be positive infinity if no node could be found.
* Set to positive infinity if there was no previous solution.
* \param previous This search will start from the \a previous NNInfo and improve it if possible.
* Even if the search fails on this call, the solution will never be worse than \a previous.
*/
public NNInfoInternal QueryClosest(Vector3 p, NNConstraint constraint, ref float distance, NNInfoInternal previous)
{
var sqrDistance = distance * distance;
var origSqrDistance = sqrDistance;
if (count > 0 && SquaredRectPointDistance(tree[0].rect, p) < sqrDistance)
{
SearchBoxClosest(0, p, ref sqrDistance, constraint, ref previous);
// Only update the distance if the squared distance changed as otherwise #distance
// might change due to rounding errors even if no better solution was found
if (sqrDistance < origSqrDistance)
{
distance = Mathf.Sqrt(sqrDistance);
}
}
return(previous);
}
void SearchBoxClosest(int boxi, Vector3 p, ref float closestSqrDist, NNConstraint constraint, ref NNInfoInternal nnInfo)
{
BBTreeBox box = tree[boxi];
if (box.IsLeaf)
{
var nodes = nodeLookup;
for (int i = 0; i < MaximumLeafSize && nodes[box.nodeOffset + i] != null; i++)
{
var node = nodes[box.nodeOffset + i];
Vector3 closest = node.ClosestPointOnNode(p);
float dist = (closest - p).sqrMagnitude;
if (dist < closestSqrDist)
{
#if !SERVER
DrawDebugNode(node, 0.2f, UnityEngine.Color.red);
#endif
if (constraint == null || constraint.Suitable(node))
{
// Update the NNInfo
nnInfo.constrainedNode = node;
nnInfo.constClampedPosition = closest;
closestSqrDist = dist;
}
}
else
{
#if !SERVER
DrawDebugNode(node, 0.0f, UnityEngine.Color.blue);
#endif
}
}
}
else
{
#if !SERVER
DrawDebugRect(box.rect);
#endif
int first = box.left, second = box.right;
float firstDist, secondDist;
GetOrderedChildren(ref first, ref second, out firstDist, out secondDist, p);
// Search children (closest box first to improve performance)
if (firstDist < closestSqrDist)
{
SearchBoxClosest(first, p, ref closestSqrDist, constraint, ref nnInfo);
}
if (secondDist < closestSqrDist)
{
SearchBoxClosest(second, p, ref closestSqrDist, constraint, ref nnInfo);
}
}
}
void SearchBoxClosestXZ(int boxi, Vector3 p, ref float closestSqrDist, NNConstraint constraint, ref NNInfoInternal nnInfo)
{
BBTreeBox box = tree[boxi];
if (box.IsLeaf)
{
var nodes = nodeLookup;
for (int i = 0; i < MaximumLeafSize && nodes[box.nodeOffset + i] != null; i++)
{
var node = nodes[box.nodeOffset + i];
// Update the NNInfo
#if !SERVER
DrawDebugNode(node, 0.2f, UnityEngine.Color.red);
#endif
if (constraint == null || constraint.Suitable(node))
{
Vector3 closest = node.ClosestPointOnNodeXZ(p);
// XZ squared distance
float dist = (closest.x - p.x) * (closest.x - p.x) + (closest.z - p.z) * (closest.z - p.z);
// There's a theoretical case when the closest point is on the edge of a node which may cause the
// closest point's xz coordinates to not line up perfectly with p's xz coordinates even though they should
// (because floating point errors are annoying). So use a tiny margin to cover most of those cases.
const float fuzziness = 0.000001f;
if (nnInfo.constrainedNode == null || dist < closestSqrDist - fuzziness || (dist <= closestSqrDist + fuzziness && Mathf.Abs(closest.y - p.y) < Mathf.Abs(nnInfo.constClampedPosition.y - p.y)))
{
nnInfo.constrainedNode = node;
nnInfo.constClampedPosition = closest;
closestSqrDist = dist;
}
}
}
}
else
{
#if !SERVER
DrawDebugRect(box.rect);
#endif
int first = box.left, second = box.right;
float firstDist, secondDist;
GetOrderedChildren(ref first, ref second, out firstDist, out secondDist, p);
// Search children (closest box first to improve performance)
if (firstDist <= closestSqrDist)
{
SearchBoxClosestXZ(first, p, ref closestSqrDist, constraint, ref nnInfo);
}
if (secondDist <= closestSqrDist)
{
SearchBoxClosestXZ(second, p, ref closestSqrDist, constraint, ref nnInfo);
}
}
}
