private void ValidateMetrics(int index)
{
if (index == TreeNode.NULL_NODE)
{
return;
}
TreeNode node = m_nodes[index];
int child1 = node.Child1;
int child2 = node.Child2;
if (node.Leaf)
{
Debug.Assert(child1 == TreeNode.NULL_NODE);
Debug.Assert(child2 == TreeNode.NULL_NODE);
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 = 1 + MathUtils.Max(height1, height2);
Debug.Assert(node.Height == height);
var aabb = new AABB();
aabb.Combine(m_nodes[child1].AABB, m_nodes[child2].AABB);
Debug.Assert(aabb.LowerBound.Equals(node.AABB.LowerBound));
Debug.Assert(aabb.UpperBound.Equals(node.AABB.UpperBound));
ValidateMetrics(child1);
ValidateMetrics(child2);
}