Add the DynamicAABBTree class

CMakeLists.txt

@@ -49,6 +49,8 @@ SET (REACTPHYSICS3D_SOURCES
     "src/collision/broadphase/PairManager.cpp"
     "src/collision/broadphase/SweepAndPruneAlgorithm.h"
     "src/collision/broadphase/SweepAndPruneAlgorithm.cpp"
+    "src/collision/broadphase/DynamicAABBTree.h"
+    "src/collision/broadphase/DynamicAABBTree.cpp"
     "src/collision/narrowphase/EPA/EdgeEPA.h"
     "src/collision/narrowphase/EPA/EdgeEPA.cpp"
     "src/collision/narrowphase/EPA/EPAAlgorithm.h"

src/collision/broadphase/BroadPhaseAlgorithm.h

@@ -30,6 +30,7 @@
 #include <vector>
 #include "../../body/CollisionBody.h"
 #include "PairManager.h"
+#include "DynamicAABBTree.h"
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
@@ -39,9 +40,8 @@ class CollisionDetection;
 
 // Class BroadPhaseAlgorithm
 /**
- * This class is an abstract class that represents an algorithm
+ * This class represents an algorithm the broad-phase collision detection. The
- * used to perform the broad-phase of a collision detection. The
+ * goal of the broad-phase collision detection is to compute the pair of bodies
- * goal of the broad-phase algorithm is to compute the pair of bodies
  * that can collide. But it's important to understand that the
  * broad-phase doesn't compute only body pairs that can collide but
  * could also pairs of body that doesn't collide but are very close.
@@ -55,6 +55,9 @@ class BroadPhaseAlgorithm {
 
         // -------------------- Attributes -------------------- //
 
+        /// Dynamic AABB tree
+        DynamicAABBTree mDynamicAABBTree;
+
         /// Pair manager containing the overlapping pairs
         PairManager mPairManager;
 

src/collision/broadphase/DynamicAABBTree.cpp

@@ -0,0 +1,554 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "DynamicAABBTree.h"
+
+using namespace reactphysics3d;
+
+// Initialization of static variables
+const int TreeNode::NULL_TREE_NODE = -1;
+
+// Constructor
+DynamicAABBTree::DynamicAABBTree() {
+
+    mRootNodeID = TreeNode::NULL_TREE_NODE;
+    mNbNodes = 0;
+    mNbAllocatedNodes = 8;
+
+    // Allocate memory for the nodes of the tree
+    mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
+    assert(mNodes);
+    memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode));
+
+    // Initialize the allocated nodes
+    for (int i=0; i<mNbAllocatedNodes - 1; i++) {
+        mNodes[i].nextNodeID = i + 1;
+        mNodes[i].height = -1;
+    }
+    mNodes[mNbAllocatedNodes - 1].nextNodeID = TreeNode::NULL_TREE_NODE;
+    mNodes[mNbAllocatedNodes - 1].height = -1;
+    mFreeNodeID = 0;
+}
+
+// Destructor
+DynamicAABBTree::~DynamicAABBTree() {
+
+    // Free the allocated memory for the nodes
+    free(mNodes);
+}
+
+// Allocate and return a new node in the tree
+int DynamicAABBTree::allocateNode() {
+
+    // If there is no more allocated node to use
+    if (mFreeNodeID == TreeNode::NULL_TREE_NODE) {
+
+        assert(mNbNodes == mNbAllocatedNodes);
+
+        // Allocate more nodes in the tree
+        mNbAllocatedNodes *= 2;
+        TreeNode* oldNodes = mNodes;
+        mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
+        assert(mNodes);
+        memcpy(mNodes, oldNodes, mNbNodes * sizeof(TreeNode));
+        free(oldNodes);
+
+        // Initialize the allocated nodes
+        for (int i=mNbNodes; i<mNbAllocatedNodes - 1; i++) {
+            mNodes[i].nextNodeID = i + 1;
+            mNodes[i].height = -1;
+        }
+        mNodes[mNbAllocatedNodes - 1].nextNodeID = TreeNode::NULL_TREE_NODE;
+        mNodes[mNbAllocatedNodes - 1].height = -1;
+        mFreeNodeID = mNbNodes;
+    }
+
+    // Get the next free node
+    int freeNodeID = mFreeNodeID;
+    mFreeNodeID = mNodes[freeNodeID].nextNodeID;
+    mNodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE;
+    mNodes[freeNodeID].leftChildID = TreeNode::NULL_TREE_NODE;
+    mNodes[freeNodeID].rightChildID = TreeNode::NULL_TREE_NODE;
+    mNodes[freeNodeID].collisionShape = NULL;
+    mNodes[freeNodeID].height = 0;
+    mNbNodes++;
+
+    return freeNodeID;
+}
+
+// Release a node
+void DynamicAABBTree::releaseNode(int nodeID) {
+
+    assert(mNbNodes > 0);
+    assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
+    mNodes[nodeID].nextNodeID = mFreeNodeID;
+    mNodes[nodeID].height = -1;
+    mFreeNodeID = nodeID;
+    mNbNodes--;
+
+    // Deallocate nodes memory here if the number of allocated nodes is large
+    // compared to the number of nodes in the tree
+    if ((mNbNodes < mNbAllocatedNodes / 4) && mNbNodes > 8) {
+
+        // Allocate less nodes in the tree
+        mNbAllocatedNodes /= 2;
+        TreeNode* oldNodes = mNodes;
+        mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
+        assert(mNodes);
+        memcpy(mNodes, oldNodes, mNbNodes * sizeof(TreeNode));
+        free(oldNodes);
+
+        // Initialize the allocated nodes
+        for (int i=mNbNodes; i<mNbAllocatedNodes - 1; i++) {
+            mNodes[i].nextNodeID = i + 1;
+            mNodes[i].height = -1;
+        }
+        mNodes[mNbAllocatedNodes - 1].nextNodeID = TreeNode::NULL_TREE_NODE;
+        mNodes[mNbAllocatedNodes - 1].height = -1;
+        mFreeNodeID = mNbNodes;
+    }
+}
+
+// Add an object into the tree. This method creates a new leaf node in the tree and
+// returns the ID of the corresponding node.
+int DynamicAABBTree::addObject(CollisionShape* collisionShape, const AABB& aabb) {
+
+    // Get the next available node (or allocate new ones if necessary)
+    int nodeID = allocateNode();
+
+    // Create the fat aabb to use in the tree
+    const Vector3 gap(DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP);
+    mNodes[nodeID].aabb.setMin(mNodes[nodeID].aabb.getMin() - gap);
+    mNodes[nodeID].aabb.setMax(mNodes[nodeID].aabb.getMax() + gap);
+
+    // Set the collision shape
+    mNodes[nodeID].collisionShape = collisionShape;
+
+    // Set the height of the node in the tree
+    mNodes[nodeID].height = 0;
+
+    // Insert the new leaf node in the tree
+    insertLeafNode(nodeID);
+
+    // Return the node ID
+    return nodeID;
+}
+
+// Remove an object from the tree
+void DynamicAABBTree::removeObject(int nodeID) {
+
+    assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
+    assert(mNodes[nodeID].isLeaf());
+
+    // Remove the node from the tree
+    removeLeafNode(nodeID);
+    releaseNode(nodeID);
+}
+
+// Update the dynamic tree after an object has moved.
+/// If the new AABB of the object that has moved is still inside its fat AABB, then
+/// nothing is done. Otherwise, the corresponding node is removed and reinserted into the tree.
+/// The method returns true if the object has been reinserted into the tree.
+bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement) {
+
+    assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
+    assert(mNodes[nodeID].isLeaf());
+
+    // If the new AABB is still inside the fat AABB of the node
+    if (mNodes[nodeID].aabb.contains(newAABB)) {
+        return false;
+    }
+
+    // If the new AABB is outside the fat AABB, we remove the corresponding node
+    removeLeafNode(nodeID);
+
+    // Compute a new fat AABB for the new AABB by taking the object displacement into account
+    AABB fatAABB = newAABB;
+    const Vector3 gap(DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP);
+    fatAABB.mMinCoordinates -= gap;
+    fatAABB.mMaxCoordinates += gap;
+    const Vector3 displacementGap = AABB_DISPLACEMENT_MULTIPLIER * displacement;
+    if (displacementGap.x < decimal(0.0)) {
+        fatAABB.mMinCoordinates.x += displacementGap.x;
+    }
+    else {
+        fatAABB.mMaxCoordinates.x += displacementGap.x;
+    }
+    if (displacementGap.y < decimal(0.0)) {
+        fatAABB.mMinCoordinates.y += displacementGap.y;
+    }
+    else {
+        fatAABB.mMaxCoordinates.y += displacementGap.y;
+    }
+    if (displacementGap.z < decimal(0.0)) {
+        fatAABB.mMinCoordinates.z += displacementGap.z;
+    }
+    else {
+        fatAABB.mMaxCoordinates.z += displacementGap.z;
+    }
+    mNodes[nodeID].aabb = fatAABB;
+
+    // Reinsert the node into the tree
+    insertLeafNode(nodeID);
+
+    return true;
+}
+
+// Insert a leaf node in the tree. The process of inserting a new leaf node
+// in the dynamic tree is described in the book "Introduction to Game Physics
+// with Box2D" by Ian Parberry.
+void DynamicAABBTree::insertLeafNode(int nodeID) {
+
+    // If the tree is empty
+    if (mRootNodeID == TreeNode::NULL_TREE_NODE) {
+        mRootNodeID = nodeID;
+        mNodes[mRootNodeID].parentID = TreeNode::NULL_TREE_NODE;
+        return;
+    }
+
+    // Find the best sibling node for the new node
+    AABB newNodeAABB = mNodes[nodeID].aabb;
+    int currentNodeID = mRootNodeID;
+    while (!mNodes[currentNodeID].isLeaf()) {
+
+        int leftChild = mNodes[currentNodeID].leftChildID;
+        int rightChild = mNodes[currentNodeID].rightChildID;
+
+        // Compute the merged AABB
+        decimal volumeAABB = mNodes[currentNodeID].aabb.getVolume();
+        AABB mergedAABBs;
+        mergedAABBs.mergeTwoAABBs(mNodes[currentNodeID].aabb, newNodeAABB);
+        decimal mergedVolume = mergedAABBs.getVolume();
+
+        // Compute the cost of making the current node the sibbling of the new node
+        decimal costS = decimal(2.0) * mergedVolume;
+
+        // Compute the minimum cost of pushing the new node further down the tree (inheritance cost)
+        decimal costI = decimal(2.0) * (mergedVolume - volumeAABB);
+
+        // Compute the cost of descending into the left child
+        decimal costLeft;
+        AABB currentAndLeftAABB;
+        currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, mNodes[leftChild].aabb);
+        if (mNodes[leftChild].isLeaf()) {   // If the left child is a leaf
+            costLeft = currentAndLeftAABB.getVolume() + costI;
+        }
+        else {
+            decimal leftChildVolume = mNodes[leftChild].aabb.getVolume();
+            costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume;
+        }
+
+        // Compute the cost of descending into the right child
+        decimal costRight;
+        AABB currentAndRightAABB;
+        currentAndRightAABB.mergeTwoAABBs(newNodeAABB, mNodes[rightChild].aabb);
+        if (mNodes[rightChild].isLeaf()) {   // If the right child is a leaf
+            costRight = currentAndRightAABB.getVolume() + costI;
+        }
+        else {
+            decimal rightChildVolume = mNodes[rightChild].aabb.getVolume();
+            costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume;
+        }
+
+        // If the cost of making the current node a sibbling of the new node is smaller than
+        // the cost of going down into the left or right child
+        if (costS < costLeft && costS < costRight) break;
+
+        // It is cheaper to go down into a child of the current node, choose the best child
+        if (costLeft < costRight) {
+            currentNodeID = leftChild;
+        }
+        else {
+            currentNodeID = rightChild;
+        }
+    }
+
+    int siblingNode = currentNodeID;
+
+    // Create a new parent for the new node and the sibling node
+    int oldParentNode = mNodes[siblingNode].parentID;
+    int newParentNode = allocateNode();
+    mNodes[newParentNode].parentID = oldParentNode;
+    mNodes[newParentNode].collisionShape = NULL;
+    mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB);
+    mNodes[newParentNode].height = mNodes[siblingNode].height + 1;
+
+    // If the sibling node was not the root node
+    if (oldParentNode != TreeNode::NULL_TREE_NODE) {
+        if (mNodes[oldParentNode].leftChildID == siblingNode) {
+            mNodes[oldParentNode].leftChildID = newParentNode;
+        }
+        else {
+            mNodes[oldParentNode].rightChildID = newParentNode;
+        }
+    }
+    else {  // If the sibling node was the root node
+        mNodes[newParentNode].leftChildID = siblingNode;
+        mNodes[newParentNode].rightChildID = nodeID;
+        mNodes[siblingNode].parentID = newParentNode;
+        mNodes[nodeID].parentID = newParentNode;
+        mRootNodeID = newParentNode;
+    }
+
+    // Move up in the tree to change the AABBs that have changed
+    currentNodeID = mNodes[nodeID].parentID;
+    while (currentNodeID != TreeNode::NULL_TREE_NODE) {
+
+        // Balance the sub-tree of the current node if it is not balanced
+        currentNodeID = balanceSubTreeAtNode(currentNodeID);
+
+        int leftChild = mNodes[currentNodeID].leftChildID;
+        int rightChild = mNodes[currentNodeID].rightChildID;
+        assert(leftChild != TreeNode::NULL_TREE_NODE);
+        assert(rightChild != TreeNode::NULL_TREE_NODE);
+
+        // Recompute the height of the node in the tree
+        mNodes[currentNodeID].height = std::max(mNodes[leftChild].height,
+                                                mNodes[rightChild].height) + 1;
+
+        // Recompute the AABB of the node
+        mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
+
+        currentNodeID = mNodes[currentNodeID].parentID;
+    }
+}
+
+// Remove a leaf node from the tree
+void DynamicAABBTree::removeLeafNode(int nodeID) {
+
+    assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
+
+    // If we are removing the root node (root node is a leaf in this case)
+    if (mRootNodeID == nodeID) {
+        mRootNodeID = TreeNode::NULL_TREE_NODE;
+        return;
+    }
+
+    int parentNodeID = mNodes[nodeID].parentID;
+    int grandParentNodeID = mNodes[parentNodeID].parentID;
+    int siblingNodeID;
+    if (mNodes[parentNodeID].leftChildID == nodeID) {
+        siblingNodeID = mNodes[parentNodeID].rightChildID;
+    }
+    else {
+        siblingNodeID = mNodes[parentNodeID].leftChildID;
+    }
+
+    // If the parent of the node to remove is not the root node
+    if (grandParentNodeID != TreeNode::NULL_TREE_NODE) {
+
+        // Destroy the parent node
+        if (mNodes[grandParentNodeID].leftChildID == parentNodeID) {
+            mNodes[grandParentNodeID].leftChildID = siblingNodeID;
+        }
+        else {
+            assert(mNodes[grandParentNodeID].rightChildID == parentNodeID);
+            mNodes[grandParentNodeID].rightChildID = siblingNodeID;
+        }
+        mNodes[siblingNodeID].parentID = grandParentNodeID;
+        releaseNode(parentNodeID);
+
+        // Now, we need to recompute the AABBs of the node on the path back to the root
+        // and make sure that the tree is still balanced
+        int currentNodeID = grandParentNodeID;
+        while(currentNodeID != TreeNode::NULL_TREE_NODE) {
+
+            // Balance the current sub-tree if necessary
+            currentNodeID = balanceSubTreeAtNode(currentNodeID);
+
+            // Get the two children of the current node
+            int leftChildID = mNodes[currentNodeID].leftChildID;
+            int rightChildID = mNodes[currentNodeID].rightChildID;
+
+            // Recompute the AABB and the height of the current node
+            mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChildID].aabb,
+                                                     mNodes[rightChildID].aabb);
+            mNodes[currentNodeID].height = std::max(mNodes[leftChildID].height,
+                                                    mNodes[rightChildID].height) + 1;
+
+            currentNodeID = mNodes[currentNodeID].parentID;
+        }
+
+    }
+    else { // If the parent of the node to remove is the root node
+
+        // The sibling node becomes the new root node
+        mRootNodeID = siblingNodeID;
+        mNodes[siblingNodeID].parentID = TreeNode::NULL_TREE_NODE;
+        releaseNode(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
+/// with Box2D" by Ian Parberry. This method returns the new root node ID.
+int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
+
+    assert(nodeID != TreeNode::NULL_TREE_NODE);
+
+    TreeNode* nodeA = mNodes + nodeID;
+
+    // If the node is a leaf or the height of A's sub-tree is less than 2
+    if (nodeA->isLeaf() || nodeA->height < 2) {
+
+        // Do not perform any rotation
+        return nodeID;
+    }
+
+    // Get the two children nodes
+    int nodeBID = nodeA->leftChildID;
+    int nodeCID = nodeA->rightChildID;
+    assert(nodeBID >= 0 && nodeBID < mNbAllocatedNodes);
+    assert(nodeCID >= 0 && nodeCID < mNbAllocatedNodes);
+    TreeNode* nodeB = mNodes + nodeBID;
+    TreeNode* nodeC = mNodes + nodeCID;
+
+    // Compute the factor of the left and right sub-trees
+    int balanceFactor = nodeC->height - nodeB->height;
+
+    // If the right node C is 2 higher than left node B
+    if (balanceFactor > 1) {
+
+        int nodeFID = nodeC->leftChildID;
+        int nodeGID = nodeC->rightChildID;
+        assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
+        assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
+        TreeNode* nodeF = mNodes + nodeFID;
+        TreeNode* nodeG = mNodes + nodeGID;
+
+        nodeC->leftChildID = nodeID;
+        nodeC->parentID = nodeA->parentID;
+        nodeA->parentID = nodeCID;
+
+        if (nodeC->parentID != TreeNode::NULL_TREE_NODE) {
+
+            if (mNodes[nodeC->parentID].leftChildID == nodeID) {
+                mNodes[nodeC->parentID].leftChildID = nodeCID;
+            }
+            else {
+                assert(mNodes[nodeC->parentID].rightChildID == nodeID);
+                mNodes[nodeC->parentID].rightChildID = nodeCID;
+            }
+        }
+        else {
+            mRootNodeID = nodeCID;
+        }
+
+        // If the right node C was higher than left node B because of the F node
+        if (nodeF->height > nodeG->height) {
+            nodeC->rightChildID = nodeFID;
+            nodeA->rightChildID = nodeGID;
+            nodeG->parentID = nodeID;
+
+            // Recompute the AABB of node A and C
+            nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeG->aabb);
+            nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb);
+
+            // Recompute the height of node A and C
+            nodeA->height = std::max(nodeB->height, nodeG->height) + 1;
+            nodeC->height = std::max(nodeA->height, nodeF->height) + 1;
+        }
+        else {  // If the right node C was higher than left node B because of node G
+            nodeC->rightChildID = nodeGID;
+            nodeA->rightChildID = nodeFID;
+            nodeF->parentID = nodeID;
+
+            // Recompute the AABB of node A and C
+            nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeF->aabb);
+            nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb);
+
+            // Recompute the height of node A and C
+            nodeA->height = std::max(nodeB->height, nodeF->height) + 1;
+            nodeC->height = std::max(nodeA->height, nodeG->height) + 1;
+        }
+
+        // Return the new root of the sub-tree
+        return nodeCID;
+    }
+
+    // If the left node B is 2 higher than right node C
+    if (balanceFactor < -1) {
+
+        int nodeFID = nodeB->leftChildID;
+        int nodeGID = nodeB->rightChildID;
+        assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
+        assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
+        TreeNode* nodeF = mNodes + nodeFID;
+        TreeNode* nodeG = mNodes + nodeGID;
+
+        nodeB->leftChildID = nodeID;
+        nodeB->parentID = nodeA->parentID;
+        nodeA->parentID = nodeBID;
+
+        if (nodeB->parentID != TreeNode::NULL_TREE_NODE) {
+
+            if (mNodes[nodeB->parentID].leftChildID == nodeID) {
+                mNodes[nodeB->parentID].leftChildID = nodeBID;
+            }
+            else {
+                assert(mNodes[nodeB->parentID].rightChildID == nodeID);
+                mNodes[nodeB->parentID].rightChildID = nodeBID;
+            }
+        }
+        else {
+            mRootNodeID = nodeBID;
+        }
+
+        // If the left node B was higher than right node C because of the F node
+        if (nodeF->height > nodeG->height) {
+            nodeB->rightChildID = nodeFID;
+            nodeA->leftChildID = nodeGID;
+            nodeG->parentID = nodeID;
+
+            // Recompute the AABB of node A and B
+            nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeG->aabb);
+            nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb);
+
+            // Recompute the height of node A and B
+            nodeA->height = std::max(nodeC->height, nodeG->height) + 1;
+            nodeB->height = std::max(nodeA->height, nodeF->height) + 1;
+        }
+        else {  // If the left node B was higher than right node C because of node G
+            nodeB->rightChildID = nodeGID;
+            nodeA->leftChildID = nodeFID;
+            nodeF->parentID = nodeID;
+
+            // Recompute the AABB of node A and B
+            nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeF->aabb);
+            nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb);
+
+            // Recompute the height of node A and B
+            nodeA->height = std::max(nodeC->height, nodeF->height) + 1;
+            nodeB->height = std::max(nodeA->height, nodeG->height) + 1;
+        }
+
+        // Return the new root of the sub-tree
+        return nodeBID;
+    }
+
+    // If the sub-tree is balanced, return the current root node
+    return nodeID;
+}

src/collision/broadphase/DynamicAABBTree.h

@@ -0,0 +1,147 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2014 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_DYNAMIC_AABB_TREE_H
+#define REACTPHYSICS3D_DYNAMIC_AABB_TREE_H
+
+// Libraries
+#include "../../configuration.h"
+#include "../shapes/AABB.h"
+#include "../shapes/CollisionShape.h"
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Structure TreeNode
+/**
+ * This structure represents a node of the dynamic AABB tree.
+ */
+struct TreeNode {
+
+    // -------------------- Constants -------------------- //
+
+    /// Null tree node constant
+    const static int NULL_TREE_NODE;
+
+    // -------------------- Attributes -------------------- //
+
+    /// Parent node ID
+    int parentID;
+
+    /// Left and right child of the node
+    int leftChildID, rightChildID;
+
+    /// Next allocated node ID
+    int nextNodeID;
+
+    /// Height of the node in the tree
+    int height;
+
+    /// Fat axis aligned bounding box (AABB) corresponding to the node
+    AABB aabb;
+
+    /// Pointer to the corresponding collision shape (in case this node is a leaf)
+    CollisionShape* collisionShape;
+
+    // -------------------- Methods -------------------- //
+
+    /// Return true if the node is a leaf of the tree
+    bool isLeaf() const;
+};
+
+// Class DynamicAABBTree
+/**
+ * This class implements a dynamic AABB tree that is used for broad-phase
+ * collision detection. This data structure is inspired by Nathanael Presson's
+ * dynamic tree implementation in BulletPhysics. The following implementation is
+ * based on the one from Erin Catto in Box2D as described in the book
+ * "Introduction to Game Physics with Box2D" by Ian Parberry.
+ */
+class DynamicAABBTree {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Pointer to the memory location of the nodes of the tree
+        TreeNode* mNodes;
+
+        /// ID of the root node of the tree
+        int mRootNodeID;
+
+        /// ID of the first node of the list of free (allocated) nodes in the tree that we can use
+        int mFreeNodeID;
+
+        /// Number of allocated nodes in the tree
+        int mNbAllocatedNodes;
+
+        /// Number of nodes in the tree
+        int mNbNodes;
+
+        // -------------------- Methods -------------------- //
+
+        /// Allocate and return a node to use in the tree
+        int allocateNode();
+
+        /// Release a node
+        void releaseNode(int nodeID);
+
+        /// Insert a leaf node in the tree
+        void insertLeafNode(int nodeID);
+
+        /// Remove a leaf node from the tree
+        void removeLeafNode(int nodeID);
+
+        /// Balance the sub-tree of a given node using left or right rotations.
+        int balanceSubTreeAtNode(int nodeID);
+
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        DynamicAABBTree();
+
+        /// Destructor
+        ~DynamicAABBTree();
+
+        /// Add an object into the tree
+        int addObject(CollisionShape* collisionShape, const AABB& aabb);
+
+        /// Remove an object from the tree
+        void removeObject(int nodeID);
+
+        /// Update the dynamic tree after an object has moved.
+        bool updateObject(int nodeID, const AABB &newAABB, const Vector3 &displacement);
+};
+
+// Return true if the node is a leaf of the tree
+inline bool TreeNode::isLeaf() const {
+    return leftChildID == NULL_TREE_NODE;
+}
+
+}
+
+#endif

