fix issues in Dynamic AABB Tree and add compound shapes in the examples

examples/collisionshapes/CMakeLists.txt

@@ -30,6 +30,7 @@ SET(COLLISION_SHAPES_SOURCES
     "../common/Sphere.cpp"
     "../common/Cylinder.cpp"
     "../common/Cone.cpp"
+    "../common/Dumbbell.cpp"
     "../common/Box.cpp"
     "../common/Viewer.cpp"
 )

examples/collisionshapes/Scene.cpp

@@ -62,6 +62,25 @@ Scene::Scene(Viewer* viewer, const std::string& shaderFolderPath, const std::str
 
     float radius = 3.0f;
 
+    for (int i=0; i<NB_COMPOUND_SHAPES; i++) {
+
+        // Position
+        float angle = i * 30.0f;
+        openglframework::Vector3 position(radius * cos(angle),
+                                          80 + i * (DUMBBELL_HEIGHT + 0.3f),
+                                          radius * sin(angle));
+
+        // Create a convex mesh and a corresponding rigid in the dynamics world
+        Dumbbell* dumbbell = new Dumbbell(position, mDynamicsWorld, meshFolderPath);
+
+        // Change the material properties of the rigid body
+        rp3d::Material& material = dumbbell->getRigidBody()->getMaterial();
+        material.setBounciness(rp3d::decimal(0.2));
+
+        // Add the mesh the list of dumbbells in the scene
+        mDumbbells.push_back(dumbbell);
+    }
+
     // Create all the boxes of the scene
     for (int i=0; i<NB_BOXES; i++) {
 
@@ -271,6 +290,17 @@ Scene::~Scene() {
         delete (*it);
     }
 
+    // Destroy all the dumbbell of the scene
+    for (std::vector<Dumbbell*>::iterator it = mDumbbells.begin();
+         it != mDumbbells.end(); ++it) {
+
+        // Destroy the corresponding rigid body from the dynamics world
+        mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
+
+        // Destroy the convex mesh
+        delete (*it);
+    }
+
     // Destroy all the visual contact points
     for (std::vector<VisualContactPoint*>::iterator it = mContactPoints.begin();
          it != mContactPoints.end(); ++it) {
@@ -342,6 +372,14 @@ void Scene::simulate() {
             (*it)->updateTransform();
         }
 
+        // Update the position and orientation of the dumbbells
+        for (std::vector<Dumbbell*>::iterator it = mDumbbells.begin();
+             it != mDumbbells.end(); ++it) {
+
+            // Update the transform used for the rendering
+            (*it)->updateTransform();
+        }
+
         // Destroy all the visual contact points
         for (std::vector<VisualContactPoint*>::iterator it = mContactPoints.begin();
              it != mContactPoints.end(); ++it) {
@@ -422,6 +460,12 @@ void Scene::render() {
         (*it)->render(mPhongShader, worldToCameraMatrix);
     }
 
+    // Render all the dumbbells of the scene
+    for (std::vector<Dumbbell*>::iterator it = mDumbbells.begin();
+         it != mDumbbells.end(); ++it) {
+        (*it)->render(mPhongShader, worldToCameraMatrix);
+    }
+
     // Render all the visual contact points
     for (std::vector<VisualContactPoint*>::iterator it = mContactPoints.begin();
          it != mContactPoints.end(); ++it) {

examples/collisionshapes/Scene.h

@@ -35,16 +35,18 @@
 #include "Cylinder.h"
 #include "Capsule.h"
 #include "ConvexMesh.h"
+#include "Dumbbell.h"
 #include "VisualContactPoint.h"
 #include "../common/Viewer.h"
 
 // Constants
 const int NB_BOXES = 3;
-const int NB_SPHERES = 3;
-const int NB_CONES = 3;
-const int NB_CYLINDERS = 3;
-const int NB_CAPSULES = 3;
-const int NB_MESHES = 3;
+const int NB_SPHERES = 2;
+const int NB_CONES = 0;
+const int NB_CYLINDERS = 0;
+const int NB_CAPSULES = 0;
+const int NB_MESHES = 0;
+const int NB_COMPOUND_SHAPES = 2;
 const openglframework::Vector3 BOX_SIZE(2, 2, 2);
 const float SPHERE_RADIUS = 1.5f;
 const float CONE_RADIUS = 2.0f;
@@ -53,6 +55,7 @@ const float CYLINDER_RADIUS = 1.0f;
 const float CYLINDER_HEIGHT = 5.0f;
 const float CAPSULE_RADIUS = 1.0f;
 const float CAPSULE_HEIGHT = 1.0f;
+const float DUMBBELL_HEIGHT = 1.0f;
 const openglframework::Vector3 FLOOR_SIZE(20, 0.5f, 20);        // Floor dimensions in meters
 const float BOX_MASS = 1.0f;
 const float CONE_MASS = 1.0f;
@@ -91,6 +94,9 @@ class Scene {
         /// All the convex meshes of the scene
         std::vector<ConvexMesh*> mConvexMeshes;
 
+        /// All the dumbbell of the scene
+        std::vector<Dumbbell*> mDumbbells;
+
         /// All the visual contact points
         std::vector<VisualContactPoint*> mContactPoints;
 

examples/common/Box.cpp

@@ -77,7 +77,7 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
 
     // Create the collision shape for the rigid body (box shape)
     // ReactPhysics3D will clone this object to create an internal one. Therefore,
-    // it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
+    // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
     const rp3d::BoxShape collisionShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2]));
 
     // Initial position and orientation of the rigid body

examples/common/Capsule.cpp

@@ -50,7 +50,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
 
     // Create the collision shape for the rigid body (sphere shape)
     // ReactPhysics3D will clone this object to create an internal one. Therefore,
-    // it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
+    // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
     const rp3d::CapsuleShape collisionShape(mRadius, mHeight);
 
     // Initial position and orientation of the rigid body

examples/common/Cone.cpp

@@ -50,7 +50,7 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
 
     // Create the collision shape for the rigid body (cone shape)
     // ReactPhysics3D will clone this object to create an internal one. Therefore,
-    // it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
+    // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
     const rp3d::ConeShape collisionShape(mRadius, mHeight);
 
     // Initial position and orientation of the rigid body

examples/common/ConvexMesh.cpp

@@ -44,7 +44,7 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
 
     // Create the collision shape for the rigid body (convex mesh shape)
     // ReactPhysics3D will clone this object to create an internal one. Therefore,
-    // it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
+    // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
     rp3d::decimal* verticesArray = (rp3d::decimal*) getVerticesPointer();
     rp3d::ConvexMeshShape collisionShape(verticesArray, mVertices.size(),
                                          sizeof(openglframework::Vector3));

examples/common/Cylinder.cpp

@@ -28,7 +28,7 @@
 
 
 // Constructor
-Cylinder::Cylinder(float radius, float height, const openglframework::Vector3 &position,
+Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position,
            float mass, reactphysics3d::DynamicsWorld* dynamicsWorld,
                    const std::string& meshFolderPath)
      : openglframework::Mesh(), mRadius(radius), mHeight(height) {
@@ -50,7 +50,7 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3 &p
 
     // Create the collision shape for the rigid body (cylinder shape)
     // ReactPhysics3D will clone this object to create an internal one. Therefore,
-    // it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
+    // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
     const rp3d::CylinderShape collisionShape(mRadius, mHeight);
 
     // Initial position and orientation of the rigid body

examples/common/Dumbbell.cpp

@@ -0,0 +1,154 @@
+/********************************************************************************
+* 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.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "Dumbbell.h"
+
+
+// Constructor
+Dumbbell::Dumbbell(const openglframework::Vector3 &position,
+                   reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath)
+         : openglframework::Mesh() {
+
+    // Load the mesh from a file
+    openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this);
+
+    // Calculate the normals of the mesh
+    calculateNormals();
+
+    // Identity scaling matrix
+    mScalingMatrix.setToIdentity();
+
+    // Initialize the position where the sphere will be rendered
+    translateWorld(position);
+
+    // Create a sphere collision shape for the two ends of the dumbbell
+    // ReactPhysics3D will clone this object to create an internal one. Therefore,
+    // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
+    const rp3d::decimal radiusSphere = rp3d::decimal(1.5);
+    const rp3d::decimal massSphere = rp3d::decimal(2.0);
+    const rp3d::SphereShape sphereCollisionShape(radiusSphere);
+
+    // Create a cylinder collision shape for the middle of the dumbbell
+    // ReactPhysics3D will clone this object to create an internal one. Therefore,
+    // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
+    const rp3d::decimal radiusCylinder = rp3d::decimal(0.5);
+    const rp3d::decimal heightCylinder = rp3d::decimal(8.0);
+    const rp3d::decimal massCylinder = rp3d::decimal(1.0);
+    const rp3d::CylinderShape cylinderCollisionShape(radiusCylinder, heightCylinder);
+
+    // Initial position and orientation of the rigid body
+    rp3d::Vector3 initPosition(position.x, position.y, position.z);
+    rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
+    rp3d::Transform transformBody(initPosition, initOrientation);
+
+    // Initial transform of the first sphere collision shape of the dumbbell (in local-space)
+    rp3d::Transform transformSphereShape1(rp3d::Vector3(0, 4.0, 0), rp3d::Quaternion::identity());
+
+    // Initial transform of the second sphere collision shape of the dumbell (in local-space)
+    rp3d::Transform transformSphereShape2(rp3d::Vector3(0, -4.0, 0), rp3d::Quaternion::identity());
+
+    // Initial transform of the cylinder collision shape of the dumbell (in local-space)
+    rp3d::Transform transformCylinderShape(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity());
+
+    // Create a rigid body corresponding to the dumbbell in the dynamics world
+    mRigidBody = dynamicsWorld->createRigidBody(transformBody);
+
+    // Add the three collision shapes to the body and specify the mass and transform of the shapes
+    mRigidBody->addCollisionShape(sphereCollisionShape, massSphere, transformSphereShape1);
+    mRigidBody->addCollisionShape(sphereCollisionShape, massSphere, transformSphereShape2);
+    //mRigidBody->addCollisionShape(cylinderCollisionShape, massCylinder, transformCylinderShape);
+}
+
+// Destructor
+Dumbbell::~Dumbbell() {
+
+    // Destroy the mesh
+    destroy();
+}
+
+// Render the sphere at the correct position and with the correct orientation
+void Dumbbell::render(openglframework::Shader& shader,
+                    const openglframework::Matrix4& worldToCameraMatrix) {
+
+    // Bind the shader
+    shader.bind();
+
+    // Set the model to camera matrix
+    const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix;
+    shader.setMatrix4x4Uniform("localToCameraMatrix", localToCameraMatrix);
+
+    // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the
+    // model-view matrix)
+    const openglframework::Matrix3 normalMatrix =
+                       localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose();
+    shader.setMatrix3x3Uniform("normalMatrix", normalMatrix);
+
+    glEnableClientState(GL_VERTEX_ARRAY);
+    glEnableClientState(GL_NORMAL_ARRAY);
+    if (hasTexture()) {
+        glEnableClientState(GL_TEXTURE_COORD_ARRAY);
+    }
+
+    glVertexPointer(3, GL_FLOAT, 0, getVerticesPointer());
+    glNormalPointer(GL_FLOAT, 0, getNormalsPointer());
+    if(hasTexture()) {
+        glTexCoordPointer(2, GL_FLOAT, 0, getUVTextureCoordinatesPointer());
+    }
+
+    // For each part of the mesh
+    for (unsigned int i=0; i<getNbParts(); i++) {
+        glDrawElements(GL_TRIANGLES, getNbFaces(i) * 3,
+                       GL_UNSIGNED_INT, getIndicesPointer());
+    }
+
+    glDisableClientState(GL_NORMAL_ARRAY);
+    glDisableClientState(GL_VERTEX_ARRAY);
+    if (hasTexture()) {
+        glDisableClientState(GL_TEXTURE_COORD_ARRAY);
+    }
+
+    // Unbind the shader
+    shader.unbind();
+}
+
+// Update the transform matrix of the sphere
+void Dumbbell::updateTransform() {
+
+    // Get the interpolated transform of the rigid body
+    rp3d::Transform transform = mRigidBody->getInterpolatedTransform();
+
+    // Compute the transform used for rendering the sphere
+    float matrix[16];
+    transform.getOpenGLMatrix(matrix);
+    openglframework::Matrix4 newMatrix(matrix[0], matrix[4], matrix[8], matrix[12],
+                                       matrix[1], matrix[5], matrix[9], matrix[13],
+                                       matrix[2], matrix[6], matrix[10], matrix[14],
+                                       matrix[3], matrix[7], matrix[11], matrix[15]);
+
+    // Apply the scaling matrix to have the correct sphere dimensions
+    mTransformMatrix = newMatrix * mScalingMatrix;
+}
+

examples/common/Dumbbell.h

@@ -0,0 +1,78 @@
+/********************************************************************************
+* 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 DUMBBELL_H
+#define DUMBBELL_H
+
+// Libraries
+#include "openglframework.h"
+#include "reactphysics3d.h"
+
+// Class Sphere
+class Dumbbell : public openglframework::Mesh {
+
+    private :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Radius of the spheres
+        float mRadius;
+
+        /// Rigid body used to simulate the dynamics of the sphere
+        rp3d::RigidBody* mRigidBody;
+
+        /// Scaling matrix (applied to a sphere to obtain the correct sphere dimensions)
+        openglframework::Matrix4 mScalingMatrix;
+
+        // -------------------- Methods -------------------- //
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        Dumbbell(const openglframework::Vector3& position, rp3d::DynamicsWorld* dynamicsWorld,
+                 const std::string& meshFolderPath);
+
+        /// Destructor
+        ~Dumbbell();
+
+        /// Return a pointer to the rigid body of the sphere
+        rp3d::RigidBody* getRigidBody();
+
+        /// Update the transform matrix of the sphere
+        void updateTransform();
+
+        /// Render the sphere at the correct position and with the correct orientation
+        void render(openglframework::Shader& shader,
+                    const openglframework::Matrix4& worldToCameraMatrix);
+};
+
+// Return a pointer to the rigid body of the sphere
+inline rp3d::RigidBody* Dumbbell::getRigidBody() {
+    return mRigidBody;
+}
+
+#endif

examples/common/Sphere.cpp

@@ -50,7 +50,7 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position,
 
     // Create the collision shape for the rigid body (sphere shape)
     // ReactPhysics3D will clone this object to create an internal one. Therefore,
-    // it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
+    // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
     const rp3d::SphereShape collisionShape(mRadius);
 
     // Initial position and orientation of the rigid body

src/body/CollisionBody.cpp

@@ -80,8 +80,12 @@ const ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisi
         mProxyCollisionShapes = proxyShape;
     }
 
+    // Compute the world-space AABB of the new collision shape
+    AABB aabb;
+    newCollisionShape->computeAABB(aabb, mTransform * transform);
+
     // Notify the collision detection about this new collision shape
-    mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape);
+    mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb);
 
     mNbCollisionShapes++;
 
@@ -180,7 +184,7 @@ void CollisionBody::updateBroadPhaseState() const {
 
         // Recompute the world-space AABB of the collision shape
         AABB aabb;
-        shape->getCollisionShape()->computeAABB(aabb, mTransform);
+        shape->getCollisionShape()->computeAABB(aabb, mTransform *shape->getLocalToBodyTransform());
 
         // Update the broad-phase state for the proxy collision shape
         mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb);

src/body/RigidBody.cpp

@@ -136,8 +136,7 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
 /// parameter is the transformation that transform the local-space of the collision shape into
 /// the local-space of the body. By default, the second parameter is the identity transform.
 const ProxyShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape,
-                                               decimal mass,
-                                               const Transform& transform) {
+                                               decimal mass, const Transform& transform) {
 
     assert(mass > decimal(0.0));
 
@@ -157,8 +156,12 @@ const ProxyShape* RigidBody::addCollisionShape(const CollisionShape& collisionSh
         mProxyCollisionShapes = proxyShape;
     }
 
+    // Compute the world-space AABB of the new collision shape
+    AABB aabb;
+    newCollisionShape->computeAABB(aabb, mTransform * transform);
+
     // Notify the collision detection about this new collision shape
-    mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape);
+    mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb);
 
     mNbCollisionShapes++;
 

src/collision/CollisionDetection.cpp

@@ -88,13 +88,33 @@ void CollisionDetection::computeNarrowPhase() {
     
     // For each possible collision pair of bodies
     map<overlappingpairid, OverlappingPair*>::iterator it;
-    for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); it++) {
+    for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
         ContactPointInfo* contactInfo = NULL;
 
         OverlappingPair* pair = it->second;
 
         ProxyShape* shape1 = pair->getShape1();
         ProxyShape* shape2 = pair->getShape2();
+
+        assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
+
+        // Check that the two shapes are overlapping. If the shapes are not overlapping
+        // anymore, we remove the overlapping pair.
+        if (!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
+
+            std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
+            ++it;
+
+            // Destroy the overlapping pair
+            itToRemove->second->OverlappingPair::~OverlappingPair();
+            mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
+            mOverlappingPairs.erase(itToRemove);
+            continue;
+        }
+        else {
+            ++it;
+        }
+
         CollisionBody* const body1 = shape1->getBody();
         CollisionBody* const body2 = shape2->getBody();
         
@@ -119,8 +139,6 @@ void CollisionDetection::computeNarrowPhase() {
         
         // Use the narrow-phase collision detection algorithm to check
         // if there really is a collision
-        const Transform transform1 = body1->getTransform() * shape1->getLocalToBodyTransform();
-        const Transform transform2 = body2->getTransform() * shape2->getLocalToBodyTransform();
         if (narrowPhaseAlgorithm.testCollision(shape1, shape2, contactInfo)) {
             assert(contactInfo != NULL);
 
@@ -141,9 +159,14 @@ void CollisionDetection::computeNarrowPhase() {
 }
 
 // Allow the broadphase to notify the collision detection about an overlapping pair.
-/// This method is called by a broad-phase collision detection algorithm
+/// This method is called by the broad-phase collision detection algorithm
 void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
 
+    assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
+
+    // If the two proxy collision shapes are from the same body, skip it
+    if (shape1->getBody()->getID() == shape2->getBody()->getID()) return;
+
     // Compute the overlapping pair ID
     overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2);
 
@@ -157,28 +180,6 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
     std::pair<map<overlappingpairid, OverlappingPair*>::iterator, bool> check =
             mOverlappingPairs.insert(make_pair(pairID, newPair));
     assert(check.second);
-
-    /* TODO : DELETE THIS
-    // Get the pair of body index
-    bodyindexpair indexPair = addedPair->getBodiesIndexPair();
-
-    // If the overlapping pair already exists, we don't do anything
-    if (mOverlappingPairs.count(indexPair) > 0) return;
-
-    // Create the corresponding broad-phase pair object
-    BroadPhasePair* broadPhasePair = new (mWorld->mMemoryAllocator.allocate(sizeof(BroadPhasePair)))
-                                             BroadPhasePair(addedPair->body1, addedPair->body2);
-    assert(broadPhasePair != NULL);
-
-    // Add the pair into the set of overlapping pairs (if not there yet)
-    pair<map<bodyindexpair, BroadPhasePair*>::iterator, bool> check = mOverlappingPairs.insert(
-                                                                            make_pair(indexPair,
-                                                                            broadPhasePair));
-    assert(check.second);
-
-    // Notify the world about the new broad-phase overlapping pair
-    mWorld->notifyAddedOverlappingPair(broadPhasePair);
-    */
 }
 
 // Remove a body from the collision detection

src/collision/CollisionDetection.h

@@ -121,7 +121,7 @@ class CollisionDetection {
         ~CollisionDetection();
 
         /// Add a proxy collision shape to the collision detection
-        void addProxyCollisionShape(ProxyShape* proxyShape);
+        void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb);
 
         /// Remove a proxy collision shape from the collision detection
         void removeProxyCollisionShape(ProxyShape* proxyShape);
@@ -162,10 +162,11 @@ inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm(
 }  
 
 // Add a body to the collision detection
-inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape) {
+inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape,
+                                                       const AABB& aabb) {
     
     // Add the body to the broad-phase
-    mBroadPhaseAlgorithm.addProxyCollisionShape(proxyShape);
+    mBroadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb);
 
     mIsCollisionShapesAdded = true;
 }  

src/collision/broadphase/BroadPhaseAlgorithm.cpp

@@ -38,11 +38,11 @@ BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
 
     // Allocate memory for the array of non-static bodies IDs
     mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
-    assert(mMovedShapes);
+    assert(mMovedShapes != NULL);
 
     // Allocate memory for the array of potential overlapping pairs
     mPotentialPairs = (BroadPair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPair));
