B2DynamicTree Validate

ByteArena · Feb 9, 2020 · add4467 · add4467
1 parent 806cbf3
commit add4467
Showing 1 changed file with 14 additions and 0 deletions.
diff --git a/CollisionB2DynamicTree.go b/CollisionB2DynamicTree.go
@@ -734,6 +734,20 @@ func (tree B2DynamicTree) ValidateMetrics(index int) {
 }
 
 func (tree B2DynamicTree) Validate() {
+	tree.ValidateStructure(tree.M_root)
+	tree.ValidateMetrics(tree.M_root)
+
+	freeCount := 0
+	freeIndex := tree.M_freeList
+	for freeIndex != B2_nullNode {
+		B2Assert(0 <= freeIndex && freeIndex < tree.M_nodeCapacity)
+		freeIndex = tree.M_nodes[freeIndex].Next
+		freeCount++
+	}
+
+	B2Assert(tree.GetHeight() == tree.ComputeTotalHeight())
+
+	B2Assert(tree.M_nodeCount+freeCount == tree.M_nodeCapacity)
 }
 
 func (tree B2DynamicTree) GetMaxBalance() int {