/** Queries the tree for the best node, searching within a circle around \a p with the specified radius.
* Will fill in both the constrained node and the not constrained node in the NNInfo.
*
* \see QueryClosest
*/
public NNInfo QueryCircle(Int3 p, int radius, NNConstraint constraint)
{
if (count == 0)
{
return(new NNInfo(null));
}
var nnInfo = new NNInfo(null);
SearchBoxCircle(0, p, radius, constraint, ref nnInfo);
nnInfo.UpdateInfo();
return(nnInfo);
}
public NNInfo Query(Int3 p, NNConstraint constraint)
{
if (count == 0)
{
return(new NNInfo(null));
}
var nnInfo = new NNInfo();
SearchBox(0, p, constraint, ref nnInfo);
nnInfo.UpdateInfo();
return(nnInfo);
}
void SearchBox(int boxi, Int3 p, NNConstraint constraint, ref NNInfo nnInfo)
{ //, int intendentLevel = 0) {
BBTreeBox box = arr[boxi];
if (box.node != null)
{
//Leaf node
if (box.node.ContainsPoint(p))
{
//Update the NNInfo
if (nnInfo.node == null)
{
nnInfo.node = box.node;
}
else if (Mathf.Abs((box.node.position).y - p.y) < Mathf.Abs((nnInfo.node.position).y - p.y))
{
nnInfo.node = box.node;
}
if (constraint.Suitable(box.node))
{
if (nnInfo.constrainedNode == null)
{
nnInfo.constrainedNode = box.node;
}
else if (Mathf.Abs(box.node.position.y - p.y) < Mathf.Abs(nnInfo.constrainedNode.position.y - p.y))
{
nnInfo.constrainedNode = box.node;
}
}
}
return;
}
//Search children
if (arr[box.left].Contains(p))
{
SearchBox(box.left, p, constraint, ref nnInfo);
}
if (arr[box.right].Contains(p))
{
SearchBox(box.right, p, constraint, ref nnInfo);
}
}
public NNInfo GetNearest(Int3 position, NNConstraint constraint, bool accurateNearestNode = false)
{
if (constraint == null)
{
constraint = NNConstraint.None;
}
//Searches in radiuses of 0.05 - 0.2 - 0.45 ... 1.28 times the average of the width and depth of the bbTree
int w = (bbTree.Size.Width + bbTree.Size.Height) / 100;
NNInfo query = bbTree.QueryCircle(position, w, constraint);
if (query.node == null)
{
for (int i = 1; i <= 8; i++)
{
query = bbTree.QueryCircle(position, i * i * w, constraint);
if (query.node != null || (i - 1) * (i - 1) * w > maxNearestNodeDistance * 2)
{ // *2 for a margin
break;
}
}
}
if (query.node != null)
{
query.clampedPosition = ClosestPointOnNode(query.node, position);
}
if (query.constrainedNode != null)
{
if (constraint.constrainDistance && ((Vector3)query.constrainedNode.position - position.vec3).sqrMagnitude > maxNearestNodeDistanceSqr)
{
query.constrainedNode = null;
}
else
{
query.constClampedPosition = ClosestPointOnNode(query.constrainedNode, position);
}
}
return(query);
}
public override Int3 GetNearestPosition(Int3 position)
{
if (IsWalkable(position))
{
return(position);
}
NNInfo info = GetNearest(position, NNConstraint.None);
Int3 clamp = info.clampedPosition;
Int3 dir = info.node.position - clamp;
Int3 mod = clamp;
int d = 10;
while (mod == clamp && d > 0)
{
mod = clamp + dir / d;
d--;
}
return(mod);
}
void SearchBoxCircle(int boxi, Int3 p, int radius, NNConstraint constraint, ref NNInfo nnInfo)
{ //, int intendentLevel = 0) {
BBTreeBox box = arr[boxi];
if (box.node != null)
{
//Leaf node
if (NodeIntersectsCircle(box.node, p, radius))
{
//Update the NNInfo
#if ASTARDEBUG
Debug.DrawLine((Vector3)box.node.GetVertex(0), (Vector3)box.node.GetVertex(1), Color.red);
Debug.DrawLine((Vector3)box.node.GetVertex(1), (Vector3)box.node.GetVertex(2), Color.red);
Debug.DrawLine((Vector3)box.node.GetVertex(2), (Vector3)box.node.GetVertex(0), Color.red);
#endif
Int3 closest = box.node.ClosestPointOnNode(p); //NavMeshGraph.ClosestPointOnNode (box.node,graph.vertices,p);
double dist = (closest - p).sqrMagnitude;
if (nnInfo.node == null)
{
nnInfo.node = box.node;
nnInfo.clampedPosition = closest;
}
else if (dist < (nnInfo.clampedPosition - p).sqrMagnitude)
{
nnInfo.node = box.node;
nnInfo.clampedPosition = closest;
}
if (constraint == null || constraint.Suitable(box.node))
{
if (nnInfo.constrainedNode == null)
{
nnInfo.constrainedNode = box.node;
nnInfo.constClampedPosition = closest;
}
else if (dist < (nnInfo.constClampedPosition - p).sqrMagnitude)
{
nnInfo.constrainedNode = box.node;
nnInfo.constClampedPosition = closest;
}
}
}
else
{
#if ASTARDEBUG
Debug.DrawLine((Vector3)box.node.GetVertex(0), (Vector3)box.node.GetVertex(1), Color.blue);
Debug.DrawLine((Vector3)box.node.GetVertex(1), (Vector3)box.node.GetVertex(2), Color.blue);
Debug.DrawLine((Vector3)box.node.GetVertex(2), (Vector3)box.node.GetVertex(0), Color.blue);
#endif
}
return;
}
#if ASTARDEBUG
Debug.DrawLine(new Vector3(box.rect.xmin, 0, box.rect.ymin), new Vector3(box.rect.xmax, 0, box.rect.ymin), Color.white);
Debug.DrawLine(new Vector3(box.rect.xmin, 0, box.rect.ymax), new Vector3(box.rect.xmax, 0, box.rect.ymax), Color.white);
Debug.DrawLine(new Vector3(box.rect.xmin, 0, box.rect.ymin), new Vector3(box.rect.xmin, 0, box.rect.ymax), Color.white);
Debug.DrawLine(new Vector3(box.rect.xmax, 0, box.rect.ymin), new Vector3(box.rect.xmax, 0, box.rect.ymax), Color.white);
#endif
//Search children
if (RectIntersectsCircle(arr[box.left].rect, p, radius))
{
SearchBoxCircle(box.left, p, radius, constraint, ref nnInfo);
}
if (RectIntersectsCircle(arr[box.right].rect, p, radius))
{
SearchBoxCircle(box.right, p, radius, constraint, ref nnInfo);
}
}
void SearchBoxClosest(int boxi, Int3 p, ref int closestDist, NNConstraint constraint, ref NNInfo nnInfo)
{
BBTreeBox box = arr[boxi];
if (box.node != null)
{
//Leaf node
if (NodeIntersectsCircle(box.node, p, closestDist))
{
//Update the NNInfo
#if ASTARDEBUG
Debug.DrawLine((Vector3)box.node.GetVertex(1) + Vector3.up * 0.2f, (Vector3)box.node.GetVertex(2) + Vector3.up * 0.2f, Color.red);
Debug.DrawLine((Vector3)box.node.GetVertex(0) + Vector3.up * 0.2f, (Vector3)box.node.GetVertex(1) + Vector3.up * 0.2f, Color.red);
Debug.DrawLine((Vector3)box.node.GetVertex(2) + Vector3.up * 0.2f, (Vector3)box.node.GetVertex(0) + Vector3.up * 0.2f, Color.red);
#endif
Int3 closest = box.node.ClosestPointOnNode(p);
if (constraint == null || constraint.Suitable(box.node))
{
double dist = (closest - p).sqrMagnitude;
if (nnInfo.constrainedNode == null)
{
nnInfo.constrainedNode = box.node;
nnInfo.constClampedPosition = closest;
closestDist = (int)Math.Round(Math.Sqrt(dist));
}
else if (dist < closestDist * closestDist)
{
nnInfo.constrainedNode = box.node;
nnInfo.constClampedPosition = closest;
closestDist = (int)Math.Round(Math.Sqrt(dist));
}
}
}
else
{
#if ASTARDEBUG
Debug.DrawLine((Vector3)box.node.GetVertex(0), (Vector3)box.node.GetVertex(1), Color.blue);
Debug.DrawLine((Vector3)box.node.GetVertex(1), (Vector3)box.node.GetVertex(2), Color.blue);
Debug.DrawLine((Vector3)box.node.GetVertex(2), (Vector3)box.node.GetVertex(0), Color.blue);
#endif
}
}
else
{
#if ASTARDEBUG
Debug.DrawLine(new Vector3(box.rect.xmin, 0, box.rect.ymin), new Vector3(box.rect.xmax, 0, box.rect.ymin), Color.white);
Debug.DrawLine(new Vector3(box.rect.xmin, 0, box.rect.ymax), new Vector3(box.rect.xmax, 0, box.rect.ymax), Color.white);
Debug.DrawLine(new Vector3(box.rect.xmin, 0, box.rect.ymin), new Vector3(box.rect.xmin, 0, box.rect.ymax), Color.white);
Debug.DrawLine(new Vector3(box.rect.xmax, 0, box.rect.ymin), new Vector3(box.rect.xmax, 0, box.rect.ymax), Color.white);
#endif
//Search children
if (RectIntersectsCircle(arr[box.left].rect, p, closestDist))
{
SearchBoxClosest(box.left, p, ref closestDist, constraint, ref nnInfo);
}
if (RectIntersectsCircle(arr[box.right].rect, p, closestDist))
{
SearchBoxClosest(box.right, p, ref closestDist, constraint, ref nnInfo);
}
}
}
/** 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, unlike QueryCircle, 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.
*
* \see QueryCircle
*/
public NNInfo QueryClosest(Int3 p, NNConstraint constraint, ref int distance, NNInfo previous)
{
if (count == 0)
{
return(previous);
}
SearchBoxClosest(0, p, ref distance, constraint, ref previous);
return(previous);
}