-    assert(mPotentialPairs);
+    assert(mPotentialPairs != NULL);
 }
 
 // Destructor
@@ -64,12 +64,14 @@ void BroadPhaseAlgorithm::addMovedCollisionShape(int broadPhaseID) {
         mNbAllocatedMovedShapes *= 2;
         int* oldArray = mMovedShapes;
         mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
-        assert(mMovedShapes);
+        assert(mMovedShapes != NULL);
         memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int));
         free(oldArray);
     }
 
     // Store the broad-phase ID into the array of bodies that have moved
+    assert(mNbMovedShapes < mNbAllocatedMovedShapes);
+    assert(mMovedShapes != NULL);
     mMovedShapes[mNbMovedShapes] = broadPhaseID;
     mNbMovedShapes++;
 }
@@ -88,7 +90,7 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
         mNbAllocatedMovedShapes /= 2;
         int* oldArray = mMovedShapes;
         mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
-        assert(mMovedShapes);
+        assert(mMovedShapes != NULL);
         uint nbElements = 0;
         for (uint i=0; i<mNbMovedShapes; i++) {
             if (oldArray[i] != -1) {
@@ -112,10 +114,10 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
 }
 
 // Add a proxy collision shape into the broad-phase collision detection
-void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape) {
+void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {
 
     // Add the collision shape into the dynamic AABB tree and get its broad-phase ID
-    mDynamicAABBTree.addObject(proxyShape);
+    mDynamicAABBTree.addObject(proxyShape, aabb);
 
     // Add the collision shape into the array of bodies that have moved (or have been created)
     // during the last simulation step
@@ -179,7 +181,7 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
 
     // Reset the array of collision shapes that have move (or have been created) during the
     // last simulation step
-    mMovedShapes = 0;
+    mNbMovedShapes = 0;
 
     // Sort the array of potential overlapping pairs in order to remove duplicate pairs
     std::sort(mPotentialPairs, mPotentialPairs + mNbPotentialPairs, BroadPair::smallerThan);

src/collision/broadphase/BroadPhaseAlgorithm.h

@@ -131,7 +131,7 @@ class BroadPhaseAlgorithm {
         ~BroadPhaseAlgorithm();
         
         /// Add a proxy collision shape into the broad-phase collision detection
-        void addProxyCollisionShape(ProxyShape* proxyShape);
+        void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb);
 
         /// Remove a proxy collision shape from the broad-phase collision detection
         void removeProxyCollisionShape(ProxyShape* proxyShape);
@@ -152,6 +152,9 @@ class BroadPhaseAlgorithm {
 
         /// Compute all the overlapping pairs of collision shapes
         void computeOverlappingPairs();
+
+        /// Return true if the two broad-phase collision shapes are overlapping
+        bool testOverlappingShapes(ProxyShape* shape1, ProxyShape* shape2) const;
 };
 
 // Method used to compare two pairs for sorting algorithm
@@ -164,6 +167,17 @@ inline bool BroadPair::smallerThan(const BroadPair& pair1, const BroadPair& pair
     return false;
 }
 
+// Return true if the two broad-phase collision shapes are overlapping
+inline bool BroadPhaseAlgorithm::testOverlappingShapes(ProxyShape* shape1,
+                                                       ProxyShape* shape2) const {
+    // Get the two AABBs of the collision shapes
+    const AABB& aabb1 = mDynamicAABBTree.getFatAABB(shape1->mBroadPhaseID);
+    const AABB& aabb2 = mDynamicAABBTree.getFatAABB(shape2->mBroadPhaseID);
+
+    // Check if the two AABBs are overlapping
+    return aabb1.testCollision(aabb2);
+}
+
 }
 
 #endif

src/collision/broadphase/DynamicAABBTree.cpp

@@ -106,45 +106,24 @@ void DynamicAABBTree::releaseNode(int nodeID) {
 
     assert(mNbNodes > 0);
     assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
+    assert(mNodes[nodeID].height >= 0);
     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.
-void DynamicAABBTree::addObject(ProxyShape* proxyShape) {
+void DynamicAABBTree::addObject(ProxyShape* proxyShape, 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);
+    mNodes[nodeID].aabb.setMin(aabb.getMin() - gap);
+    mNodes[nodeID].aabb.setMax(aabb.getMax() + gap);
 
     // Set the collision shape
     mNodes[nodeID].proxyShape = proxyShape;
@@ -154,6 +133,7 @@ void DynamicAABBTree::addObject(ProxyShape* proxyShape) {
 
     // Insert the new leaf node in the tree
     insertLeafNode(nodeID);
+    assert(mNodes[nodeID].isLeaf());
 
     // Set the broad-phase ID of the proxy shape
     proxyShape->mBroadPhaseID = nodeID;
@@ -179,6 +159,7 @@ bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB) {
 
     assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
     assert(mNodes[nodeID].isLeaf());
+    assert(mNodes[nodeID].height >= 0);
 
     // If the new AABB is still inside the fat AABB of the node
     if (mNodes[nodeID].aabb.contains(newAABB)) {
@@ -212,6 +193,8 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
         return;
     }
 
+    assert(mRootNodeID != TreeNode::NULL_TREE_NODE);
+
     // Find the best sibling node for the new node
     AABB newNodeAABB = mNodes[nodeID].aabb;
     int currentNodeID = mRootNodeID;
@@ -278,6 +261,7 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
     mNodes[newParentNode].proxyShape = NULL;
     mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB);
     mNodes[newParentNode].height = mNodes[siblingNode].height + 1;
+    assert(mNodes[newParentNode].height > 0);
 
     // If the sibling node was not the root node
     if (oldParentNode != TreeNode::NULL_TREE_NODE) {
@@ -287,6 +271,10 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
         else {
             mNodes[oldParentNode].rightChildID = newParentNode;
         }
+        mNodes[newParentNode].leftChildID = siblingNode;
+        mNodes[newParentNode].rightChildID = nodeID;
+        mNodes[siblingNode].parentID = newParentNode;
+        mNodes[nodeID].parentID = newParentNode;
     }
     else {  // If the sibling node was the root node
         mNodes[newParentNode].leftChildID = siblingNode;
@@ -302,6 +290,7 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
 
         // Balance the sub-tree of the current node if it is not balanced
         currentNodeID = balanceSubTreeAtNode(currentNodeID);
+        assert(mNodes[nodeID].isLeaf());
 
         int leftChild = mNodes[currentNodeID].leftChildID;
         int rightChild = mNodes[currentNodeID].rightChildID;
@@ -311,18 +300,25 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
         // Recompute the height of the node in the tree
         mNodes[currentNodeID].height = std::max(mNodes[leftChild].height,
                                                 mNodes[rightChild].height) + 1;
+        assert(mNodes[currentNodeID].height > 0);
 
         // Recompute the AABB of the node
         mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
 
         currentNodeID = mNodes[currentNodeID].parentID;
     }
+
+    assert(mNodes[nodeID].isLeaf());
+
+    // Check the structure of the tree
+    check();
 }
 
 // Remove a leaf node from the tree
 void DynamicAABBTree::removeLeafNode(int nodeID) {
 
     assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
+    assert(mNodes[nodeID].isLeaf());
 
     // If we are removing the root node (root node is a leaf in this case)
     if (mRootNodeID == nodeID) {
@@ -371,18 +367,21 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
                                                      mNodes[rightChildID].aabb);
             mNodes[currentNodeID].height = std::max(mNodes[leftChildID].height,
                                                     mNodes[rightChildID].height) + 1;
+            assert(mNodes[currentNodeID].height > 0);
 
             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);
+        releaseNode(parentNodeID);
     }
+
+    // Check the structure of the tree
+    check();
 }
 
 // Balance the sub-tree of a given node using left or right rotations.
