/// <summary>
/// Query an AABB for overlapping proxies.
/// </summary>
/// <param name="aabb"></param>
public void Query(ITreeQueryCallback callback, AABB aabb)
{
Stack <int> stack = new Stack <int>();
stack.Push(rootIndex);
while (stack.Count > 0)
{
int nodeId = stack.Pop();
if (nodeId == nullNode)
{
continue;
}
DTNode node = nodes[nodeId];
if (node.aabb.Overlaps(aabb))
{
if (node.IsLeaf())
{
bool proceed = callback.QueryCallback(nodeId);
if (proceed == false)
{
return;
}
}
else
{
stack.Push(node.leftChildIndex);
stack.Push(node.rightChildIndex);
}
}
}
}
void OnDrawGizmosSelected()
{
if (!debug)
{
return;
}
DynamicTree dt = physicsScene.dynamicTree;
if (dt == null)
{
return;
}
for (int i = 0; i < dt.nodeCount; i++)
{
DTNode node = dt.nodes[i];
if (i == dt.rootIndex)
{
Handles.color = Color.white;
}
else if (node.IsLeaf())
{
Handles.color = Color.green;
}
else
{
Handles.color = Color.yellow;
}
Handles.DrawLine((Vector3)(node.aabb.min), (Vector3)(new FixVec3(node.aabb.max.x, node.aabb.min.y, 0)));
Handles.DrawLine((Vector3)(new FixVec3(node.aabb.max.x, node.aabb.min.y, 0)), (Vector3)(node.aabb.max));
Handles.DrawLine((Vector3)(node.aabb.max), (Vector3)(new FixVec3(node.aabb.min.x, node.aabb.max.y, 0)));
Handles.DrawLine((Vector3)(new FixVec3(node.aabb.min.x, node.aabb.max.y, 0)), (Vector3)(node.aabb.min));
}
}
/// <summary>
/// Performs a left/right rotation if node A is imbalanced.
/// </summary>
/// <param name="indexA">The index of node A.</param>
/// <returns>The new root index.</returns>
private int Rotate(int indexA)
{
DTNode node = nodes[indexA];
if (node.IsLeaf() || node.height < 2)
{
return(indexA);
}
int indexLeft = node.leftChildIndex;
int indexRight = node.rightChildIndex;
DTNode leftChild = nodes[indexLeft];
DTNode rightChild = nodes[indexRight];
int balance = rightChild.height - leftChild.height;
// Rotate right branch up.
if (balance > 1)
{
int iF = rightChild.leftChildIndex;
int iG = rightChild.rightChildIndex;
DTNode F = nodes[iF];
DTNode G = nodes[iG];
// Swap node and it's right child.
rightChild.leftChildIndex = indexA;
rightChild.parentIndex = node.parentIndex;
node.parentIndex = indexRight;
// The node's old parent should point to its right child.
if (rightChild.parentIndex != nullNode)
{
if (nodes[rightChild.parentIndex].leftChildIndex == indexA)
{
nodes[rightChild.parentIndex].leftChildIndex = indexRight;
}
else
{
nodes[rightChild.parentIndex].rightChildIndex = indexRight;
}
}
else
{
rootIndex = indexRight;
}
// Rotate
if (F.height > G.height)
{
rightChild.rightChildIndex = iF;
node.rightChildIndex = iG;
G.parentIndex = indexA;
node.aabb = AABB.Union(leftChild.aabb, G.aabb);
rightChild.aabb = AABB.Union(node.aabb, F.aabb);
node.height = 1 + Mathf.Max(leftChild.height, G.height);
rightChild.height = 1 + Mathf.Max(node.height, F.height);
}
else
{
rightChild.rightChildIndex = iG;
node.rightChildIndex = iF;
F.parentIndex = indexA;
node.aabb = AABB.Union(leftChild.aabb, F.aabb);
rightChild.aabb = AABB.Union(node.aabb, G.aabb);
node.height = 1 + Mathf.Max(leftChild.height, F.height);
rightChild.height = 1 + Mathf.Max(node.height, G.height);
}
return(indexRight);
}
// Rotate left branch up
if (balance < -1)
{
int iD = leftChild.leftChildIndex;
int iE = leftChild.rightChildIndex;
DTNode D = nodes[iD];
DTNode E = nodes[iE];
// Swap node and its left child.
leftChild.leftChildIndex = indexA;
leftChild.parentIndex = node.parentIndex;
node.parentIndex = indexLeft;
// A's old parent should point to B
if (leftChild.parentIndex != nullNode)
{
if (nodes[leftChild.parentIndex].leftChildIndex == indexA)
{
nodes[leftChild.parentIndex].leftChildIndex = indexLeft;
}
else
{
nodes[leftChild.parentIndex].rightChildIndex = indexLeft;
}
}
else
{
rootIndex = indexLeft;
}
// Rotate
if (D.height > E.height)
{
leftChild.rightChildIndex = iD;
node.leftChildIndex = iE;
E.parentIndex = indexA;
node.aabb = AABB.Union(rightChild.aabb, E.aabb);
leftChild.aabb = AABB.Union(node.aabb, D.aabb);
node.height = 1 + Mathf.Max(rightChild.height, E.height);
leftChild.height = 1 + Mathf.Max(node.height, D.height);
}
else
{
leftChild.rightChildIndex = iE;
node.leftChildIndex = iD;
D.parentIndex = indexA;
node.aabb = AABB.Union(rightChild.aabb, D.aabb);
leftChild.aabb = AABB.Union(node.aabb, E.aabb);
node.height = 1 + Mathf.Max(rightChild.height, D.height);
leftChild.height = 1 + Mathf.Max(node.height, E.height);
}
return(indexLeft);
}
return(indexA);
}
