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);
}
}
int GetBox ( IntRect rect ) {
if ( count >= arr.Length ) EnsureCapacity ( count+1 );
arr[count] = new BBTreeBox ( rect );
count++;
return count-1;
}
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;
}
}
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((Int3)p))
{
#if !SERVER
DrawDebugNode(node, 0.2f, UnityEngine.Color.red);
#endif
if (constraint == null || constraint.Suitable(node))
{
return(node);
}
}
else
{
#if !SERVER
DrawDebugNode(node, 0.0f, UnityEngine.Color.blue);
#endif
}
}
}
else
{
#if !SERVER
DrawDebugRect(box.rect);
#endif
//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);
}
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);
}
}
}
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);
}
}
}
int GetBox(IntRect rect)
{
if (count >= arr.Length)
{
EnsureCapacity(count + 1);
}
arr[count] = new BBTreeBox(rect);
count++;
return(count - 1);
}
int GetBox(NavMeshNode node, IntRect bounds)
{
if (count >= arr.Length)
{
EnsureCapacity(count + 1);
}
arr[count] = new BBTreeBox(node, bounds);
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;
}
}
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);
}
}
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);
}
}
NavMeshNode SearchBoxInside(int boxi, Int3 p, NNConstraint constraint)
{
BBTreeBox box = arr[boxi];
if (box.node != null)
{
if (box.node.ContainsPoint(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
NavMeshNode 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, 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);
}
}
}