@@ -453,6 +452,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
             // 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;
+            assert(nodeA->height > 0);
+            assert(nodeC->height > 0);
         }
         else {  // If the right node C was higher than left node B because of node G
             nodeC->rightChildID = nodeGID;
@@ -466,6 +467,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
             // 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;
+            assert(nodeA->height > 0);
+            assert(nodeC->height > 0);
         }
 
         // Return the new root of the sub-tree
@@ -513,6 +516,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
             // 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;
+            assert(nodeA->height > 0);
+            assert(nodeB->height > 0);
         }
         else {  // If the left node B was higher than right node C because of node G
             nodeB->rightChildID = nodeGID;
@@ -526,6 +531,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
             // 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;
+            assert(nodeA->height > 0);
+            assert(nodeB->height > 0);
         }
 
         // Return the new root of the sub-tree
@@ -576,3 +583,74 @@ void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aab
         }
     }
 }
+
+// Check if the tree structure is valid (for debugging purpose)
+void DynamicAABBTree::check() const {
+
+    // Recursively check each node
+    checkNode(mRootNodeID);
+
+    int nbFreeNodes = 0;
+    int freeNodeID = mFreeNodeID;
+
+    // Check the free nodes
+    while(freeNodeID != TreeNode::NULL_TREE_NODE) {
+        assert(0 <= freeNodeID && freeNodeID < mNbAllocatedNodes);
+        freeNodeID = mNodes[freeNodeID].nextNodeID;
+        nbFreeNodes++;
+    }
+
+    assert(mNbNodes + nbFreeNodes == mNbAllocatedNodes);
+}
+
+// Check if the node structure is valid (for debugging purpose)
+void DynamicAABBTree::checkNode(int nodeID) const {
+
+    if (nodeID == TreeNode::NULL_TREE_NODE) return;
+
+    // If it is the root
+    if (nodeID == mRootNodeID) {
+        assert(mNodes[nodeID].parentID == TreeNode::NULL_TREE_NODE);
+    }
+
+    // Get the children nodes
+    TreeNode* pNode = mNodes + nodeID;
+    int leftChild = pNode->leftChildID;
+    int rightChild = pNode->rightChildID;
+
+    assert(pNode->height >= 0);
+    assert(pNode->aabb.getVolume() > 0);
+
+    // If the current node is a leaf
+    if (pNode->isLeaf()) {
+
+        // Check that there are no children
+        assert(leftChild == TreeNode::NULL_TREE_NODE);
+        assert(rightChild == TreeNode::NULL_TREE_NODE);
+        assert(pNode->height == 0);
+    }
+    else {
+
+        // Check that the children node IDs are valid
+        assert(0 <= leftChild && leftChild < mNbAllocatedNodes);
+        assert(0 <= rightChild && rightChild < mNbAllocatedNodes);
+
+        // Check that the children nodes have the correct parent node
+        assert(mNodes[leftChild].parentID == nodeID);
+        assert(mNodes[rightChild].parentID == nodeID);
+
+        // Check the height of node
+        int height = 1 + std::max(mNodes[leftChild].height, mNodes[rightChild].height);
+        assert(mNodes[nodeID].height == height);
+
+        // Check the AABB of the node
+        AABB aabb;
+        aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
+        assert(aabb.getMin() == mNodes[nodeID].aabb.getMin());
+        assert(aabb.getMax() == mNodes[nodeID].aabb.getMax());
+
+        // Recursively check the children nodes
+        checkNode(leftChild);
+        checkNode(rightChild);
+    }
+}

src/collision/broadphase/DynamicAABBTree.h

@@ -123,6 +123,12 @@ class DynamicAABBTree {
         /// Balance the sub-tree of a given node using left or right rotations.
         int balanceSubTreeAtNode(int nodeID);
 
+        /// Check if the tree structure is valid (for debugging purpose)
+        void check() const;
+
+        /// Check if the node structure is valid (for debugging purpose)
+        void checkNode(int nodeID) const;
+
     public:
 
         // -------------------- Methods -------------------- //
@@ -134,7 +140,7 @@ class DynamicAABBTree {
         ~DynamicAABBTree();
 
         /// Add an object into the tree
-        void addObject(ProxyShape* proxyShape);
+        void addObject(ProxyShape* proxyShape, const AABB& aabb);
 
         /// Remove an object from the tree
         void removeObject(int nodeID);