src/collision/shapes/AABB.cpp

@@ -42,8 +42,39 @@ AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates)
 
 }
 
+// Copy-constructor
+AABB::AABB(const AABB& aabb)
+     : mMinCoordinates(aabb.mMinCoordinates), mMaxCoordinates(aabb.mMaxCoordinates) {
+
+}
+
 // Destructor
 AABB::~AABB() {
 
 }
 
+// Replace the current AABB with a new AABB that is the union of two AABBs in parameters
+void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) {
+    mMinCoordinates.x = std::min(aabb1.mMinCoordinates.x, aabb2.mMinCoordinates.x);
+    mMinCoordinates.y = std::min(aabb1.mMinCoordinates.y, aabb2.mMinCoordinates.y);
+    mMinCoordinates.z = std::min(aabb1.mMinCoordinates.z, aabb2.mMinCoordinates.z);
+
+    mMaxCoordinates.x = std::max(aabb1.mMaxCoordinates.x, aabb2.mMaxCoordinates.x);
+    mMaxCoordinates.y = std::max(aabb1.mMaxCoordinates.y, aabb2.mMaxCoordinates.y);
+    mMaxCoordinates.z = std::max(aabb1.mMaxCoordinates.z, aabb2.mMaxCoordinates.z);
+}
+
+// Return true if the current AABB contains the AABB given in parameter
+bool AABB::contains(const AABB& aabb) {
+
+    bool isInside = true;
+    isInside = isInside && mMinCoordinates.x <= aabb.mMinCoordinates.x;
+    isInside = isInside && mMinCoordinates.y <= aabb.mMinCoordinates.y;
+    isInside = isInside && mMinCoordinates.z <= aabb.mMinCoordinates.z;
+
+    isInside = isInside && mMaxCoordinates.x >= aabb.mMaxCoordinates.x;
+    isInside = isInside && mMaxCoordinates.y >= aabb.mMaxCoordinates.y;
+    isInside = isInside && mMaxCoordinates.z >= aabb.mMaxCoordinates.z;
+    return isInside;
+}
+

