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