From c057e88983eb1beaa8e3116170750af18bc2e01d Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 27 Jan 2015 22:40:31 +0100
Subject: [PATCH] Small changes in collision detection

---
 src/collision/broadphase/DynamicAABBTree.cpp  | 25 ++++++++++++++++++-
 src/collision/broadphase/DynamicAABBTree.h    |  6 +++++
 .../narrowphase/EPA/EPAAlgorithm.cpp          |  1 -
 3 files changed, 30 insertions(+), 2 deletions(-)

diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp
index ede50bf6..68187ea6 100644
--- a/src/collision/broadphase/DynamicAABBTree.cpp
+++ b/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
diff --git a/src/collision/broadphase/DynamicAABBTree.h b/src/collision/broadphase/DynamicAABBTree.h
index 332493e3..26895c8d 100644
--- a/src/collision/broadphase/DynamicAABBTree.h
+++ b/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
diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp
index 1f0bcc9a..c7849cce 100644
--- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp
+++ b/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;