public b2DynamicTree()
{
m_root = b2TreeNode.b2_nullNode;
m_nodeCapacity = 16;
m_nodeCount = 0;
m_nodes = new b2TreeNode[m_nodeCapacity];
// Build a linked list for the free list.
for (int i = 0; i < m_nodeCapacity - 1; ++i)
{
if (m_nodes[i] == null)
{
m_nodes[i] = new b2TreeNode();
}
m_nodes[i].parentOrNext = i + 1;
m_nodes[i].height = -1;
}
if (m_nodes[m_nodeCapacity - 1] == null)
{
m_nodes[m_nodeCapacity - 1] = new b2TreeNode();
}
m_nodes[m_nodeCapacity - 1].parentOrNext = b2TreeNode.b2_nullNode;
m_nodes[m_nodeCapacity - 1].height = -1;
m_freeList = 0;
// m_path = 0;
m_insertionCount = 0;
}
public b2DynamicTree()
{
m_root = b2TreeNode.b2_nullNode;
m_nodeCapacity = 16;
m_nodeCount = 0;
m_nodes = new b2TreeNode[m_nodeCapacity];
// Build a linked list for the free list.
for (int i = 0; i < m_nodeCapacity - 1; ++i)
{
if (m_nodes[i] == null)
m_nodes[i] = new b2TreeNode();
m_nodes[i].parentOrNext = i + 1;
m_nodes[i].height = -1;
}
if (m_nodes[m_nodeCapacity - 1] == null)
m_nodes[m_nodeCapacity - 1] = new b2TreeNode();
m_nodes[m_nodeCapacity - 1].parentOrNext = b2TreeNode.b2_nullNode;
m_nodes[m_nodeCapacity - 1].height = -1;
m_freeList = 0;
// m_path = 0;
m_insertionCount = 0;
}
public void ValidateStructure(int index)
{
if (index == b2TreeNode.b2_nullNode)
{
return;
}
if (index == m_root)
{
Debug.Assert(m_nodes[index].parentOrNext == b2TreeNode.b2_nullNode);
}
b2TreeNode node = m_nodes[index];
int child1 = node.child1;
int child2 = node.child2;
if (node.IsLeaf())
{
Debug.Assert(child1 == b2TreeNode.b2_nullNode);
Debug.Assert(child2 == b2TreeNode.b2_nullNode);
Debug.Assert(node.height == 0);
return;
}
Debug.Assert(0 <= child1 && child1 < m_nodeCapacity);
Debug.Assert(0 <= child2 && child2 < m_nodeCapacity);
Debug.Assert(m_nodes[child1].parentOrNext == index);
Debug.Assert(m_nodes[child2].parentOrNext == index);
ValidateStructure(child1);
ValidateStructure(child2);
}
//
public float GetAreaRatio()
{
if (m_root == b2TreeNode.b2_nullNode)
{
return(0.0f);
}
b2TreeNode root = m_nodes[m_root];
float rootArea = root.aabb.Perimeter;
float totalArea = 0.0f;
for (int i = 0; i < m_nodeCapacity; ++i)
{
b2TreeNode node = m_nodes[i];
if (node.height < 0)
{
// Free node in pool
continue;
}
totalArea += node.aabb.Perimeter;
}
return(totalArea / rootArea);
}
// Allocate a node from the pool. Grow the pool if necessary.
public int AllocateNode()
{
// Expand the node pool as needed.
if (m_freeList == b2TreeNode.b2_nullNode)
{
Debug.Assert(m_nodeCount == m_nodeCapacity);
// The free list is empty. Rebuild a bigger pool.
b2TreeNode[] oldNodes = m_nodes;
m_nodeCapacity *= 2;
m_nodes = new b2TreeNode[m_nodeCapacity];
// initialize new b2TreeNode
oldNodes.CopyTo(m_nodes, 0);
// Build a linked list for the free list. The parent
// pointer becomes the "next" pointer.
for (int i = m_nodeCount; i < m_nodeCapacity - 1; ++i)
{
if (m_nodes[i] == null)
{
m_nodes[i] = new b2TreeNode();
}
m_nodes[i].parentOrNext = i + 1;
m_nodes[i].height = -1;
}
if (m_nodes[m_nodeCapacity - 1] == null)
{
m_nodes[m_nodeCapacity - 1] = new b2TreeNode();
}
m_nodes[m_nodeCapacity - 1].parentOrNext = b2TreeNode.b2_nullNode;
m_nodes[m_nodeCapacity - 1].height = -1;
m_freeList = m_nodeCount;
}
// Peel a node off the free list.
int nodeId = m_freeList;
m_freeList = m_nodes[nodeId].parentOrNext;
m_nodes[nodeId].parentOrNext = b2TreeNode.b2_nullNode;
m_nodes[nodeId].child1 = b2TreeNode.b2_nullNode;
m_nodes[nodeId].child2 = b2TreeNode.b2_nullNode;
m_nodes[nodeId].height = 0;
m_nodes[nodeId].userData = null;
++m_nodeCount;
return(nodeId);
}
// Compute the height of a sub-tree.
public int ComputeHeight(int nodeId)
{
Debug.Assert(0 <= nodeId && nodeId < m_nodeCapacity);
b2TreeNode node = m_nodes[nodeId];
if (node.IsLeaf())
{
return(0);
}
int height1 = ComputeHeight(node.child1);
int height2 = ComputeHeight(node.child2);
return(1 + Math.Max(height1, height2));
}
public void Query(Ib2QueryCallback w, b2AABB aabb)
{
int stackCount = 0;
var stack = _stack;
var nodes = m_nodes;
stack[stackCount++] = m_root;
while (stackCount > 0)
{
int nodeId = stack[--stackCount];
if (nodeId == b2TreeNode.b2_nullNode)
{
continue;
}
b2TreeNode node = nodes[nodeId];
if (b2Collision.b2TestOverlap(ref node.aabb, ref aabb))
{
if (node.child1 == b2TreeNode.b2_nullNode)
{
bool proceed = w.QueryCallback(nodeId);
if (proceed == false)
{
return;
}
}
else
{
//if (node.child1 != b2TreeNode.b2_nullNode)
stack[stackCount++] = node.child1;
if (node.child2 != b2TreeNode.b2_nullNode)
{
stack[stackCount++] = node.child2;
}
}
}
}
}
public void Query(Ib2QueryCallback w, b2AABB aabb)
{
Stack <int> stack = new Stack <int>();
stack.Push(m_root);
while (stack.Count > 0)
{
int nodeId = stack.Pop();
if (nodeId == b2TreeNode.b2_nullNode)
{
continue;
}
b2TreeNode node = m_nodes[nodeId];
if (b2Collision.b2TestOverlap(ref node.aabb, ref aabb))
{
if (node.IsLeaf())
{
bool proceed = w.QueryCallback(nodeId);
if (proceed == false)
{
return;
}
}
else
{
if (node.child1 != b2TreeNode.b2_nullNode)
{
stack.Push(node.child1);
}
if (node.child2 != b2TreeNode.b2_nullNode)
{
stack.Push(node.child2);
}
}
}
}
}
public void ValidateMetrics(int index)
{
if (index == b2TreeNode.b2_nullNode)
{
return;
}
b2TreeNode node = m_nodes[index];
int child1 = node.child1;
int child2 = node.child2;
if (node.IsLeaf())
{
Debug.Assert(child1 == b2TreeNode.b2_nullNode);
Debug.Assert(child2 == b2TreeNode.b2_nullNode);
Debug.Assert(node.height == 0);
return;
}
Debug.Assert(0 <= child1 && child1 < m_nodeCapacity);
Debug.Assert(0 <= child2 && child2 < m_nodeCapacity);
int height1 = m_nodes[child1].height;
int height2 = m_nodes[child2].height;
int height;
height = 1 + Math.Max(height1, height2);
Debug.Assert(node.height == height);
b2AABB aabb = b2AABB.Default;
aabb.Combine(ref m_nodes[child1].aabb, ref m_nodes[child2].aabb);
Debug.Assert(aabb.LowerBound == node.aabb.LowerBound);
Debug.Assert(aabb.UpperBound == node.aabb.UpperBound);
ValidateMetrics(child1);
ValidateMetrics(child2);
}
public int GetMaxBalance()
{
int maxBalance = 0;
for (int i = 0; i < m_nodeCapacity; ++i)
{
b2TreeNode node = m_nodes[i];
if (node.height <= 1)
{
continue;
}
Debug.Assert(node.IsLeaf() == false);
int child1 = node.child1;
int child2 = node.child2;
int balance = Math.Abs(m_nodes[child2].height - m_nodes[child1].height);
maxBalance = Math.Max(maxBalance, balance);
}
return(maxBalance);
}
// Allocate a node from the pool. Grow the pool if necessary.
public int AllocateNode()
{
// Expand the node pool as needed.
if (m_freeList == b2TreeNode.b2_nullNode)
{
Debug.Assert(m_nodeCount == m_nodeCapacity);
// The free list is empty. Rebuild a bigger pool.
b2TreeNode[] oldNodes = m_nodes;
m_nodeCapacity *= 2;
m_nodes = new b2TreeNode[m_nodeCapacity];
// initialize new b2TreeNode
oldNodes.CopyTo(m_nodes, 0);
// Build a linked list for the free list. The parent
// pointer becomes the "next" pointer.
for (int i = m_nodeCount; i < m_nodeCapacity - 1; ++i)
{
if (m_nodes[i] == null)
m_nodes[i] = new b2TreeNode();
m_nodes[i].parentOrNext = i + 1;
m_nodes[i].height = -1;
}
if (m_nodes[m_nodeCapacity - 1] == null)
m_nodes[m_nodeCapacity - 1] = new b2TreeNode();
m_nodes[m_nodeCapacity - 1].parentOrNext = b2TreeNode.b2_nullNode;
m_nodes[m_nodeCapacity - 1].height = -1;
m_freeList = m_nodeCount;
}
// Peel a node off the free list.
int nodeId = m_freeList;
m_freeList = m_nodes[nodeId].parentOrNext;
m_nodes[nodeId].parentOrNext = b2TreeNode.b2_nullNode;
m_nodes[nodeId].child1 = b2TreeNode.b2_nullNode;
m_nodes[nodeId].child2 = b2TreeNode.b2_nullNode;
m_nodes[nodeId].height = 0;
m_nodes[nodeId].userData = null;
++m_nodeCount;
return nodeId;
}
public void RayCast(b2WorldRayCastWrapper callback, b2RayCastInput input)
{
b2Vec2 p1 = input.p1;
b2Vec2 p2 = input.p2;
b2Vec2 r = p2 - p1;
Debug.Assert(r.LengthSquared > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
b2Vec2 v = r.NegUnitCross(); // b2Math.b2Cross(1.0f, r);
b2Vec2 abs_v = b2Math.b2Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.maxFraction;
// Build a bounding box for the segment.
b2AABB segmentAABB = b2AABB.Default;
{
b2Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.Set(b2Math.b2Min(p1, t), b2Math.b2Max(p1, t));
}
Stack <int> stack = new Stack <int>();
stack.Push(m_root);
while (stack.Count > 0)
{
int nodeId = stack.Pop();
if (nodeId == b2TreeNode.b2_nullNode)
{
continue;
}
b2TreeNode node = m_nodes[nodeId];
if (b2Collision.b2TestOverlap(ref node.aabb, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
b2Vec2 c = node.aabb.Center;
b2Vec2 h = node.aabb.Extents;
float separation = b2Math.b2Abs(b2Math.b2Dot(v, p1 - c)) - b2Math.b2Dot(abs_v, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
b2RayCastInput subInput = new b2RayCastInput();
subInput.p1 = input.p1;
subInput.p2 = input.p2;
subInput.maxFraction = maxFraction;
float value = callback.RayCastCallback(subInput, nodeId);
if (value == 0.0f)
{
// The client has terminated the ray cast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
b2Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.Set(b2Math.b2Min(p1, t), b2Math.b2Max(p1, t));
}
}
else
{
stack.Push(node.child1);
stack.Push(node.child2);
}
}
}
public void RebuildBottomUp()
{
int[] nodes = new int[m_nodeCount];
int count = 0;
// Build array of leaves. Free the rest.
for (int i = 0; i < m_nodeCapacity; ++i)
{
if (m_nodes[i].height < 0)
{
// free node in pool
continue;
}
if (m_nodes[i].IsLeaf())
{
m_nodes[i].parentOrNext = b2TreeNode.b2_nullNode;
nodes[count] = i;
++count;
}
else
{
FreeNode(i);
}
}
while (count > 1)
{
float minCost = b2Settings.b2_maxFloat;
int iMin = -1, jMin = -1;
for (int i = 0; i < count; ++i)
{
b2AABB aabbi = m_nodes[nodes[i]].aabb;
for (int j = i + 1; j < count; ++j)
{
b2AABB aabbj = m_nodes[nodes[j]].aabb;
b2AABB b = b2AABB.Default;
b.Combine(ref aabbi, ref aabbj);
float cost = b.Perimeter;
if (cost < minCost)
{
iMin = i;
jMin = j;
minCost = cost;
}
}
}
int index1 = nodes[iMin];
int index2 = nodes[jMin];
b2TreeNode child1 = m_nodes[index1];
b2TreeNode child2 = m_nodes[index2];
int parentIndex = AllocateNode();
b2TreeNode parent = m_nodes[parentIndex];
parent.child1 = index1;
parent.child2 = index2;
parent.height = 1 + Math.Max(child1.height, child2.height);
parent.aabb.Combine(ref child1.aabb, ref child2.aabb);
parent.parentOrNext = b2TreeNode.b2_nullNode;
child1.parentOrNext = parentIndex;
child2.parentOrNext = parentIndex;
nodes[jMin] = nodes[count - 1];
nodes[iMin] = parentIndex;
--count;
}
m_root = nodes[0];
Validate();
}
// Perform a left or right rotation if node A is imbalanced.
// Returns the new root index.
int Balance(int iA)
{
Debug.Assert(iA != b2TreeNode.b2_nullNode);
b2TreeNode A = m_nodes[iA];
if (A.IsLeaf() || A.height < 2)
{
return(iA);
}
int iB = A.child1;
int iC = A.child2;
Debug.Assert(0 <= iB && iB < m_nodeCapacity);
Debug.Assert(0 <= iC && iC < m_nodeCapacity);
b2TreeNode B = m_nodes[iB];
b2TreeNode C = m_nodes[iC];
int balance = C.height - B.height;
// Rotate C up
if (balance > 1)
{
int iF = C.child1;
int iG = C.child2;
b2TreeNode F = m_nodes[iF];
b2TreeNode G = m_nodes[iG];
Debug.Assert(0 <= iF && iF < m_nodeCapacity);
Debug.Assert(0 <= iG && iG < m_nodeCapacity);
// Swap A and C
C.child1 = iA;
C.parentOrNext = A.parentOrNext;
A.parentOrNext = iC;
// A's old parent should point to C
if (C.parentOrNext != b2TreeNode.b2_nullNode)
{
if (m_nodes[C.parentOrNext].child1 == iA)
{
m_nodes[C.parentOrNext].child1 = iC;
}
else
{
Debug.Assert(m_nodes[C.parentOrNext].child2 == iA);
m_nodes[C.parentOrNext].child2 = iC;
}
}
else
{
m_root = iC;
}
// Rotate
if (F.height > G.height)
{
C.child2 = iF;
A.child2 = iG;
G.parentOrNext = iA;
A.aabb.Combine(ref B.aabb, ref G.aabb);
C.aabb.Combine(ref A.aabb, ref F.aabb);
A.height = 1 + Math.Max(B.height, G.height);
C.height = 1 + Math.Max(A.height, F.height);
}
else
{
C.child2 = iG;
A.child2 = iF;
F.parentOrNext = iA;
A.aabb.Combine(ref B.aabb, ref F.aabb);
C.aabb.Combine(ref A.aabb, ref G.aabb);
A.height = 1 + Math.Max(B.height, F.height);
C.height = 1 + Math.Max(A.height, G.height);
}
return(iC);
}
// Rotate B up
if (balance < -1)
{
int iD = B.child1;
int iE = B.child2;
b2TreeNode D = m_nodes[iD];
b2TreeNode E = m_nodes[iE];
Debug.Assert(0 <= iD && iD < m_nodeCapacity);
Debug.Assert(0 <= iE && iE < m_nodeCapacity);
// Swap A and B
B.child1 = iA;
B.parentOrNext = A.parentOrNext;
A.parentOrNext = iB;
// A's old parent should point to B
if (B.parentOrNext != b2TreeNode.b2_nullNode)
{
if (m_nodes[B.parentOrNext].child1 == iA)
{
m_nodes[B.parentOrNext].child1 = iB;
}
else
{
Debug.Assert(m_nodes[B.parentOrNext].child2 == iA);
m_nodes[B.parentOrNext].child2 = iB;
}
}
else
{
m_root = iB;
}
// Rotate
if (D.height > E.height)
{
B.child2 = iD;
A.child1 = iE;
E.parentOrNext = iA;
A.aabb.Combine(ref C.aabb, ref E.aabb);
B.aabb.Combine(ref A.aabb, ref D.aabb);
A.height = 1 + Math.Max(C.height, E.height);
B.height = 1 + Math.Max(A.height, D.height);
}
else
{
B.child2 = iE;
A.child1 = iD;
D.parentOrNext = iA;
A.aabb.Combine(ref C.aabb, ref D.aabb);
B.aabb.Combine(ref A.aabb, ref E.aabb);
A.height = 1 + Math.Max(C.height, D.height);
B.height = 1 + Math.Max(A.height, E.height);
}
return(iB);
}
return(iA);
}
