int GetBox ( IntRect rect ) { if ( count >= arr.Length ) EnsureCapacity ( count+1 ); arr[count] = new BBTreeBox ( rect ); count++; return count-1; }
public void TestIntersections(Vector3 p, float radius) { BBTreeBox box = root; TestIntersections(box, p, radius); }
void OnDrawGizmos(int boxi, int depth) { BBTreeBox box = arr[boxi]; var min = (Vector3) new Int3(box.rect.xmin, 0, box.rect.ymin); var max = (Vector3) new Int3(box.rect.xmax, 0, box.rect.ymax); Vector3 center = (min + max) * 0.5F; Vector3 size = (max - center) * 2; size = new Vector3(size.x, 1, size.z); center.y += depth * 2; Gizmos.color = AstarMath.IntToColor(depth, 1f); //new Color (0,0,0,0.2F); Gizmos.DrawCube(center, size); if (box.node != null) { } else { OnDrawGizmos(box.left, depth + 1); OnDrawGizmos(box.right, depth + 1); } }
private void SearchBoxClosestXZ(int boxi, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfoInternal nnInfo) { BBTreeBox box = this.arr[boxi]; if (box.node != null) { Vector3 vector = box.node.ClosestPointOnNodeXZ(p); if ((constraint == null) || constraint.Suitable(box.node)) { float num = ((vector.x - p.x) * (vector.x - p.x)) + ((vector.z - p.z) * (vector.z - p.z)); if ((nnInfo.constrainedNode == null) || (num < (closestDist * closestDist))) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = vector; closestDist = (float)Math.Sqrt((double)num); } } } else { if (RectIntersectsCircle(this.arr[box.left].rect, p, closestDist)) { this.SearchBoxClosestXZ(box.left, p, ref closestDist, constraint, ref nnInfo); } if (RectIntersectsCircle(this.arr[box.right].rect, p, closestDist)) { this.SearchBoxClosestXZ(box.right, p, ref closestDist, constraint, ref nnInfo); } } }
private MeshNode SearchBoxInside(int boxi, Vector3 p, NNConstraint constraint) { BBTreeBox box = this.arr[boxi]; if (box.node != null) { if (box.node.ContainsPoint((Int3)p) && ((constraint == null) || constraint.Suitable(box.node))) { return(box.node); } } else { MeshNode node; if (this.arr[box.left].Contains(p)) { node = this.SearchBoxInside(box.left, p, constraint); if (node != null) { return(node); } } if (this.arr[box.right].Contains(p)) { node = this.SearchBoxInside(box.right, p, constraint); if (node != null) { return(node); } } } return(null); }
/** Queries the tree for the best node, searching within a circle around \a p with the specified radius */ public NNInfo QueryCircle(Vector3 p, float radius, NNConstraint constraint) { BBTreeBox c = root; if (c == null) { return(null); } #if DEBUG Vector3 prev = new Vector3(1, 0, 0) * radius + p; for (double i = 0; i < Math.PI * 2; i += Math.PI / 50.0) { Vector3 cpos = new Vector3((float)Math.Cos(i), 0, (float)Math.Sin(i)) * radius + p; Debug.DrawLine(prev, cpos, Color.yellow); prev = cpos; } #endif NNInfo nnInfo = new NNInfo(null); SearchBoxCircle(c, p, radius, constraint, ref nnInfo); nnInfo.UpdateInfo(); return(nnInfo); }
private void SearchBoxClosest(int boxi, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfoInternal nnInfo) { BBTreeBox box = this.arr[boxi]; if (box.node != null) { if (NodeIntersectsCircle(box.node, p, closestDist)) { Vector3 vector = box.node.ClosestPointOnNode(p); if ((constraint == null) || constraint.Suitable(box.node)) { Vector3 vector2 = vector - p; float sqrMagnitude = vector2.sqrMagnitude; if ((nnInfo.constrainedNode == null) || (sqrMagnitude < (closestDist * closestDist))) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = vector; closestDist = (float)Math.Sqrt((double)sqrMagnitude); } } } } else { if (RectIntersectsCircle(this.arr[box.left].rect, p, closestDist)) { this.SearchBoxClosest(box.left, p, ref closestDist, constraint, ref nnInfo); } if (RectIntersectsCircle(this.arr[box.right].rect, p, closestDist)) { this.SearchBoxClosest(box.right, p, ref closestDist, constraint, ref nnInfo); } } }
int GetBox ( MeshNode node ) { if ( count >= arr.Length ) EnsureCapacity ( count+1 ); arr[count] = new BBTreeBox ( node ); count++; return count-1; }
void EnsureCapacity ( int c ) { if ( arr.Length < c ) { var narr = new BBTreeBox[Math.Max ( c , (int)(arr.Length*1.5f))]; for ( int i = 0; i < count; i++ ) { narr[i] = arr[i]; } arr = narr; } }
private void SearchBoxCircle(int boxi, Vector3 p, float radius, NNConstraint constraint, ref NNInfo nnInfo) { BBTreeBox box = this.arr[boxi]; if (box.node != null) { if (NodeIntersectsCircle(box.node, p, radius)) { Vector3 vector = box.node.ClosestPointOnNode(p); Vector3 vector2 = vector - p; float sqrMagnitude = vector2.sqrMagnitude; if (nnInfo.node == null) { nnInfo.node = box.node; nnInfo.clampedPosition = vector; } else { Vector3 vector3 = nnInfo.clampedPosition - p; if (sqrMagnitude < vector3.sqrMagnitude) { nnInfo.node = box.node; nnInfo.clampedPosition = vector; } } if ((constraint == null) || constraint.Suitable(box.node)) { if (nnInfo.constrainedNode == null) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = vector; } else { Vector3 vector4 = nnInfo.constClampedPosition - p; if (sqrMagnitude < vector4.sqrMagnitude) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = vector; } } } } } else { if (RectIntersectsCircle(this.arr[box.left].rect, p, radius)) { this.SearchBoxCircle(box.left, p, radius, constraint, ref nnInfo); } if (RectIntersectsCircle(this.arr[box.right].rect, p, radius)) { this.SearchBoxCircle(box.right, p, radius, constraint, ref nnInfo); } } }
void SearchBoxCircle(int boxi, Vector3 p, float 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 DrawDebugNode(box.node, 0.0f, Color.red); Vector3 closest = box.node.ClosestPointOnNode(p); //NavMeshGraph.ClosestPointOnNode (box.node,graph.vertices,p); float 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 || dist < (nnInfo.constClampedPosition - p).sqrMagnitude) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = closest; } } } else { DrawDebugNode(box.node, 0.0f, Color.blue); } return; } DrawDebugRect(box.rect); //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 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 DrawDebugNode(node, 0.2f, Color.red); 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 { DrawDebugRect(box.rect); 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); } } }
public MeshNode QueryInside(Vector3 p, NNConstraint constraint) { BBTreeBox c = root; if (c == null) { return(null); } return(SearchBoxInside(c, p, constraint)); }
int GetBox(MeshNode node, IntRect bounds) { if (count >= arr.Length) { EnsureCapacity(count + 1); } arr[count] = new BBTreeBox(node, bounds); count++; return(count - 1); }
int GetBox(IntRect rect) { if (count >= tree.Length) { EnsureCapacity(count + 1); } tree[count] = new BBTreeBox(rect); count++; return(count - 1); }
private void EnsureCapacity(int c) { if (this.arr.Length < c) { BBTreeBox[] boxArray = new BBTreeBox[Math.Max(c, (int)(this.arr.Length * 1.5f))]; for (int i = 0; i < this.count; i++) { boxArray[i] = this.arr[i]; } this.arr = boxArray; } }
void EnsureCapacity(int c) { if (arr.Length < c) { var narr = new BBTreeBox[Math.Max(c, (int)(arr.Length * 1.5f))]; for (int i = 0; i < count; i++) { narr[i] = arr[i]; } arr = narr; } }
public new void ToString() { Console.WriteLine("Root " + (root.node != null ? root.node.ToString() : "")); BBTreeBox c = root; Stack <BBTreeBox> stack = new Stack <BBTreeBox>(); stack.Push(c); c.WriteChildren(0); }
public void TestIntersections(BBTreeBox box, Vector3 p, float radius) { if (box == null) { return; } RectIntersectsCircle(box.rect, p, radius); TestIntersections(box.c1, p, radius); TestIntersections(box.c2, p, radius); }
/** 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 * * 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. * * * \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. * * * \see QueryCircle */ public NNInfo QueryClosest(Vector3 p, NNConstraint constraint, ref float distance, NNInfo previous) { BBTreeBox c = root; if (c == null) { return(previous); } SearchBoxClosest(c, p, ref distance, constraint, ref previous); return(previous); }
TriangleMeshNode SearchBoxInside(int boxi, Vector3 p, NNConstraint constraint) { 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]; if (node.ContainsPoint((VInt3)p)) { DrawDebugNode(node, 0.2f, Color.red); if (constraint == null || constraint.Suitable(node)) { return(node); } } else { DrawDebugNode(node, 0.0f, Color.blue); } } } else { DrawDebugRect(box.rect); //Search children if (tree[box.left].Contains(p)) { var result = SearchBoxInside(box.left, p, constraint); if (result != null) { return(result); } } if (tree[box.right].Contains(p)) { var result = SearchBoxInside(box.right, p, constraint); if (result != null) { return(result); } } } return(null); }
//Good Game //void SearchBoxClosest (int boxi, Vector3 p, ref float closestSqrDist, NNConstraint constraint, ref NNInfoInternal nnInfo) { void SearchBoxClosest(int boxi, VInt3 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]; //Good Game //Vector3 closest = node.ClosestPointOnNode(p); VInt3 closest = node.ClosestPointOnNode(p); float dist = (closest - p).sqrMagnitude; if (dist < closestSqrDist) { DrawDebugNode(node, 0.2f, Color.red); if (constraint == null || constraint.Suitable(node)) { // Update the NNInfo nnInfo.constrainedNode = node; nnInfo.constClampedPosition = closest; closestSqrDist = dist; } } else { DrawDebugNode(node, 0.0f, Color.blue); } } } else { DrawDebugRect(box.rect); 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); } } }
public void Insert(MeshNode node) { BBTreeBox box = new BBTreeBox(this, node); if (root == null) { root = box; return; } BBTreeBox c = root; while (true) { c.rect = ExpandToContain(c.rect, box.rect); if (c.node != null) { //Is Leaf c.c1 = box; BBTreeBox box2 = new BBTreeBox(this, c.node); //Console.WriteLine ("Inserted "+box.node+", rect "+box.rect.ToString ()); c.c2 = box2; c.node = null; //c.rect = c.rect. return; } else { float e1 = ExpansionRequired(c.c1.rect, box.rect); float e2 = ExpansionRequired(c.c2.rect, box.rect); //Choose the rect requiring the least expansion to contain box.rect if (e1 < e2) { c = c.c1; } else if (e2 < e1) { c = c.c2; } else { //Equal, Choose the one with the smallest area c = RectArea(c.c1.rect) < RectArea(c.c2.rect) ? c.c1 : c.c2; } } } }
private void SearchBox(int boxi, Vector3 p, NNConstraint constraint, ref NNInfo nnInfo) { BBTreeBox box = this.arr[boxi]; if (box.node != null) { if (box.node.ContainsPoint((VInt3)p)) { if (nnInfo.node == null) { nnInfo.node = box.node; } else { Vector3 position = (Vector3)box.node.position; Vector3 vector2 = (Vector3)nnInfo.node.position; if (Mathf.Abs((float)(position.y - p.y)) < Mathf.Abs((float)(vector2.y - p.y))) { nnInfo.node = box.node; } } if (constraint.Suitable(box.node)) { if (nnInfo.constrainedNode == null) { nnInfo.constrainedNode = box.node; } else { float introduced3 = Mathf.Abs((float)(box.node.position.y - p.y)); if (introduced3 < Mathf.Abs((float)(nnInfo.constrainedNode.position.y - p.y))) { nnInfo.constrainedNode = box.node; } } } } } else { if (RectContains(this.arr[box.left].rect, p)) { this.SearchBox(box.left, p, constraint, ref nnInfo); } if (RectContains(this.arr[box.right].rect, p)) { this.SearchBox(box.right, p, constraint, ref nnInfo); } } }
MeshNode SearchBoxInside(int boxi, Vector3 p, NNConstraint constraint) { BBTreeBox box = arr[boxi]; if (box.node != null) { if (box.node.ContainsPoint((Int3)p)) { DrawDebugNode(box.node, 0.2f, Color.red); if (constraint == null || constraint.Suitable(box.node)) { return(box.node); } } else { DrawDebugNode(box.node, 0.0f, Color.blue); } } else { DrawDebugRect(box.rect); //Search children MeshNode g; if (arr[box.left].Contains(p)) { g = SearchBoxInside(box.left, p, constraint); if (g != null) { return(g); } } if (arr[box.right].Contains(p)) { g = SearchBoxInside(box.right, p, constraint); if (g != null) { return(g); } } } return(null); }
void SearchBoxClosest(int boxi, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfo nnInfo) { BBTreeBox box = arr[boxi]; if (box.node != null) { //Leaf node if (NodeIntersectsCircle(box.node, p, closestDist)) { DrawDebugNode(box.node, 0.2f, Color.red); Vector3 closest = box.node.ClosestPointOnNode(p); if (constraint == null || constraint.Suitable(box.node)) { float dist = (closest - p).sqrMagnitude; //Update the NNInfo if (nnInfo.constrainedNode == null || dist < closestDist * closestDist) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = closest; closestDist = (float)Math.Sqrt(dist); } } } else { DrawDebugNode(box.node, 0.0f, Color.blue); } } else { DrawDebugRect(box.rect); //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); } } }
public NNInfo Query(Vector3 p, NNConstraint constraint) { BBTreeBox c = root; if (c == null) { return(null); } NNInfo nnInfo = new NNInfo(); SearchBox(c, p, constraint, ref nnInfo); nnInfo.UpdateInfo(); return(nnInfo); }
private void OnDrawGizmos(int boxi, int depth) { BBTreeBox box = this.arr[boxi]; Vector3 vector = new Vector3(box.rect.xMin, 0f, box.rect.yMin); Vector3 vector2 = new Vector3(box.rect.xMax, 0f, box.rect.yMax); Vector3 center = (Vector3)((vector + vector2) * 0.5f); Vector3 size = (Vector3)((vector2 - center) * 2f); center.y += depth * 0.2f; Gizmos.color = AstarMath.IntToColor(depth, 0.05f); Gizmos.DrawCube(center, size); if (box.node == null) { this.OnDrawGizmos(box.left, depth + 1); this.OnDrawGizmos(box.right, depth + 1); } }
void SearchBox(int boxi, Vector3 p, NNConstraint constraint, ref NNInfo nnInfo) //, int intendentLevel = 0) { { BBTreeBox box = arr[boxi]; if (box.node != null) { //Leaf node if (box.node.ContainsPoint((Int3)p)) { //Update the NNInfo if (nnInfo.node == null) { nnInfo.node = box.node; } else if (Mathf.Abs(((Vector3)box.node.position).y - p.y) < Mathf.Abs(((Vector3)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); } }
private void OnDrawGizmos(int boxi, int depth) { BBTreeBox box = this.arr[boxi]; Vector3 vector = (Vector3) new Int3(box.rect.xmin, 0, box.rect.ymin); Vector3 vector2 = (Vector3) new Int3(box.rect.xmax, 0, box.rect.ymax); Vector3 center = (Vector3)((vector + vector2) * 0.5f); Vector3 size = (Vector3)((vector2 - center) * 2f); size = new Vector3(size.x, 1f, size.z); center.y += depth * 2; Gizmos.color = AstarMath.IntToColor(depth, 1f); Gizmos.DrawCube(center, size); if (box.node == null) { this.OnDrawGizmos(box.left, depth + 1); this.OnDrawGizmos(box.right, depth + 1); } }
public void OnDrawGizmos(BBTreeBox box) { if (box == null) { return; } Vector3 min = new Vector3(box.rect.xMin, 0, box.rect.yMin); Vector3 max = new Vector3(box.rect.xMax, 0, box.rect.yMax); Vector3 center = (min + max) * 0.5F; Vector3 size = (max - center) * 2; Gizmos.DrawCube(center, size); OnDrawGizmos(box.c1); OnDrawGizmos(box.c2); }
public void Insert(MeshNode node) { BBTreeBox box2; int index = this.GetBox(node); if (index == 0) { return; } BBTreeBox box = this.arr[index]; int left = 0; Label_0023: box2 = this.arr[left]; box2.rect = ExpandToContain(box2.rect, box.rect); if (box2.node != null) { box2.left = index; int num3 = this.GetBox(box2.node); box2.right = num3; box2.node = null; this.arr[left] = box2; } else { this.arr[left] = box2; int num4 = ExpansionRequired(this.arr[box2.left].rect, box.rect); int num5 = ExpansionRequired(this.arr[box2.right].rect, box.rect); if (num4 < num5) { left = box2.left; } else if (num5 < num4) { left = box2.right; } else { left = (RectArea(this.arr[box2.left].rect) >= RectArea(this.arr[box2.right].rect)) ? box2.right : box2.left; } goto Label_0023; } }
public void SearchBox(BBTreeBox box, Vector3 p, NNConstraint constraint, ref NNInfo nnInfo) //, int intendentLevel = 0) { { if (box.node != null) { //Leaf node if (NavMeshGraph.ContainsPoint(box.node, p, graph.vertices)) { //Update the NNInfo if (nnInfo.node == null) { nnInfo.node = box.node; } else if (Mathf.Abs(((Vector3)box.node.position).y - p.y) < Mathf.Abs(((Vector3)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 (RectContains(box.c1.rect, p)) { SearchBox(box.c1, p, constraint, ref nnInfo); } if (RectContains(box.c2.rect, p)) { SearchBox(box.c2, p, constraint, ref nnInfo); } }
public void Insert(MeshNode node) { BBTreeBox box = new BBTreeBox (this,node); if (root == null) { root = box; return; } BBTreeBox c = root; while (true) { c.rect = ExpandToContain (c.rect,box.rect); if (c.node != null) { //Is Leaf c.c1 = box; BBTreeBox box2 = new BBTreeBox (this,c.node); //Console.WriteLine ("Inserted "+box.node+", rect "+box.rect.ToString ()); c.c2 = box2; c.node = null; //c.rect = c.rect. return; } else { float e1 = ExpansionRequired (c.c1.rect,box.rect); float e2 = ExpansionRequired (c.c2.rect,box.rect); //Choose the rect requiring the least expansion to contain box.rect if (e1 < e2) { c = c.c1; } else if (e2 < e1) { c = c.c2; } else { //Equal, Choose the one with the smallest area c = RectArea (c.c1.rect) < RectArea (c.c2.rect) ? c.c1 : c.c2; } } } }
public MeshNode SearchBoxInside (BBTreeBox box, Vector3 p, NNConstraint constraint) { if (box.node != null) { if (box.node.ContainsPoint ((Int3)p)) { //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 if (constraint == null || constraint.Suitable (box.node)) { return box.node; } } 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 MeshNode g; if (box.c1.rect.Contains (new Vector2(p.x,p.z))) { g = SearchBoxInside (box.c1,p, constraint); if (g != null) return g; } if (box.c2.rect.Contains (new Vector2(p.x,p.z))) { g = SearchBoxInside (box.c2, p, constraint); if (g != null) return g; } } return null; }
public void TestIntersections(BBTreeBox box, Vector3 p, float radius) { if (box == null) { return; } RectIntersectsCircle (box.rect,p,radius); TestIntersections (box.c1,p,radius); TestIntersections (box.c2,p,radius); }
public void SearchBoxCircle(BBTreeBox box, Vector3 p, float radius, NNConstraint constraint, ref NNInfo nnInfo) { //, int intendentLevel = 0) { if (box.node != null) { //Leaf node if (NodeIntersectsCircle (box.node,p,radius)) { //Update the NNInfo #if DEBUG Debug.DrawLine (graph.vertices[box.node[0]],graph.vertices[box.node[1]],Color.red); Debug.DrawLine (graph.vertices[box.node[1]],graph.vertices[box.node[2]],Color.red); Debug.DrawLine (graph.vertices[box.node[2]],graph.vertices[box.node[0]],Color.red); #endif Vector3 closest = NavMeshGraph.ClosestPointOnNode (box.node,graph.vertices,p); float 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.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; } } } return; } #if DEBUG 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 (box.c1.rect,p,radius)) { SearchBoxCircle (box.c1,p, radius, constraint, ref nnInfo); } if (RectIntersectsCircle (box.c2.rect,p,radius)) { SearchBoxCircle (box.c2,p, radius, constraint, ref nnInfo); } }
public void SearchBox(BBTreeBox box, Vector3 p, NNConstraint constraint, ref NNInfo nnInfo) { //, int intendentLevel = 0) { if (box.node != null) { //Leaf node if (NavMeshGraph.ContainsPoint (box.node,p,graph.vertices)) { //Update the NNInfo if (nnInfo.node == null) { nnInfo.node = box.node; } else if (Mathf.Abs(((Vector3)box.node.position).y - p.y) < Mathf.Abs (((Vector3)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 (RectContains (box.c1.rect,p)) { SearchBox (box.c1,p, constraint, ref nnInfo); } if (RectContains (box.c2.rect,p)) { SearchBox (box.c2,p, constraint, ref nnInfo); } }
public void OnDrawGizmos(BBTreeBox box) { if (box == null) { return; } Vector3 min = new Vector3 (box.rect.xMin,0,box.rect.yMin); Vector3 max = new Vector3 (box.rect.xMax,0,box.rect.yMax); Vector3 center = (min+max)*0.5F; Vector3 size = (max-center)*2; Gizmos.DrawCube (center,size); OnDrawGizmos (box.c1); OnDrawGizmos (box.c2); }
public void SearchBoxClosestXZ (BBTreeBox box, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfo nnInfo) { 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 Vector3 closest = box.node.ClosestPointOnNodeXZ (p);//NavMeshGraph.ClosestPointOnNode (box.node,graph.vertices,p); // XZ distance float dist = (closest.x-p.x)*(closest.x-p.x)+(closest.z-p.z)*(closest.z-p.z); if (constraint == null || constraint.Suitable (box.node)) { if (nnInfo.constrainedNode == null) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = closest; closestDist = (float)System.Math.Sqrt (dist); } else if (dist < closestDist*closestDist) { nnInfo.constrainedNode = box.node; nnInfo.constClampedPosition = closest; closestDist = (float)System.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 (box.c1.rect,p,closestDist)) { SearchBoxClosestXZ (box.c1,p, ref closestDist, constraint, ref nnInfo); } if (RectIntersectsCircle (box.c2.rect,p,closestDist)) { SearchBoxClosestXZ (box.c2,p, ref closestDist, constraint, ref nnInfo); } } }