src/collision/shapes/AABB.h

@@ -31,9 +31,6 @@
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
-
-// Declaration
-class Body;
     
 // Class AABB
 /**
@@ -54,17 +51,6 @@ class AABB {
         /// Maximum world coordinates of the AABB on the x,y and z axis
         Vector3 mMaxCoordinates;
 
-        // -------------------- Methods -------------------- //
-
-        /// Private copy-constructor
-        AABB(const AABB& aabb);
-
-        /// Private assignment operator
-        AABB& operator=(const AABB& aabb);
-
-        /// Constructor
-        AABB(const Transform& transform, const Vector3& extents);
-
     public :
 
         // -------------------- Methods -------------------- //
@@ -75,10 +61,11 @@ class AABB {
         /// Constructor
         AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates);
 
-
+        /// Copy-constructor
+        AABB(const AABB& aabb);
 
         /// Destructor
-        virtual ~AABB();
+        ~AABB();
 
         /// Return the center point
         Vector3 getCenter() const;
@@ -97,11 +84,27 @@ class AABB {
 
         /// Return true if the current AABB is overlapping with the AABB in argument
         bool testCollision(const AABB& aabb) const;
+
+        /// Return the volume of the AABB
+        decimal getVolume() const;
+
+        /// Replace the current AABB with a new AABB that is the union of two AABBs in parameters
+        void mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2);
+
+        /// Return true if the current AABB contains the AABB given in parameter
+        bool contains(const AABB& aabb);
+
+        /// Assignment operator
+        AABB& operator=(const AABB& aabb);
+
+        // -------------------- Friendship -------------------- //
+
+        friend class DynamicAABBTree;
 };
 
 // Return the center point of the AABB in world coordinates
 inline Vector3 AABB::getCenter() const {
-    return (mMinCoordinates + mMaxCoordinates) * 0.5;
+    return (mMinCoordinates + mMaxCoordinates) * decimal(0.5);
 }
 
 // Return the minimum coordinates of the AABB
@@ -119,7 +122,7 @@ inline const Vector3& AABB::getMax() const {
     return mMaxCoordinates;
 }
 
-/// Set the maximum coordinates of the AABB
+// Set the maximum coordinates of the AABB
 inline void AABB::setMax(const Vector3& max) {
     mMaxCoordinates = max;
 }
@@ -136,6 +139,21 @@ inline bool AABB::testCollision(const AABB& aabb) const {
     return true;
 }
 
+// Return the volume of the AABB
+inline decimal AABB::getVolume() const {
+    const Vector3 diff = mMaxCoordinates - mMinCoordinates;
+    return (diff.x * diff.y * diff.z);
+}
+
+// Assignment operator
+inline AABB& AABB::operator=(const AABB& aabb) {
+    if (this != &aabb) {
+        mMinCoordinates = aabb.mMinCoordinates;
+        mMaxCoordinates = aabb.mMaxCoordinates;
+    }
+    return *this;
+}
+
 }
 
 #endif

src/configuration.h

@@ -121,6 +121,15 @@ const decimal DEFAULT_SLEEP_LINEAR_VELOCITY = decimal(0.02);
 /// might enter sleeping mode
 const decimal DEFAULT_SLEEP_ANGULAR_VELOCITY = decimal(3.0 * (PI / 180.0));
 
+/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
+/// fatten to allow the collision shape to move a little bit without triggering
+/// a large modification of the tree which can be costly
+const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1);
+
+/// In the dynamic AABB tree, we multiply this factor by the displacement of
+/// an object that has moved to recompute a new fat AABB
+const decimal AABB_DISPLACEMENT_MULTIPLIER = decimal(2.0);
+
 }
 
 #endif