Small changes in collision detection

src/collision/broadphase/DynamicAABBTree.cpp

@@ -406,7 +406,7 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
 }
 
 // Balance the sub-tree of a given node using left or right rotations.
-/// The rotation schemes are described in in the book "Introduction to Game Physics
+/// The rotation schemes are described in the book "Introduction to Game Physics
 /// with Box2D" by Ian Parberry. This method returns the new root node ID.
 int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
 
@@ -756,4 +756,27 @@ void DynamicAABBTree::checkNode(int nodeID) const {
     }
 }
 
+// Compute the height of the tree
+int DynamicAABBTree::computeHeight() {
+   return computeHeight(mRootNodeID);
+}
+
+// Compute the height of a given node in the tree
+int DynamicAABBTree::computeHeight(int nodeID) {
+    assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
+    TreeNode* node = mNodes + nodeID;
+
+    // If the node is a leaf, its height is zero
+    if (node->isLeaf()) {
+        return 0;
+    }
+
+    // Compute the height of the left and right sub-tree
+    int leftHeight = computeHeight(node->leftChildID);
+    int rightHeight = computeHeight(node->rightChildID);
+
+    // Return the height of the node
+    return 1 + std::max(leftHeight, rightHeight);
+}
+
 #endif

src/collision/broadphase/DynamicAABBTree.h

@@ -129,6 +129,9 @@ class DynamicAABBTree {
         /// Balance the sub-tree of a given node using left or right rotations.
         int balanceSubTreeAtNode(int nodeID);
 
+        /// Compute the height of a given node in the tree
+        int computeHeight(int nodeID);
+
 #ifndef NDEBUG
 
         /// Check if the tree structure is valid (for debugging purpose)
@@ -170,6 +173,9 @@ class DynamicAABBTree {
         /// Ray casting method
         void raycast(const Ray& ray, RaycastTest& raycastTest,
                      unsigned short raycastWithCategoryMaskBits) const;
+
+        /// Compute the height of the tree
+        int computeHeight();
 };
 
 // Return true if the node is a leaf of the tree

src/collision/narrowphase/EPA/EPAAlgorithm.cpp

@@ -27,7 +27,6 @@
 #include "EPAAlgorithm.h"
 #include "collision/narrowphase//GJK/GJKAlgorithm.h"
 #include "TrianglesStore.h"
-#include <iostream> // TODO : DELETE THIS
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;