/** 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); }