continue to replace SAP broad-phase by a dynamic AABB tree

CMakeLists.txt

@@ -43,10 +43,6 @@ SET (REACTPHYSICS3D_SOURCES
     "src/body/RigidBody.cpp"
     "src/collision/broadphase/BroadPhaseAlgorithm.h"
     "src/collision/broadphase/BroadPhaseAlgorithm.cpp"
-    "src/collision/broadphase/NoBroadPhaseAlgorithm.h"
-    "src/collision/broadphase/NoBroadPhaseAlgorithm.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"

examples/common/Box.cpp

@@ -85,8 +85,11 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
     rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
     rp3d::Transform transform(initPosition, initOrientation);
 
-    // Create a rigid body corresponding to the cube in the dynamics world
+    // Create a rigid body in the dynamics world
-    mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
+    mRigidBody = dynamicsWorld->createRigidBody(transform);
+
+    // Add the collision shape to the body
+    mRigidBody->addCollisionShape(collisionShape, mass);
 
     // If the Vertex Buffer object has not been created yet
     if (!areVBOsCreated) {

examples/common/Capsule.cpp

@@ -58,8 +58,11 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
     rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
     rp3d::Transform transform(initPosition, initOrientation);
 
-    // Create a rigid body corresponding to the sphere in the dynamics world
+    // Create a rigid body corresponding in the dynamics world
-    mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
+    mRigidBody = dynamicsWorld->createRigidBody(transform);
+
+    // Add a collision shape to the body and specify the mass of the shape
+    mRigidBody->addCollisionShape(collisionShape, mass);
 }
 
 // Destructor

examples/common/Cone.cpp

@@ -59,7 +59,10 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
     rp3d::Transform transform(initPosition, initOrientation);
 
     // Create a rigid body corresponding to the cone in the dynamics world
-    mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
+    mRigidBody = dynamicsWorld->createRigidBody(transform);
+
+    // Add a collision shape to the body and specify the mass of the shape
+    mRigidBody->addCollisionShape(collisionShape, mass);
 }
 
 // Destructor

examples/common/ConvexMesh.cpp

@@ -72,7 +72,10 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
     rp3d::Transform transform(initPosition, initOrientation);
 
     // Create a rigid body corresponding to the sphere in the dynamics world
-    mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
+    mRigidBody = dynamicsWorld->createRigidBody(transform);
+
+    // Add a collision shape to the body and specify the mass of the collision shape
+    mRigidBody->addCollisionShape(collisionShape, mass);
 }
 
 // Destructor

examples/common/Cylinder.cpp

@@ -59,7 +59,10 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3 &p
     rp3d::Transform transform(initPosition, initOrientation);
 
     // Create a rigid body corresponding to the cylinder in the dynamics world
-    mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
+    mRigidBody = dynamicsWorld->createRigidBody(transform);
+
+    // Add a collision shape to the body and specify the mass of the shape
+    mRigidBody->addCollisionShape(collisionShape, mass);
 }
 
 // Destructor

examples/common/Sphere.cpp

@@ -59,7 +59,10 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position,
     rp3d::Transform transform(initPosition, initOrientation);
 
     // Create a rigid body corresponding to the sphere in the dynamics world
-    mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
+    mRigidBody = dynamicsWorld->createRigidBody(transform);
+
+    // Add a collision shape to the body and specify the mass of the shape
+    mRigidBody->addCollisionShape(collisionShape, mass);
 }
 
 // Destructor

src/body/CollisionBody.cpp

@@ -62,7 +62,7 @@ CollisionBody::~CollisionBody() {
 /// This method will return a pointer to the proxy collision shape that links the body with
 /// the collision shape you have added.
 const ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisionShape,
-                                                       const Transform& transform) {
+                                                   const Transform& transform) {
 
     // Create an internal copy of the collision shape into the world (if it does not exist yet)
     CollisionShape* newCollisionShape = mWorld.createCollisionShape(collisionShape);
@@ -90,14 +90,15 @@ const ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisi
 }
 
 // Remove a collision shape from the body
-void CollisionBody::removeCollisionShape(const CollisionShape* collisionShape) {
+void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
 
     ProxyShape* current = mProxyCollisionShapes;
 
     // If the the first proxy shape is the one to remove
-    if (current->getCollisionShape() == collisionShape) {
+    if (current == proxyShape) {
         mProxyCollisionShapes = current->mNext;
         mWorld.mCollisionDetection.removeProxyCollisionShape(current);
+        mWorld.removeCollisionShape(proxyShape->getInternalCollisionShape());
         size_t sizeBytes = current->getSizeInBytes();
         current->ProxyShape::~ProxyShape();
         mWorld.mMemoryAllocator.release(current, sizeBytes);
@@ -109,12 +110,13 @@ void CollisionBody::removeCollisionShape(const CollisionShape* collisionShape) {
     while(current->mNext != NULL) {
 
         // If we have found the collision shape to remove
-        if (current->mNext->getCollisionShape() == collisionShape) {
+        if (current->mNext == proxyShape) {
 
             // Remove the proxy collision shape
             ProxyShape* elementToRemove = current->mNext;
             current->mNext = elementToRemove->mNext;
             mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
+            mWorld.removeCollisionShape(proxyShape->getInternalCollisionShape());
             size_t sizeBytes = elementToRemove->getSizeInBytes();
             elementToRemove->ProxyShape::~ProxyShape();
             mWorld.mMemoryAllocator.release(elementToRemove, sizeBytes);
@@ -140,6 +142,7 @@ void CollisionBody::removeAllCollisionShapes() {
         // Remove the proxy collision shape
         ProxyShape* nextElement = current->mNext;
         mWorld.mCollisionDetection.removeProxyCollisionShape(current);
+        mWorld.removeCollisionShape(current->getInternalCollisionShape());
         current->ProxyShape::~ProxyShape();
         mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
 

src/body/CollisionBody.h

@@ -137,10 +137,10 @@ class CollisionBody : public Body {
 
         /// Add a collision shape to the body.
         const ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
-                                                const Transform& transform = Transform::identity());
+                                            const Transform& transform = Transform::identity());
 
         /// Remove a collision shape from the body
-        virtual void removeCollisionShape(const CollisionShape* collisionShape);
+        virtual void removeCollisionShape(const ProxyShape* proxyShape);
 
         /// Return the interpolated transform for rendering
         Transform getInterpolatedTransform() const;
@@ -159,6 +159,7 @@ class CollisionBody : public Body {
 
         // -------------------- Friendship -------------------- //
 
+        friend class CollisionWorld;
         friend class DynamicsWorld;
         friend class CollisionDetection;
         friend class BroadPhaseAlgorithm;

src/body/RigidBody.cpp

@@ -130,15 +130,14 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
 /// return a pointer to the actual collision shape in the world. You can use this pointer to
 /// remove the collision from the body. Note that when the body is destroyed, all the collision
 /// shapes will also be destroyed automatically. Because an internal copy of the collision shape
-/// you provided is performed, you can delete it right after calling this method. The second
+/// you provided is performed, you can delete it right after calling this method.
+/// The second parameter is the mass of the collision shape (this will used to compute the
+/// total mass of the rigid body and its inertia tensor). The mass must be positive. The third
 /// 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.
-/// The third parameter is the mass of the collision shape (this will used to compute the
+const ProxyShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape,
-/// total mass of the rigid body and its inertia tensor). The mass must be positive.
+                                               decimal mass,
-const CollisionShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape,
+                                               const Transform& transform) {
-                                                   decimal mass,
-                                                   const Transform& transform
-                                                   ) {
 
     assert(mass > decimal(0.0));
 
@@ -167,15 +166,15 @@ const CollisionShape* RigidBody::addCollisionShape(const CollisionShape& collisi
     // collision shape
     recomputeMassInformation();
 
-    // Return a pointer to the collision shape
+    // Return a pointer to the proxy collision shape
-    return newCollisionShape;
+    return proxyShape;
 }
 
 // Remove a collision shape from the body
-void RigidBody::removeCollisionShape(const CollisionShape* collisionShape) {
+void RigidBody::removeCollisionShape(const ProxyShape* proxyCollisionShape) {
 
     // Remove the collision shape
-    CollisionBody::removeCollisionShape(collisionShape);
+    CollisionBody::removeCollisionShape(proxyCollisionShape);
 
     // Recompute the total mass, center of mass and inertia tensor
     recomputeMassInformation();

src/body/RigidBody.h

@@ -195,12 +195,12 @@ class RigidBody : public CollisionBody {
         void applyTorque(const Vector3& torque);
 
         /// Add a collision shape to the body.
-        const CollisionShape* addCollisionShape(const CollisionShape& collisionShape,
+        const ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
-                                                decimal mass,
+                                            decimal mass,
-                                                const Transform& transform = Transform::identity());
+                                            const Transform& transform = Transform::identity());
 
         /// Remove a collision shape from the body
-        virtual void removeCollisionShape(const CollisionShape* collisionShape);
+        virtual void removeCollisionShape(const ProxyShape* proxyCollisionShape);
 
         /// Recompute the center of mass, total mass and inertia tensor of the body using all
         /// the collision shapes attached to the body.

src/collision/CollisionDetection.cpp

@@ -26,8 +26,6 @@
 // Libraries
 #include "CollisionDetection.h"
 #include "../engine/CollisionWorld.h"
-#include "broadphase/SweepAndPruneAlgorithm.h"
-#include "broadphase/NoBroadPhaseAlgorithm.h"
 #include "../body/Body.h"
 #include "../collision/shapes/BoxShape.h"
 #include "../body/RigidBody.h"
@@ -101,7 +99,7 @@ void CollisionDetection::computeNarrowPhase() {
         CollisionBody* const body2 = shape2->getBody();
         
         // Update the contact cache of the overlapping pair
-        mWorld->updateOverlappingPair(pair);
+        pair->update();
 
         // Check if the two bodies are allowed to collide, otherwise, we do not test for collision
         if (body1->getType() != DYNAMIC && body2->getType() != DYNAMIC) continue;
@@ -123,9 +121,7 @@ void CollisionDetection::computeNarrowPhase() {
         // if there really is a collision
         const Transform transform1 = body1->getTransform() * shape1->getLocalToBodyTransform();
         const Transform transform2 = body2->getTransform() * shape2->getLocalToBodyTransform();
-        if (narrowPhaseAlgorithm.testCollision(shape1->getCollisionShape(), transform1,
+        if (narrowPhaseAlgorithm.testCollision(shape1, shape2, contactInfo)) {
-                                               shape2->getCollisionShape(), transform2,
-                                               contactInfo)) {
             assert(contactInfo != NULL);
 
             // Set the bodies of the contact
@@ -134,8 +130,8 @@ void CollisionDetection::computeNarrowPhase() {
             assert(contactInfo->body1 != NULL);
             assert(contactInfo->body2 != NULL);
 
-            // Notify the world about the new narrow-phase contact
+            // Create a new contact
-            mWorld->notifyNewContact(pair, contactInfo);
+            createContact(pair, contactInfo);
 
             // Delete and remove the contact info from the memory allocator
             contactInfo->ContactPointInfo::~ContactPointInfo();
@@ -185,37 +181,20 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
     */
 }
 
-// Allow the broadphase to notify the collision detection about a removed overlapping pair
-void CollisionDetection::removeOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
-
-    // Compute the overlapping pair ID
-    overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2);
-
-    // If the overlapping
-    std::map<overlappingpairid, OverlappingPair*>::iterator it;
-    it = mOverlappingPairs.find(indexPair);
-    if ()
-
-    // Notify the world about the removed broad-phase pair
-    // TODO : DELETE THIS
-    //mWorld->notifyRemovedOverlappingPair(broadPhasePair);
-
-    // Remove the overlapping pair from the memory allocator
-    mOverlappingPairs.find(pairID)->second->OverlappingPair::~OverlappingPair();
-    mWorld->mMemoryAllocator.release(mOverlappingPairs[indexPair], sizeof(OverlappingPair));
-    mOverlappingPairs.erase(pairID);
-}
-
 // Remove a body from the collision detection
 void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
 
     // Remove all the overlapping pairs involving this proxy shape
     std::map<overlappingpairid, OverlappingPair*>::iterator it;
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
-        if (it->second->getShape1()->getBroadPhaseID() == proxyShape->getBroadPhaseID() ||
+        if (it->second->getShape1()->mBroadPhaseID == proxyShape->mBroadPhaseID||
-            it->second->getShape2()->getBroadPhaseID() == proxyShape->getBroadPhaseID()) {
+            it->second->getShape2()->mBroadPhaseID == proxyShape->mBroadPhaseID) {
             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);
         }
         else {
@@ -226,3 +205,58 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
     // Remove the body from the broad-phase
     mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape);
 }
+
+// Create a new contact
+void CollisionDetection::createContact(OverlappingPair* overlappingPair,
+                                       const ContactPointInfo* contactInfo) {
+
+    // Create a new contact
+    ContactPoint* contact = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPoint)))
+                                ContactPoint(*contactInfo);
+    assert(contact != NULL);
+
+    // If it is the first contact since the pair are overlapping
+    if (overlappingPair->getNbContactPoints() == 0) {
+
+        // Trigger a callback event
+        if (mWorld->mEventListener != NULL) mWorld->mEventListener->beginContact(*contactInfo);
+    }
+
+    // Add the contact to the contact cache of the corresponding overlapping pair
+    overlappingPair->addContact(contact);
+
+    // Add the contact manifold to the list of contact manifolds
+    mContactManifolds.push_back(overlappingPair->getContactManifold());
+
+    // Add the contact manifold into the list of contact manifolds
+    // of the two bodies involved in the contact
+    addContactManifoldToBody(overlappingPair->getContactManifold(),
+                             overlappingPair->getShape1()->getBody(),
+                             overlappingPair->getShape2()->getBody());
+
+    // Trigger a callback event for the new contact
+    if (mWorld->mEventListener != NULL) mWorld->mEventListener->newContact(*contactInfo);
+}
+
+// Add a contact manifold to the linked list of contact manifolds of the two bodies involved
+// in the corresponding contact
+void CollisionDetection::addContactManifoldToBody(ContactManifold* contactManifold,
+                                                  CollisionBody* body1, CollisionBody* body2) {
+
+    assert(contactManifold != NULL);
+
+    // Add the contact manifold at the beginning of the linked
+    // list of contact manifolds of the first body
+    void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
+    ContactManifoldListElement* listElement1 = new (allocatedMemory1)
+                                                  ContactManifoldListElement(contactManifold,
+                                                                     body1->mContactManifoldsList);
+    body1->mContactManifoldsList = listElement1;
+
+    // Add the joint at the beginning of the linked list of joints of the second body
+    void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
+    ContactManifoldListElement* listElement2 = new (allocatedMemory2)
+                                                  ContactManifoldListElement(contactManifold,
+                                                                     body2->mContactManifoldsList);
+    body2->mContactManifoldsList = listElement2;
+}

src/collision/CollisionDetection.h

@@ -80,6 +80,10 @@ class CollisionDetection {
         /// True if some collision shapes have been added previously
         bool mIsCollisionShapesAdded;
 
+        /// All the contact constraints
+        // TODO : Remove this variable (we will use the ones in the island now)
+        std::vector<ContactManifold*> mContactManifolds;
+
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -98,8 +102,13 @@ class CollisionDetection {
         NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(const CollisionShape* collisionShape1,
                                                          const CollisionShape* collisionShape2);
 
-        /// Remove an overlapping pair if it is not overlapping anymore
+        /// Create a new contact
-        void removeOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
+        void createContact(OverlappingPair* overlappingPair, const ContactPointInfo* contactInfo);
+
+        /// Add a contact manifold to the linked list of contact manifolds of the two bodies
+        /// involed in the corresponding contact.
+        void addContactManifoldToBody(ContactManifold* contactManifold,
+                                      CollisionBody *body1, CollisionBody *body2);
    
     public :
 
@@ -131,6 +140,11 @@ class CollisionDetection {
 
         /// Allow the broadphase to notify the collision detection about an overlapping pair.
         void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
+
+        // -------------------- Friendship -------------------- //
+
+        // TODO : REMOVE THIS
+        friend class DynamicsWorld;
 };
 
 // Select the narrow-phase collision algorithm to use given two collision shapes

src/collision/broadphase/BroadPhaseAlgorithm.cpp

@@ -25,6 +25,7 @@
 
 // Libraries
 #include "BroadPhaseAlgorithm.h"
+#include "../CollisionDetection.h"
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
@@ -33,7 +34,7 @@ using namespace reactphysics3d;
 BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
                     :mDynamicAABBTree(*this), mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
                      mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8),
-                     mPairManager(collisionDetection), mCollisionDetection(collisionDetection) {
+                     mCollisionDetection(collisionDetection) {
 
     // Allocate memory for the array of non-static bodies IDs
     mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));

src/collision/broadphase/BroadPhaseAlgorithm.h

@@ -29,7 +29,6 @@
 // Libraries
 #include <vector>
 #include "../../body/CollisionBody.h"
-#include "PairManager.h"
 #include "DynamicAABBTree.h"
 
 /// Namespace ReactPhysics3D
@@ -110,9 +109,6 @@ class BroadPhaseAlgorithm {
         /// Number of allocated elements for the array of potential overlapping pairs
         uint mNbAllocatedPotentialPairs;
 
-        /// Pair manager containing the overlapping pairs
-        PairManager mPairManager;
-
         /// Reference to the collision detection object
         CollisionDetection& mCollisionDetection;
         
@@ -156,14 +152,6 @@ class BroadPhaseAlgorithm {
 
         /// Compute all the overlapping pairs of collision shapes
         void computeOverlappingPairs();
-
-        /// Return a pointer to the first active pair (used to iterate over the active pairs)
-        // TODO : DELETE THIS
-        BodyPair* beginOverlappingPairsPointer() const;
-
-        /// Return a pointer to the last active pair (used to iterate over the active pairs)
-        // TODO : DELETE THIS
-        BodyPair* endOverlappingPairsPointer() const;
 };
 
 // Method used to compare two pairs for sorting algorithm
@@ -176,16 +164,6 @@ inline bool BroadPair::smallerThan(const BroadPair& pair1, const BroadPair& pair
     return false;
 }
 
-// Return a pointer to the first active pair (used to iterate over the overlapping pairs)
-inline BodyPair* BroadPhaseAlgorithm::beginOverlappingPairsPointer() const {
-    return mPairManager.beginOverlappingPairsPointer();
-}                                                           
-
-// Return a pointer to the last active pair (used to iterate over the overlapping pairs)
-inline BodyPair* BroadPhaseAlgorithm::endOverlappingPairsPointer() const {
-   return mPairManager.endOverlappingPairsPointer();
-}   
-
 }
 
 #endif

src/collision/broadphase/NoBroadPhaseAlgorithm.cpp

@@ -1,43 +0,0 @@
-/********************************************************************************
-* 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 "NoBroadPhaseAlgorithm.h"
-#include "../CollisionDetection.h"
-
-// We want to use the ReactPhysics3D namespace
-using namespace reactphysics3d;
-using namespace std;
-
-// Constructor
-NoBroadPhaseAlgorithm::NoBroadPhaseAlgorithm(CollisionDetection& collisionDetection)
-                      : BroadPhaseAlgorithm(collisionDetection) {
-
-}
-
-// Destructor
-NoBroadPhaseAlgorithm::~NoBroadPhaseAlgorithm() {
-
-}

src/collision/broadphase/NoBroadPhaseAlgorithm.h

@@ -1,123 +0,0 @@
-/********************************************************************************
-* 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.    *
-*                                                                               *
-********************************************************************************/
-
-#ifndef REACTPHYSICS3D_NO_BROAD_PHASE_ALGORITHM_H
-#define REACTPHYSICS3D_NO_BROAD_PHASE_ALGORITHM_H
-
-// Libraries
-#include "BroadPhaseAlgorithm.h"
-#include <algorithm>
-#include <set>
-#include <iostream>
-
-/// Namespace ReactPhysics3D
-namespace reactphysics3d {
-
-// Class NoBroadPhaseAlgorithm
-/**
- * This class implements a broad-phase algorithm that does nothing.
- * It should be use if we don't want to perform a broad-phase for
- * the collision detection.
- */
-class NoBroadPhaseAlgorithm : public BroadPhaseAlgorithm {
-
-    protected :
-
-        // -------------------- Attributes -------------------- //
-
-        /// All bodies of the world
-        std::set<CollisionBody*> mBodies;
-
-        // -------------------- Methods -------------------- //
-
-        /// Private copy-constructor
-        NoBroadPhaseAlgorithm(const NoBroadPhaseAlgorithm& algorithm);
-
-        /// Private assignment operator
-        NoBroadPhaseAlgorithm& operator=(const NoBroadPhaseAlgorithm& algorithm);
-
-    public :
-
-        // -------------------- Methods -------------------- //
-
-        /// Constructor
-        NoBroadPhaseAlgorithm(CollisionDetection& collisionDetection);
-
-        /// Destructor
-        virtual ~NoBroadPhaseAlgorithm();
-        
-        /// Notify the broad-phase about a new object in the world
-        virtual void addProxyCollisionShape(CollisionBody* body, const AABB& aabb);
-
-        /// Notify the broad-phase about an object that has been removed from the world
-        virtual void removeProxyCollisionShape(CollisionBody* body);
-
-        /// Notify the broad-phase that the AABB of an object has changed
-        virtual void updateProxyCollisionShape(CollisionBody* body, const AABB& aabb);
-};
-
-        
-// Notify the broad-phase about a new object in the world
-inline void NoBroadPhaseAlgorithm::addProxyCollisionShape(CollisionBody* body, const AABB& aabb) {
-        
-    // For each body that is already in the world
-    for (std::set<CollisionBody*>::iterator it = mBodies.begin(); it != mBodies.end(); ++it) {
-        
-        // Add an overlapping pair with the new body
-        mPairManager.addPair(*it, body);
-    }
-    
-    // Add the new body into the list of bodies
-    mBodies.insert(body);
-}   
-
-// Notify the broad-phase about an object that has been removed from the world
-inline void NoBroadPhaseAlgorithm::removeProxyCollisionShape(CollisionBody* body) {
-    
-    // For each body that is in the world
-    for (std::set<CollisionBody*>::iterator it = mBodies.begin(); it != mBodies.end(); ++it) {
-        
-        if ((*it)->getID() != body->getID()) {
-            
-           // Remove the overlapping pair with the new body
-           mPairManager.removePair((*it)->getID(), body->getID());
-        }
-    }
-    
-    // Remove the body from the broad-phase
-    mBodies.erase(body);
-}  
-        
-// Notify the broad-phase that the AABB of an object has changed
-inline void NoBroadPhaseAlgorithm::updateProxyCollisionShape(CollisionBody* body, const AABB& aabb) {
-    // Do nothing
-    return;
-}     
-
-}
-
-#endif
-
-

src/collision/broadphase/SweepAndPruneAlgorithm.cpp

@@ -1,596 +0,0 @@
-/********************************************************************************
-* 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 "SweepAndPruneAlgorithm.h"
-#include "../CollisionDetection.h"
-#include "PairManager.h"
-#include <climits>
-#include <cstring>
-
-// Namespaces
-using namespace reactphysics3d;
-using namespace std;
-
-// Initialization of static variables
-const bodyindex SweepAndPruneAlgorithm::INVALID_INDEX = std::numeric_limits<bodyindex>::max();
-const luint SweepAndPruneAlgorithm::NB_SENTINELS = 2;
-
-// Constructor that takes an AABB as input
-AABBInt::AABBInt(const AABB& aabb) {
-    min[0] = encodeFloatIntoInteger(aabb.getMin().x);
-    min[1] = encodeFloatIntoInteger(aabb.getMin().y);
-    min[2] = encodeFloatIntoInteger(aabb.getMin().z);
-
-    max[0] = encodeFloatIntoInteger(aabb.getMax().x);
-    max[1] = encodeFloatIntoInteger(aabb.getMax().y);
-    max[2] = encodeFloatIntoInteger(aabb.getMax().z);
-}
-
-// Constructor that set all the axis with an minimum and maximum value
-AABBInt::AABBInt(uint minValue, uint maxValue) {
-    min[0] = minValue;
-    min[1] = minValue;
-    min[2] = minValue;
-
-    max[0] = maxValue;
-    max[1] = maxValue;
-    max[2] = maxValue;
-}
-
-// Constructor
-SweepAndPruneAlgorithm::SweepAndPruneAlgorithm(CollisionDetection& collisionDetection)
-             :BroadPhaseAlgorithm(collisionDetection) {
-    mBoxes = NULL;
-    mEndPoints[0] = NULL;
-    mEndPoints[1] = NULL;
-    mEndPoints[2] = NULL;
-    mNbBoxes = 0;
-    mNbMaxBoxes = 0;
-}
-
-// Destructor
-SweepAndPruneAlgorithm::~SweepAndPruneAlgorithm() {
-    delete[] mBoxes;
-    delete[] mEndPoints[0];
-    delete[] mEndPoints[1];
-    delete[] mEndPoints[2];
-}
-
-// Notify the broad-phase about a new object in the world
-/// This method adds the AABB of the object ion to broad-phase
-void SweepAndPruneAlgorithm::addProxyCollisionShape(CollisionBody* body, const AABB& aabb) {
-
-    bodyindex boxIndex;
-
-    assert(encodeFloatIntoInteger(aabb.getMin().x) > encodeFloatIntoInteger(-FLT_MAX));
-    assert(encodeFloatIntoInteger(aabb.getMin().y) > encodeFloatIntoInteger(-FLT_MAX));
-    assert(encodeFloatIntoInteger(aabb.getMin().z) > encodeFloatIntoInteger(-FLT_MAX));
-    assert(encodeFloatIntoInteger(aabb.getMax().x) < encodeFloatIntoInteger(FLT_MAX));
-    assert(encodeFloatIntoInteger(aabb.getMax().y) < encodeFloatIntoInteger(FLT_MAX));
-    assert(encodeFloatIntoInteger(aabb.getMax().z) < encodeFloatIntoInteger(FLT_MAX));
-
-    // If the index of the first free box is valid (means that
-    // there is a bucket in the middle of the array that doesn't
-    // contain a box anymore because it has been removed)
-    if (!mFreeBoxIndices.empty()) {
-        boxIndex = mFreeBoxIndices.back();
-        mFreeBoxIndices.pop_back();
-    }
-    else {
-        // If the array boxes and end-points arrays are full
-        if (mNbBoxes == mNbMaxBoxes) {
-            // Resize the arrays to make them larger
-            resizeArrays();
-        }
-        
-        boxIndex = mNbBoxes;
-    }
-    
-    // Move the maximum limit end-point two elements further
-    // at the end-points array in all three axis
-    const luint indexLimitEndPoint = 2 * mNbBoxes + NB_SENTINELS - 1;
-    for (uint axis=0; axis<3; axis++) {
-        EndPoint* maxLimitEndPoint = &mEndPoints[axis][indexLimitEndPoint];
-        assert(mEndPoints[axis][0].boxID == INVALID_INDEX && mEndPoints[axis][0].isMin);
-        assert(maxLimitEndPoint->boxID == INVALID_INDEX && !maxLimitEndPoint->isMin);
-        EndPoint* newMaxLimitEndPoint = &mEndPoints[axis][indexLimitEndPoint + 2];
-        newMaxLimitEndPoint->setValues(maxLimitEndPoint->boxID, maxLimitEndPoint->isMin,
-                                       maxLimitEndPoint->value);
-    }
-    
-    // Create a new box
-    BoxAABB* box = &mBoxes[boxIndex];
-    box->body = body;
-    const uint maxEndPointValue = encodeFloatIntoInteger(FLT_MAX) - 1;
-    const uint minEndPointValue = encodeFloatIntoInteger(FLT_MAX) - 2;
-    for (uint axis=0; axis<3; axis++) {
-        box->min[axis] = indexLimitEndPoint;
-        box->max[axis] = indexLimitEndPoint + 1;
-        EndPoint* minimumEndPoint = &mEndPoints[axis][box->min[axis]];
-        minimumEndPoint->setValues(boxIndex, true, minEndPointValue);
-        EndPoint* maximumEndPoint = &mEndPoints[axis][box->max[axis]];
-        maximumEndPoint->setValues(boxIndex, false, maxEndPointValue);
-    }
-    
-    // Add the body pointer to box index mapping
-    mMapBodyToBoxIndex.insert(pair<CollisionBody*, bodyindex>(body, boxIndex));
-    
-    mNbBoxes++;
-
-    // Call the update method to put the end-points of the new AABB at the
-    // correct position in the array. This will also create the overlapping
-    // pairs in the pair manager if the new AABB is overlapping with others
-    // AABBs
-    updateProxyCollisionShape(body, aabb);
-} 
-
-// Notify the broad-phase about an object that has been removed from the world
-void SweepAndPruneAlgorithm::removeProxyCollisionShape(CollisionBody* body) {
-
-    assert(mNbBoxes > 0);
-
-    // Call the update method with an AABB that is very far away
-    // in order to remove all overlapping pairs from the pair manager
-    const uint maxEndPointValue = encodeFloatIntoInteger(FLT_MAX) - 1;
-    const uint minEndPointValue = encodeFloatIntoInteger(FLT_MAX) - 2;
-    const AABBInt aabbInt(minEndPointValue, maxEndPointValue);
-    updateObjectIntegerAABB(body, aabbInt);
-
-    // Get the corresponding box
-    bodyindex boxIndex = mMapBodyToBoxIndex.find(body)->second;
-
-    // Remove the end-points of the box by moving the maximum end-points two elements back in
-    // the end-points array
-    const luint indexLimitEndPoint = 2 * mNbBoxes + NB_SENTINELS - 1;
-    for (uint axis=0; axis < 3; axis++) {
-        EndPoint* maxLimitEndPoint = &mEndPoints[axis][indexLimitEndPoint];
-        assert(mEndPoints[axis][0].boxID == INVALID_INDEX && mEndPoints[axis][0].isMin);
-        assert(maxLimitEndPoint->boxID == INVALID_INDEX && !maxLimitEndPoint->isMin);
-        EndPoint* newMaxLimitEndPoint = &mEndPoints[axis][indexLimitEndPoint - 2];
-        assert(mEndPoints[axis][indexLimitEndPoint - 1].boxID == boxIndex);
-        assert(!mEndPoints[axis][indexLimitEndPoint - 1].isMin);
-        assert(newMaxLimitEndPoint->boxID == boxIndex);
-        assert(newMaxLimitEndPoint->isMin);
-        newMaxLimitEndPoint->setValues(maxLimitEndPoint->boxID, maxLimitEndPoint->isMin,
-                                       maxLimitEndPoint->value);
-    }
-
-    // Add the box index into the list of free indices
-    mFreeBoxIndices.push_back(boxIndex);
-
-    mMapBodyToBoxIndex.erase(body);
-    mNbBoxes--;
-
-    // Check if we need to shrink the allocated memory
-    const luint nextPowerOf2 = PairManager::computeNextPowerOfTwo((mNbBoxes-1) / 100 );
-    if (nextPowerOf2 * 100 < mNbMaxBoxes) {
-        shrinkArrays();
-    }
-} 
-        
-// Notify the broad-phase that the AABB of an object has changed.
-/// The input is an AABB with integer coordinates
-void SweepAndPruneAlgorithm::updateObjectIntegerAABB(CollisionBody* body, const AABBInt& aabbInt) {
-
-    assert(aabbInt.min[0] > encodeFloatIntoInteger(-FLT_MAX));
-    assert(aabbInt.min[1] > encodeFloatIntoInteger(-FLT_MAX));
-    assert(aabbInt.min[2] > encodeFloatIntoInteger(-FLT_MAX));
-    assert(aabbInt.max[0] < encodeFloatIntoInteger(FLT_MAX));
-    assert(aabbInt.max[1] < encodeFloatIntoInteger(FLT_MAX));
-    assert(aabbInt.max[2] < encodeFloatIntoInteger(FLT_MAX));
-    
-    // Get the corresponding box
-    bodyindex boxIndex = mMapBodyToBoxIndex.find(body)->second;
-    BoxAABB* box = &mBoxes[boxIndex];
-        
-    // Current axis
-    for (uint axis=0; axis<3; axis++) {
-        
-        // Get the two others axis
-        const uint otherAxis1 = (1 << axis) & 3;
-        const uint otherAxis2 = (1 << otherAxis1) & 3;
-        
-        // Get the starting end-point of the current axis
-        EndPoint* startEndPointsCurrentAxis = mEndPoints[axis];
-        
-        // -------- Update the minimum end-point ------------//
-        
-        EndPoint* currentMinEndPoint = &startEndPointsCurrentAxis[box->min[axis]];
-        assert(currentMinEndPoint->isMin);
-        
-        // Get the minimum value of the AABB on the current axis
-        uint limit = aabbInt.min[axis];
-        
-        // If the minimum value of the AABB is smaller
-        // than the current minimum endpoint
-        if (limit < currentMinEndPoint->value) {
-            
-            currentMinEndPoint->value = limit;
-            
-            // The minimum end-point is moving left
-            EndPoint savedEndPoint = *currentMinEndPoint;
-            luint indexEndPoint = (size_t(currentMinEndPoint) -
-                                   size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
-            const luint savedEndPointIndex = indexEndPoint;
-            
-            while ((--currentMinEndPoint)->value > limit) {
-                BoxAABB* id1 = &mBoxes[currentMinEndPoint->boxID];
-                const bool isMin = currentMinEndPoint->isMin;
-                
-                // If it's a maximum end-point
-                if (!isMin) {
-                    // The minimum end-point is moving to the left and
-                    // passed a maximum end-point. Thus, the boxes start
-                    // overlapping on the current axis. Therefore we test
-                    // for box intersection
-                    if (box != id1) {
-                        if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2) &&
-                            testIntersect1DSortedAABBs(*id1, aabbInt,
-                                                       startEndPointsCurrentAxis, axis)) {
-                            
-                            // Add an overlapping pair to the pair manager
-                            mPairManager.addPair(body, id1->body);
-                        }
-                    }
-                    
-                    id1->max[axis] = indexEndPoint--;
-                }
-                else {
-                    id1->min[axis] = indexEndPoint--;
-                }
-                
-                *(currentMinEndPoint+1) = *currentMinEndPoint;
-            }
-            
-            // Update the current minimum endpoint that we are moving
-            if (savedEndPointIndex != indexEndPoint) {
-                if (savedEndPoint.isMin) {
-                    mBoxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
-                }
-                else {
-                    mBoxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
-                }
-
-                startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
-            }
-        }
-        else if (limit > currentMinEndPoint->value){// The minimum of the box has moved to the right
-            
-            currentMinEndPoint->value = limit;
-            
-            // The minimum en-point is moving right
-            EndPoint savedEndPoint = *currentMinEndPoint;
-            luint indexEndPoint = (size_t(currentMinEndPoint) -
-                                   size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
-            const luint savedEndPointIndex = indexEndPoint;
-            
-            // For each end-point between the current position of the minimum
-            // end-point and the new position of the minimum end-point
-            while ((++currentMinEndPoint)->value < limit) {
-                BoxAABB* id1 = &mBoxes[currentMinEndPoint->boxID];
-                const bool isMin = currentMinEndPoint->isMin;
-                
-                // If it's a maximum end-point
-                if (!isMin) {
-                    // The minimum end-point is moving to the right and
-                    // passed a maximum end-point. Thus, the boxes stop
-                    // overlapping on the current axis.
-                    if (box != id1) {
-                        if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2)) {
-                            
-                            // Remove the pair from the pair manager
-                            mPairManager.removePair(body->getID(), id1->body->getID());
-                        }
-                    }
-                    
-                    id1->max[axis] = indexEndPoint++;
-                }
-                else {
-                    id1->min[axis] = indexEndPoint++;
-                }
-                
-                *(currentMinEndPoint-1) = *currentMinEndPoint;
-            }
-            
-            // Update the current minimum endpoint that we are moving
-            if (savedEndPointIndex != indexEndPoint) {
-                if (savedEndPoint.isMin) {
-                    mBoxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
-                }
-                else {
-                    mBoxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
-                }
-
-                startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
-            }
-        }
-        
-        // ------- Update the maximum end-point ------------ //
-        
-        EndPoint* currentMaxEndPoint = &startEndPointsCurrentAxis[box->max[axis]];
-        assert(!currentMaxEndPoint->isMin);
-        
-        // Get the maximum value of the AABB on the current axis
-        limit = aabbInt.max[axis];
-        
-        // If the new maximum value of the AABB is larger
-        // than the current maximum end-point value. It means
-        // that the AABB is moving to the right.
-        if (limit > currentMaxEndPoint->value) {
-            
-            currentMaxEndPoint->value = limit;
-            
-            EndPoint savedEndPoint = *currentMaxEndPoint;
-            luint indexEndPoint = (size_t(currentMaxEndPoint) -
-                                   size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
-            const luint savedEndPointIndex = indexEndPoint;
-            
-            while ((++currentMaxEndPoint)->value < limit) {
-                
-                // Get the next end-point
-                BoxAABB* id1 = &mBoxes[currentMaxEndPoint->boxID];
-                const bool isMin = currentMaxEndPoint->isMin;
-                
-                // If it's a maximum end-point
-                if (isMin) {
-                    // The maximum end-point is moving to the right and
-                    // passed a minimum end-point. Thus, the boxes start
-                    // overlapping on the current axis. Therefore we test
-                    // for box intersection
-                    if (box != id1) {
-                        if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2) &&
-                            testIntersect1DSortedAABBs(*id1, aabbInt,
-                                                       startEndPointsCurrentAxis,axis)) {
-                            
-                            // Add an overlapping pair to the pair manager
-                            mPairManager.addPair(body, id1->body);
-                        }
-                    }
-                    
-                    id1->min[axis] = indexEndPoint++;
-                }
-                else {
-                    id1->max[axis] = indexEndPoint++;
-                }
-                
-                *(currentMaxEndPoint-1) = *currentMaxEndPoint;
-            }
-            
-            // Update the current minimum endpoint that we are moving
-            if (savedEndPointIndex != indexEndPoint) {
-                if (savedEndPoint.isMin) {
-                    mBoxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
-                }
-                else {
-                    mBoxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
-                }
-
-                startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
-            }
-        }
-        else if (limit < currentMaxEndPoint->value) {   // If the AABB is moving to the left 
-            currentMaxEndPoint->value = limit;
-            
-            EndPoint savedEndPoint = *currentMaxEndPoint;
-            luint indexEndPoint = (size_t(currentMaxEndPoint) -
-                                   size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
-            const luint savedEndPointIndex = indexEndPoint;
-            
-            // For each end-point between the current position of the maximum
-            // end-point and the new position of the maximum end-point
-            while ((--currentMaxEndPoint)->value > limit) {
-                BoxAABB* id1 = &mBoxes[currentMaxEndPoint->boxID];
-                const bool isMin = currentMaxEndPoint->isMin;
-                
-                // If it's a minimum end-point
-                if (isMin) {
-                    // The maximum end-point is moving to the right and
-                    // passed a minimum end-point. Thus, the boxes stop
-                    // overlapping on the current axis.
-                    if (box != id1) {
-                        if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2)) {
-                            
-                            // Remove the pair from the pair manager
-                            mPairManager.removePair(body->getID(), id1->body->getID());
-                        }
-                    }
-                    
-                    id1->min[axis] = indexEndPoint--;
-                }
-                else {
-                    id1->max[axis] = indexEndPoint--;
-                }
-                
-                *(currentMaxEndPoint+1) = *currentMaxEndPoint;
-            }
-            
-            // Update the current minimum endpoint that we are moving
-            if (savedEndPointIndex != indexEndPoint) {
-                if (savedEndPoint.isMin) {
-                    mBoxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
-                }
-                else {
-                    mBoxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
-                }
-
-                startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
-            }
-        }
-    }
-}
-
-// Resize the boxes and end-points arrays when it is full
-void SweepAndPruneAlgorithm::resizeArrays() {
-    
-    // New number of boxes in the array
-    const luint newNbMaxBoxes = mNbMaxBoxes ? 2 * mNbMaxBoxes : 100;
-    const luint nbEndPoints = mNbBoxes * 2 + NB_SENTINELS;
-    const luint newNbEndPoints = newNbMaxBoxes * 2 + NB_SENTINELS;
-    
-    // Allocate memory for the new boxes and end-points arrays
-    BoxAABB* newBoxesArray = new BoxAABB[newNbMaxBoxes];
-    EndPoint* newEndPointsXArray = new EndPoint[newNbEndPoints];
-    EndPoint* newEndPointsYArray = new EndPoint[newNbEndPoints];
-    EndPoint* newEndPointsZArray = new EndPoint[newNbEndPoints];
-    
-    assert(newBoxesArray != NULL);
-    assert(newEndPointsXArray != NULL);
-    assert(newEndPointsYArray != NULL);
-    assert(newEndPointsZArray != NULL);
-    
-    // If the arrays were not empty before
-    if (mNbBoxes > 0) {
-        
-        // Copy the data from the old arrays into the new one
-        memcpy(newBoxesArray, mBoxes, sizeof(BoxAABB) * mNbBoxes);
-        const size_t nbBytesNewEndPoints = sizeof(EndPoint) * nbEndPoints;
-        memcpy(newEndPointsXArray, mEndPoints[0], nbBytesNewEndPoints);
-        memcpy(newEndPointsYArray, mEndPoints[1], nbBytesNewEndPoints);
-        memcpy(newEndPointsZArray, mEndPoints[2], nbBytesNewEndPoints);
-    }
-    else {   // If the arrays were empty
-
-        // Add the limits endpoints (sentinels) into the array
-        const uint min = encodeFloatIntoInteger(-FLT_MAX);
-        const uint max = encodeFloatIntoInteger(FLT_MAX);
-        newEndPointsXArray[0].setValues(INVALID_INDEX, true, min);
-        newEndPointsXArray[1].setValues(INVALID_INDEX, false, max);
-        newEndPointsYArray[0].setValues(INVALID_INDEX, true, min);
-        newEndPointsYArray[1].setValues(INVALID_INDEX, false, max);
-        newEndPointsZArray[0].setValues(INVALID_INDEX, true, min);
-        newEndPointsZArray[1].setValues(INVALID_INDEX, false, max);
-    }
-
-    // Delete the old arrays
-    delete[] mBoxes;
-    delete[] mEndPoints[0];
-    delete[] mEndPoints[1];
-    delete[] mEndPoints[2];
-    
-    // Assign the pointer to the new arrays
-    mBoxes = newBoxesArray;
-    mEndPoints[0] = newEndPointsXArray;
-    mEndPoints[1] = newEndPointsYArray;
-    mEndPoints[2] = newEndPointsZArray;
-    
-    mNbMaxBoxes = newNbMaxBoxes;
-}
-
-// Shrink the boxes and end-points arrays when too much memory is allocated
-void SweepAndPruneAlgorithm::shrinkArrays() {
-
-    // New number of boxes and end-points in the array
-    const luint nextPowerOf2 = PairManager::computeNextPowerOfTwo((mNbBoxes-1) / 100 );
-    const luint newNbMaxBoxes = (mNbBoxes > 100) ? nextPowerOf2 * 100 : 100;
-    const luint nbEndPoints = mNbBoxes * 2 + NB_SENTINELS;
-    const luint newNbEndPoints = newNbMaxBoxes * 2 + NB_SENTINELS;
-
-    assert(newNbMaxBoxes < mNbMaxBoxes);
-
-    // Sort the list of the free boxes indices in ascending order
-    mFreeBoxIndices.sort();
-
-    // Reorganize the boxes inside the boxes array so that all the boxes are at the
-    // beginning of the array
-    std::map<CollisionBody*, bodyindex> newMapBodyToBoxIndex;
-    std::map<CollisionBody*,bodyindex>::const_iterator it;
-    for (it = mMapBodyToBoxIndex.begin(); it != mMapBodyToBoxIndex.end(); ++it) {
-
-        CollisionBody* body = it->first;
-        bodyindex boxIndex = it->second;
-
-        // If the box index is outside the range of the current number of boxes
-        if (boxIndex >= mNbBoxes) {
-
-            assert(!mFreeBoxIndices.empty());
-
-            // Get a new box index for that body (from the list of free box indices)
-            bodyindex newBoxIndex = mFreeBoxIndices.front();
-            mFreeBoxIndices.pop_front();
-            assert(newBoxIndex < mNbBoxes);
-
-            // Copy the box to its new location in the boxes array
-            BoxAABB* oldBox = &mBoxes[boxIndex];
-            BoxAABB* newBox = &mBoxes[newBoxIndex];
-            assert(oldBox->body->getID() == body->getID());
-            newBox->body = oldBox->body;
-            for (uint axis=0; axis<3; axis++) {
-
-                // Copy the minimum and maximum end-points indices
-                newBox->min[axis] = oldBox->min[axis];
-                newBox->max[axis] = oldBox->max[axis];
-
-                // Update the box index of the end-points
-                EndPoint* minimumEndPoint = &mEndPoints[axis][newBox->min[axis]];
-                EndPoint* maximumEndPoint = &mEndPoints[axis][newBox->max[axis]];
-                assert(minimumEndPoint->boxID == boxIndex);
-                assert(maximumEndPoint->boxID == boxIndex);
-                minimumEndPoint->boxID = newBoxIndex;
-                maximumEndPoint->boxID = newBoxIndex;
-            }
-
-            newMapBodyToBoxIndex.insert(pair<CollisionBody*, bodyindex>(body, newBoxIndex));
-        }
-        else {
-            newMapBodyToBoxIndex.insert(pair<CollisionBody*, bodyindex>(body, boxIndex));
-        }
-    }
-
-    assert(newMapBodyToBoxIndex.size() == mMapBodyToBoxIndex.size());
-    mMapBodyToBoxIndex = newMapBodyToBoxIndex;
-
-    // Allocate memory for the new boxes and end-points arrays
-    BoxAABB* newBoxesArray = new BoxAABB[newNbMaxBoxes];
-    EndPoint* newEndPointsXArray = new EndPoint[newNbEndPoints];
-    EndPoint* newEndPointsYArray = new EndPoint[newNbEndPoints];
-    EndPoint* newEndPointsZArray = new EndPoint[newNbEndPoints];
-
-    assert(newBoxesArray != NULL);
-    assert(newEndPointsXArray != NULL);
-    assert(newEndPointsYArray != NULL);
-    assert(newEndPointsZArray != NULL);
-
-    // Copy the data from the old arrays into the new one
-    memcpy(newBoxesArray, mBoxes, sizeof(BoxAABB) * mNbBoxes);
-    const size_t nbBytesNewEndPoints = sizeof(EndPoint) * nbEndPoints;
-    memcpy(newEndPointsXArray, mEndPoints[0], nbBytesNewEndPoints);
-    memcpy(newEndPointsYArray, mEndPoints[1], nbBytesNewEndPoints);
-    memcpy(newEndPointsZArray, mEndPoints[2], nbBytesNewEndPoints);
-
-    // Delete the old arrays
-    delete[] mBoxes;
-    delete[] mEndPoints[0];
-    delete[] mEndPoints[1];
-    delete[] mEndPoints[2];
-
-    // Assign the pointer to the new arrays
-    mBoxes = newBoxesArray;
-    mEndPoints[0] = newEndPointsXArray;
-    mEndPoints[1] = newEndPointsYArray;
-    mEndPoints[2] = newEndPointsZArray;
-
-    mNbMaxBoxes = newNbMaxBoxes;
-}

src/collision/broadphase/SweepAndPruneAlgorithm.h

@@ -1,245 +0,0 @@
-/********************************************************************************
-* 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.    *
-*                                                                               *
-********************************************************************************/
-
-#ifndef REACTPHYSICS3D_SWEEP_AND_PRUNE_ALGORITHM_H
-#define REACTPHYSICS3D_SWEEP_AND_PRUNE_ALGORITHM_H
-
-// Libraries
-#include "BroadPhaseAlgorithm.h"
-#include "../../collision/shapes/AABB.h"
-#include <map>
-#include <vector>
-#include <list>
-
-
-/// Namespace ReactPhysics3D
-namespace reactphysics3d {
-
-// Structure EndPoint
-/**
- * EndPoint structure that represent an end-point of an AABB
- * on one of the three x,y or z axis.
- */
-struct EndPoint {
-
-    public:
-        
-        /// ID of the AABB box corresponding to this end-point
-        bodyindex boxID;
-
-        /// True if the end-point is a minimum end-point of a box
-        bool isMin;
-
-        /// Value (one dimension coordinate) of the end-point
-        uint value;
-        
-        /// Set the values of the endpoint
-        void setValues(bodyindex boxID, bool isMin, uint value) {
-            this->boxID = boxID;
-            this->isMin = isMin;
-            this->value = value;
-        }
-};            
-
-// Structure BoxAABB
-/**
- * This structure represents an AABB in the Sweep-And-Prune algorithm
- */
-struct BoxAABB {
-
-    public:
-
-        /// Index of the 3 minimum end-points of the AABB over the x,y,z axis
-        bodyindex min[3];
-
-        /// Index of the 3 maximum end-points of the AABB over the x,y,z axis
-        bodyindex max[3];
-
-        /// Body that corresponds to the owner of the AABB
-        CollisionBody* body;
-};
-
-// Structure AABBInt
-/**
- * Axis-Aligned Bounding box with integer coordinates.
- */
-struct AABBInt {
-
-    public:
-
-        /// Minimum values on the three axis
-        uint min[3];
-
-        /// Maximum values on the three axis
-        uint max[3];
-        
-        /// Constructor that takes an AABB as input
-        AABBInt(const AABB& aabb);
-
-        /// Constructor that set all the axis with an minimum and maximum value
-        AABBInt(uint minValue, uint maxValue);
-};
-
-// Class SweepAndPruneAlgorithm
-/**
- * This class implements the Sweep-And-Prune (SAP) broad-phase
- * collision detection algorithm. This class implements an
- * array-based implementation of the algorithm from Pierre Terdiman
- * that is described here : www.codercorner.com/SAP.pdf.
- */
-class SweepAndPruneAlgorithm : public BroadPhaseAlgorithm {
-    
-    protected :
-
-        // -------------------- Constants -------------------- //
-
-        /// Invalid array index
-        const static bodyindex INVALID_INDEX;
-
-        /// Number of sentinel end-points in the array of a given axis
-        const static luint NB_SENTINELS;
-
-        // -------------------- Attributes -------------------- //
-
-        /// Array that contains all the AABB boxes of the broad-phase
-        BoxAABB* mBoxes;
-
-        /// Array of end-points on the three axis
-        EndPoint* mEndPoints[3];
-
-        /// Number of AABB boxes in the broad-phase
-        bodyindex mNbBoxes;
-
-        /// Max number of boxes in the boxes array
-        bodyindex mNbMaxBoxes;
-
-        /// Indices that are not used by any boxes
-        std::list<bodyindex> mFreeBoxIndices;
-
-        /// Map a body pointer to a box index
-        std::map<CollisionBody*,bodyindex> mMapBodyToBoxIndex;
-        
-        // -------------------- Methods -------------------- //
-
-        /// Private copy-constructor
-        SweepAndPruneAlgorithm(const SweepAndPruneAlgorithm& algorithm);
-
-        /// Private assignment operator
-        SweepAndPruneAlgorithm& operator=(const SweepAndPruneAlgorithm& algorithm);
-
-        /// Resize the boxes and end-points arrays when it's full
-        void resizeArrays();
-
-        /// Shrink the boxes and end-points arrays when too much memory is allocated
-        void shrinkArrays();
-
-        /// Add an overlapping pair of AABBS
-        void addPair(CollisionBody* body1, CollisionBody* body2);
-
-        /// Check for 1D box intersection between two boxes that are sorted on the given axis.
-        bool testIntersect1DSortedAABBs(const BoxAABB& box1, const AABBInt& box2,
-                                        const EndPoint* const baseEndPoint, uint axis) const;
-
-        /// Check for 2D box intersection.
-        bool testIntersect2D(const BoxAABB& box1, const BoxAABB& box2,
-                             luint axis1, uint axis2) const;
-
-        /// Notify the broad-phase that the AABB of an object has changed.
-        void updateObjectIntegerAABB(CollisionBody* body, const AABBInt& aabbInt);
-        
-    public :
-
-        // -------------------- Methods -------------------- //
-
-        /// Constructor
-        SweepAndPruneAlgorithm(CollisionDetection& mCollisionDetection);
-
-        /// Destructor
-        virtual ~SweepAndPruneAlgorithm();
-
-        /// Notify the broad-phase about a new object in the world.
-        virtual void addProxyCollisionShape(CollisionBody* body, const AABB& aabb);
-
-        /// Notify the broad-phase about a object that has been removed from the world
-        virtual void removeProxyCollisionShape(CollisionBody* body);
-
-        /// Notify the broad-phase that the AABB of an object has changed
-        virtual void updateProxyCollisionShape(CollisionBody* body, const AABB& aabb);
-};
-
-/// Encode a floating value into a integer value in order to
-/// work with integer comparison in the Sweep-And-Prune algorithm
-/// because it is faster. The main issue when encoding floating
-/// number into integer is to keep to sorting order. This is a
-/// problem for negative float number. This article describes
-/// how to solve this issue : http://www.stereopsis.com/radix.html
-inline uint encodeFloatIntoInteger(float number) {
-    uint intNumber = (uint&) number;
-    
-    // If it's a negative number
-    if(intNumber & 0x80000000)
-        intNumber = ~intNumber;
-    else {     // If it is a positive number
-        intNumber |= 0x80000000;
-    }
-     
-    return intNumber;
-}                
-
-        
-// Check for 1D box intersection between two boxes that are sorted on the given axis.
-/// Therefore, only one test is necessary here. We know that the
-/// minimum of box1 cannot be larger that the maximum of box2 on the axis.
-inline bool SweepAndPruneAlgorithm::testIntersect1DSortedAABBs(const BoxAABB& box1,
-                                                               const AABBInt& box2,
-                                                               const EndPoint* const endPointsArray,
-                                                               uint axis) const {
-    return !(endPointsArray[box1.max[axis]].value < box2.min[axis]);
-}    
-
-// Check for 2D box intersection. This method is used when we know
-/// that two boxes already overlap on one axis and when want to test
-/// if they also overlap on the two others axis.
-inline bool SweepAndPruneAlgorithm::testIntersect2D(const BoxAABB& box1, const BoxAABB& box2,
-                                                    luint axis1, uint axis2) const {
-    return !(box2.max[axis1] < box1.min[axis1] || box1.max[axis1] < box2.min[axis1] ||
-             box2.max[axis2] < box1.min[axis2] || box1.max[axis2] < box2.min[axis2]);
-}
-
-// Notify the broad-phase that the AABB of an object has changed
-inline void SweepAndPruneAlgorithm::updateProxyCollisionShape(CollisionBody* body, const AABB& aabb) {
-
-    // Compute the corresponding AABB with integer coordinates
-    AABBInt aabbInt(aabb);
-
-    // Call the update object method that uses an AABB with integer coordinates
-    updateObjectIntegerAABB(body, aabbInt);
-}
-
-}
-
-#endif
-
-

src/collision/shapes/BoxShape.cpp

@@ -63,7 +63,7 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const
 }
 
 // Constructor
-ProxyBoxShape::ProxyBoxShape(const BoxShape* shape, CollisionBody* body,
+ProxyBoxShape::ProxyBoxShape(BoxShape* shape, CollisionBody* body,
                              const Transform& transform, decimal mass)
               :ProxyShape(body, transform, mass), mCollisionShape(shape){
 

src/collision/shapes/BoxShape.h

@@ -102,7 +102,7 @@ class BoxShape : public CollisionShape {
 
         /// Create a proxy collision shape for the collision shape
         virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
-                                             const Transform& transform, decimal mass) const;
+                                             const Transform& transform, decimal mass);
 };
 
 // Class ProxyBoxShape
@@ -116,7 +116,7 @@ class ProxyBoxShape : public ProxyShape {
         // -------------------- Attributes -------------------- //
 
         /// Pointer to the actual collision shape
-        const BoxShape* mCollisionShape;
+        BoxShape* mCollisionShape;
 
 
         // -------------------- Methods -------------------- //
@@ -127,12 +127,15 @@ class ProxyBoxShape : public ProxyShape {
         /// Private assignment operator
         ProxyBoxShape& operator=(const ProxyBoxShape& proxyShape);
 
+        /// Return the non-const collision shape
+        virtual CollisionShape* getInternalCollisionShape() const;
+
     public:
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ProxyBoxShape(const BoxShape* shape, CollisionBody* body,
+        ProxyBoxShape(BoxShape* shape, CollisionBody* body,
                       const Transform& transform, decimal mass);
 
         /// Destructor
@@ -206,11 +209,16 @@ inline bool BoxShape::isEqualTo(const CollisionShape& otherCollisionShape) const
 
 // Create a proxy collision shape for the collision shape
 inline ProxyShape* BoxShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
-                                              const Transform& transform, decimal mass) const {
+                                              const Transform& transform, decimal mass) {
     return new (allocator.allocate(sizeof(ProxyBoxShape))) ProxyBoxShape(this, body,
                                                                          transform, mass);
 }
 
+// Return the non-const collision shape
+inline CollisionShape* ProxyBoxShape::getInternalCollisionShape() const {
+    return mCollisionShape;
+}
+
 // Return the collision shape
 inline const CollisionShape* ProxyBoxShape::getCollisionShape() const {
     return mCollisionShape;

src/collision/shapes/CapsuleShape.cpp

@@ -125,7 +125,7 @@ void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) co
 }
 
 // Constructor
-ProxyCapsuleShape::ProxyCapsuleShape(const CapsuleShape* shape, CollisionBody* body,
+ProxyCapsuleShape::ProxyCapsuleShape(CapsuleShape* shape, CollisionBody* body,
                                      const Transform& transform, decimal mass)
                    :ProxyShape(body, transform, mass), mCollisionShape(shape){
 

src/collision/shapes/CapsuleShape.h

@@ -102,7 +102,7 @@ class CapsuleShape : public CollisionShape {
 
         /// Create a proxy collision shape for the collision shape
         virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
-                                             const Transform& transform, decimal mass) const;
+                                             const Transform& transform, decimal mass);
 };
 
 // Class ProxyCapsuleShape
@@ -116,7 +116,7 @@ class ProxyCapsuleShape : public ProxyShape {
         // -------------------- Attributes -------------------- //
 
         /// Pointer to the actual collision shape
-        const CapsuleShape* mCollisionShape;
+        CapsuleShape* mCollisionShape;
 
 
         // -------------------- Methods -------------------- //
@@ -127,12 +127,15 @@ class ProxyCapsuleShape : public ProxyShape {
         /// Private assignment operator
         ProxyCapsuleShape& operator=(const ProxyCapsuleShape& proxyShape);
 
+        /// Return the non-const collision shape
+        virtual CollisionShape* getInternalCollisionShape() const;
+
     public:
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ProxyCapsuleShape(const CapsuleShape* shape, CollisionBody* body,
+        ProxyCapsuleShape(CapsuleShape* shape, CollisionBody* body,
                           const Transform& transform, decimal mass);
 
         /// Destructor
@@ -197,11 +200,16 @@ inline bool CapsuleShape::isEqualTo(const CollisionShape& otherCollisionShape) c
 
 // Create a proxy collision shape for the collision shape
 inline ProxyShape* CapsuleShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
-                                                  const Transform& transform, decimal mass) const {
+                                                  const Transform& transform, decimal mass) {
     return new (allocator.allocate(sizeof(ProxyCapsuleShape))) ProxyCapsuleShape(this, body,
                                                                            transform, mass);
 }
 
+// Return the non-const collision shape
+inline CollisionShape* ProxyCapsuleShape::getInternalCollisionShape() const {
+    return mCollisionShape;
+}
+
 // Return the collision shape
 inline const CollisionShape* ProxyCapsuleShape::getCollisionShape() const {
     return mCollisionShape;

src/collision/shapes/CollisionShape.h

@@ -120,7 +120,7 @@ class CollisionShape {
 
         /// Create a proxy collision shape for the collision shape
         virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
-                                             const Transform& transform, decimal mass) const=0;
+                                             const Transform& transform, decimal mass)=0;
 };
 
 
@@ -163,6 +163,9 @@ class ProxyShape {
         /// Private assignment operator
         ProxyShape& operator=(const ProxyShape& proxyShape);
 
+        /// Return the non-const collision shape
+        virtual CollisionShape* getInternalCollisionShape() const=0;
+
     public:
 
         // -------------------- Methods -------------------- //
@@ -204,6 +207,7 @@ class ProxyShape {
         friend class RigidBody;
         friend class BroadPhaseAlgorithm;
         friend class DynamicAABBTree;
+        friend class CollisionDetection;
 };
 
 // Return the type of the collision shape

src/collision/shapes/ConeShape.cpp

@@ -94,7 +94,7 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction) c
 }
 
 // Constructor
-ProxyConeShape::ProxyConeShape(const ConeShape* shape, CollisionBody* body,
+ProxyConeShape::ProxyConeShape(ConeShape* shape, CollisionBody* body,
                                const Transform& transform, decimal mass)
                :ProxyShape(body, transform, mass), mCollisionShape(shape){
 

src/collision/shapes/ConeShape.h

@@ -110,7 +110,7 @@ class ConeShape : public CollisionShape {
 
         /// Create a proxy collision shape for the collision shape
         virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
-                                             const Transform& transform, decimal mass) const;
+                                             const Transform& transform, decimal mass);
 };
 
 // Class ProxyConeShape
@@ -124,7 +124,7 @@ class ProxyConeShape : public ProxyShape {
         // -------------------- Attributes -------------------- //
 
         /// Pointer to the actual collision shape
-        const ConeShape* mCollisionShape;
+        ConeShape* mCollisionShape;
 
 
         // -------------------- Methods -------------------- //
@@ -135,12 +135,15 @@ class ProxyConeShape : public ProxyShape {
         /// Private assignment operator
         ProxyConeShape& operator=(const ProxyConeShape& proxyShape);
 
+        /// Return the non-const collision shape
+        virtual CollisionShape* getInternalCollisionShape() const;
+
     public:
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ProxyConeShape(const ConeShape* shape, CollisionBody* body,
+        ProxyConeShape(ConeShape* shape, CollisionBody* body,
                        const Transform& transform, decimal mass);
 
         /// Destructor
@@ -213,11 +216,16 @@ inline bool ConeShape::isEqualTo(const CollisionShape& otherCollisionShape) cons
 
 // Create a proxy collision shape for the collision shape
 inline ProxyShape* ConeShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
-                                               const Transform& transform, decimal mass) const {
+                                               const Transform& transform, decimal mass) {
     return new (allocator.allocate(sizeof(ProxyConeShape))) ProxyConeShape(this, body,
                                                                            transform, mass);
 }
 
+// Return the non-const collision shape
+inline CollisionShape* ProxyConeShape::getInternalCollisionShape() const {
+    return mCollisionShape;
+}
+
 // Return the collision shape
 inline const CollisionShape* ProxyConeShape::getCollisionShape() const {
     return mCollisionShape;

src/collision/shapes/ConvexMeshShape.cpp

@@ -225,8 +225,8 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const
 }
 
 // Constructor
-ProxyConvexMeshShape::ProxyConvexMeshShape(const ConvexMeshShape* shape, CollisionBody* body,
+ProxyConvexMeshShape::ProxyConvexMeshShape(ConvexMeshShape* shape, CollisionBody* body,
-                                             const Transform& transform, decimal mass)
+                                           const Transform& transform, decimal mass)
                       :ProxyShape(body, transform, mass), mCollisionShape(shape),
                        mCachedSupportVertex(0) {
 

src/collision/shapes/ConvexMeshShape.h

@@ -140,7 +140,7 @@ class ConvexMeshShape : public CollisionShape {
 
         /// Create a proxy collision shape for the collision shape
         virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
-                                             const Transform& transform, decimal mass) const;
+                                             const Transform& transform, decimal mass);
 };
 
 
@@ -155,7 +155,7 @@ class ProxyConvexMeshShape : public ProxyShape {
         // -------------------- Attributes -------------------- //
 
         /// Pointer to the actual collision shape
-        const ConvexMeshShape* mCollisionShape;
+        ConvexMeshShape* mCollisionShape;
 
         /// Cached support vertex index (previous support vertex for hill-climbing)
         uint mCachedSupportVertex;
@@ -168,12 +168,15 @@ class ProxyConvexMeshShape : public ProxyShape {
         /// Private assignment operator
         ProxyConvexMeshShape& operator=(const ProxyConvexMeshShape& proxyShape);
 
+        /// Return the non-const collision shape
+        virtual CollisionShape* getInternalCollisionShape() const;
+
     public:
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ProxyConvexMeshShape(const ConvexMeshShape* shape, CollisionBody* body,
+        ProxyConvexMeshShape(ConvexMeshShape* shape, CollisionBody* body,
                               const Transform& transform, decimal mass);
 
         /// Destructor
@@ -281,11 +284,16 @@ inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) {
 inline ProxyShape* ConvexMeshShape::createProxyShape(MemoryAllocator& allocator,
                                                            CollisionBody* body,
                                                            const Transform& transform,
-                                                           decimal mass) const {
+                                                           decimal mass) {
     return new (allocator.allocate(sizeof(ProxyConvexMeshShape))) ProxyConvexMeshShape(this, body,
                                                                                transform, mass);
 }
 
+// Return the non-const collision shape
+inline CollisionShape* ProxyConvexMeshShape::getInternalCollisionShape() const {
+    return mCollisionShape;
+}
+
 // Return the collision shape
 inline const CollisionShape* ProxyConvexMeshShape::getCollisionShape() const {
     return mCollisionShape;

src/collision/shapes/CylinderShape.cpp

@@ -87,7 +87,7 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
 }
 
 // Constructor
-ProxyCylinderShape::ProxyCylinderShape(const CylinderShape* cylinderShape, CollisionBody* body,
+ProxyCylinderShape::ProxyCylinderShape(CylinderShape* cylinderShape, CollisionBody* body,
                                        const Transform& transform, decimal mass)
                    :ProxyShape(body, transform, mass), mCollisionShape(cylinderShape){
 

src/collision/shapes/CylinderShape.h

@@ -107,7 +107,7 @@ class CylinderShape : public CollisionShape {
 
         /// Create a proxy collision shape for the collision shape
         virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
-                                             const Transform& transform, decimal mass) const;
+                                             const Transform& transform, decimal mass);
 
 };
 
@@ -122,8 +122,7 @@ class ProxyCylinderShape : public ProxyShape {
         // -------------------- Attributes -------------------- //
 
         /// Pointer to the actual collision shape
-        const CylinderShape* mCollisionShape;
+        CylinderShape* mCollisionShape;
-
 
         // -------------------- Methods -------------------- //
 
@@ -133,12 +132,15 @@ class ProxyCylinderShape : public ProxyShape {
         /// Private assignment operator
         ProxyCylinderShape& operator=(const ProxyCylinderShape& proxyShape);
 
+        /// Return the non-const collision shape
+        virtual CollisionShape* getInternalCollisionShape() const;
+
     public:
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ProxyCylinderShape(const CylinderShape* cylinderShape, CollisionBody* body,
+        ProxyCylinderShape(CylinderShape* cylinderShape, CollisionBody* body,
                            const Transform& transform, decimal mass);
 
         /// Destructor
@@ -211,11 +213,16 @@ inline bool CylinderShape::isEqualTo(const CollisionShape& otherCollisionShape)
 
 // Create a proxy collision shape for the collision shape
 inline ProxyShape* CylinderShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
-                                                 const Transform& transform, decimal mass) const {
+                                                 const Transform& transform, decimal mass) {
     return new (allocator.allocate(sizeof(ProxyCylinderShape))) ProxyCylinderShape(this, body,
                                                                                transform, mass);
 }
 
+// Return the non-const collision shape
+inline CollisionShape* ProxyCylinderShape::getInternalCollisionShape() const {
+    return mCollisionShape;
+}
+
 // Return the collision shape
 inline const CollisionShape* ProxyCylinderShape::getCollisionShape() const {
     return mCollisionShape;

src/collision/shapes/SphereShape.cpp

@@ -47,7 +47,7 @@ SphereShape::~SphereShape() {
 }
 
 // Constructor
-ProxySphereShape::ProxySphereShape(const SphereShape* shape, CollisionBody* body,
+ProxySphereShape::ProxySphereShape(SphereShape* shape, CollisionBody* body,
                                    const Transform& transform, decimal mass)
                  :ProxyShape(body, transform, mass), mCollisionShape(shape){
 

src/collision/shapes/SphereShape.h

@@ -97,7 +97,7 @@ class SphereShape : public CollisionShape {
 
         /// Create a proxy collision shape for the collision shape
         virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
-                                             const Transform& transform, decimal mass) const;
+                                             const Transform& transform, decimal mass);
 };
 
 
@@ -112,7 +112,7 @@ class ProxySphereShape : public ProxyShape {
         // -------------------- Attributes -------------------- //
 
         /// Pointer to the actual collision shape
-        const SphereShape* mCollisionShape;
+        SphereShape* mCollisionShape;
 
 
         // -------------------- Methods -------------------- //
@@ -123,12 +123,15 @@ class ProxySphereShape : public ProxyShape {
         /// Private assignment operator
         ProxySphereShape& operator=(const ProxySphereShape& proxyShape);
 
+        /// Return the non-const collision shape
+        virtual CollisionShape* getInternalCollisionShape() const;
+
     public:
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ProxySphereShape(const SphereShape* shape, CollisionBody* body,
+        ProxySphereShape(SphereShape* shape, CollisionBody* body,
                          const Transform& transform, decimal mass);
 
         /// Destructor
@@ -229,11 +232,16 @@ inline bool SphereShape::isEqualTo(const CollisionShape& otherCollisionShape) co
 
 // Create a proxy collision shape for the collision shape
 inline ProxyShape* SphereShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
-                                                 const Transform& transform, decimal mass) const {
+                                                 const Transform& transform, decimal mass)  {
     return new (allocator.allocate(sizeof(ProxySphereShape))) ProxySphereShape(this, body,
                                                                                transform, mass);
 }
 
+// Return the non-const collision shape
+inline CollisionShape* ProxySphereShape::getInternalCollisionShape() const {
+    return mCollisionShape;
+}
+
 // Return the collision shape
 inline const CollisionShape* ProxySphereShape::getCollisionShape() const {
     return mCollisionShape;

src/constraint/SliderJoint.cpp

@@ -76,7 +76,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
     mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
 
     // Get the bodies positions and orientations
-    const Vector3& x1 = mBody1-mCenterOfMassWorld;
+    const Vector3& x1 = mBody1->mCenterOfMassWorld;
     const Vector3& x2 = mBody2->mCenterOfMassWorld;
     const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
     const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();

src/engine/CollisionWorld.cpp

@@ -33,7 +33,8 @@ using namespace std;
 
 // Constructor
 CollisionWorld::CollisionWorld()
-               : mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0) {
+               : mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0),
+                 mEventListener(NULL) {
 
 }
 
@@ -43,21 +44,8 @@ CollisionWorld::~CollisionWorld() {
     assert(mBodies.empty());
 }
 
-// Notify the world about a new narrow-phase contact
-void CollisionWorld::notifyNewContact(const OverlappingPair *broadPhasePair,
-                                      const ContactPointInfo* contactInfo) {
-
-    // TODO : Implement this method
-}
-
-// Update the overlapping pair
-inline void CollisionWorld::updateOverlappingPair(const OverlappingPair *pair) {
-
-}
-
 // Create a collision body and add it to the world
-CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform,
+CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) {
-                                                   CollisionShape* collisionShape) {
 
     // Get the next available body ID
     bodyindex bodyID = computeNextAvailableBodyID();
@@ -67,16 +55,13 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform,
 
     // Create the collision body
     CollisionBody* collisionBody = new (mMemoryAllocator.allocate(sizeof(CollisionBody)))
-                                        CollisionBody(transform, collisionShape, bodyID);
+                                        CollisionBody(transform, *this, bodyID);
 
     assert(collisionBody != NULL);
 
     // Add the collision body to the world
     mBodies.insert(collisionBody);
 
-    // Add the collision body to the collision detection
-    mCollisionDetection.addProxyCollisionShape(collisionBody);
-
     // Return the pointer to the rigid body
     return collisionBody;
 }
@@ -84,8 +69,8 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform,
 // Destroy a collision body
 void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
 
-    // Remove the body from the collision detection
+    // Remove all the collision shapes of the body
-    mCollisionDetection.removeProxyCollisionShape(collisionBody);
+    collisionBody->removeAllCollisionShapes();
 
     // Add the body ID to the list of free IDs
     mFreeBodiesIDs.push_back(collisionBody->getID());

src/engine/CollisionWorld.h

@@ -77,6 +77,9 @@ class CollisionWorld {
         /// Memory allocator
         MemoryAllocator mMemoryAllocator;
 
+        /// Pointer to an event listener object
+        EventListener* mEventListener;
+
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -85,13 +88,6 @@ class CollisionWorld {
         /// Private assignment operator
         CollisionWorld& operator=(const CollisionWorld& world);
 
-        /// Notify the world about a new narrow-phase contact
-        virtual void notifyNewContact(const OverlappingPair* pair,
-                                      const ContactPointInfo* contactInfo);
-
-        /// Update the overlapping pair
-        virtual void updateOverlappingPair(const OverlappingPair* pair);
-
         /// Return the next available body ID
         bodyindex computeNextAvailableBodyID();
 
@@ -118,8 +114,7 @@ class CollisionWorld {
         std::set<CollisionBody*>::iterator getBodiesEndIterator();
 
         /// Create a collision body
-        CollisionBody* createCollisionBody(const Transform& transform,
+        CollisionBody* createCollisionBody(const Transform& transform);
-                                           CollisionShape* collisionShape);
 
         /// Destroy a collision body
         void destroyCollisionBody(CollisionBody* collisionBody);

src/engine/ConstraintSolver.cpp

@@ -30,14 +30,9 @@
 using namespace reactphysics3d;
 
 // Constructor
-ConstraintSolver::ConstraintSolver(std::vector<Vector3>& positions,
+ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
-                                   std::vector<Quaternion>& orientations,
+                 : mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
-                                   const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
+                   mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) {
-                 : mLinearVelocities(NULL), mAngularVelocities(NULL), mPositions(positions),
-                   mOrientations(orientations),
-                   mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
-                   mIsWarmStartingActive(true), mConstraintSolverData(positions, orientations,
-                                                                      mapBodyToVelocityIndex){
 
 }
 
@@ -51,8 +46,6 @@ void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
 
     PROFILE("ConstraintSolver::initializeForIsland()");
 
-    assert(mLinearVelocities != NULL);
-    assert(mAngularVelocities != NULL);
     assert(island != NULL);
     assert(island->getNbBodies() > 0);
     assert(island->getNbJoints() > 0);

src/engine/ConstraintSolver.h

@@ -55,10 +55,10 @@ struct ConstraintSolverData {
         Vector3* angularVelocities;
 
         /// Reference to the bodies positions
-        std::vector<Vector3>& positions;
+        Vector3* positions;
 
         /// Reference to the bodies orientations
-        std::vector<Quaternion>& orientations;
+        Quaternion* orientations;
 
         /// Reference to the map that associates rigid body to their index
         /// in the constrained velocities array
@@ -68,12 +68,9 @@ struct ConstraintSolverData {
         bool isWarmStartingActive;
 
         /// Constructor
-        ConstraintSolverData(std::vector<Vector3>& refPositions,
+        ConstraintSolverData(const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
-                             std::vector<Quaternion>& refOrientations,
+                           :linearVelocities(NULL), angularVelocities(NULL),
-                             const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
+                            positions(NULL), orientations(NULL),
-                           :linearVelocities(NULL),
-                            angularVelocities(NULL),
-                            positions(refPositions), orientations(refOrientations),
                             mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
 
         }
@@ -155,20 +152,6 @@ class ConstraintSolver {
 
         // -------------------- Attributes -------------------- //
 
-        /// Array of constrained linear velocities (state of the linear velocities
-        /// after solving the constraints)
-        Vector3* mLinearVelocities;
-
-        /// Array of constrained angular velocities (state of the angular velocities
-        /// after solving the constraints)
-        Vector3* mAngularVelocities;
-
-        /// Reference to the array of bodies positions (for position error correction)
-        std::vector<Vector3>& mPositions;
-
-        /// Reference to the array of bodies orientations (for position error correction)
-        std::vector<Quaternion>& mOrientations;
-
         /// Reference to the map that associates rigid body to their index in
         /// the constrained velocities array
         const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
@@ -187,8 +170,7 @@ class ConstraintSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ConstraintSolver(std::vector<Vector3>& positions, std::vector<Quaternion>& orientations,
+        ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
-                         const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
 
         /// Destructor
         ~ConstraintSolver();
@@ -211,6 +193,10 @@ class ConstraintSolver {
         /// Set the constrained velocities arrays
         void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
                                             Vector3* constrainedAngularVelocities);
+
+        /// Set the constrained positions/orientations arrays
+        void setConstrainedPositionsArrays(Vector3* constrainedPositions,
+                                           Quaternion* constrainedOrientations);
 };
 
 // Set the constrained velocities arrays
@@ -218,10 +204,17 @@ inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constraine
                                                             Vector3* constrainedAngularVelocities) {
     assert(constrainedLinearVelocities != NULL);
     assert(constrainedAngularVelocities != NULL);
-    mLinearVelocities = constrainedLinearVelocities;
+    mConstraintSolverData.linearVelocities = constrainedLinearVelocities;
-    mAngularVelocities = constrainedAngularVelocities;
+    mConstraintSolverData.angularVelocities = constrainedAngularVelocities;
-    mConstraintSolverData.linearVelocities = mLinearVelocities;
+}
-    mConstraintSolverData.angularVelocities = mAngularVelocities;
+
+// Set the constrained positions/orientations arrays
+inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrainedPositions,
+                                                           Quaternion* constrainedOrientations) {
+    assert(constrainedPositions != NULL);
+    assert(constrainedOrientations != NULL);
+    mConstraintSolverData.positions = constrainedPositions;
+    mConstraintSolverData.orientations = constrainedOrientations;
 }
 
 }

src/engine/DynamicsWorld.cpp

@@ -46,9 +46,9 @@ using namespace std;
 DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
               : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityEnabled(true),
                 mConstrainedLinearVelocities(NULL), mConstrainedAngularVelocities(NULL),
+                mConstrainedPositions(NULL), mConstrainedOrientations(NULL),
                 mContactSolver(mMapBodyToConstrainedVelocityIndex),
-                mConstraintSolver(mConstrainedPositions, mConstrainedOrientations,
+                mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
-                                  mMapBodyToConstrainedVelocityIndex),
                 mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
                 mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
                 mIsSleepingEnabled(SPLEEPING_ENABLED), mSplitLinearVelocities(NULL),
@@ -56,7 +56,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_
                 mIslands(NULL), mNbBodiesCapacity(0),
                 mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY),
                 mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY),
-                mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP), mEventListener(NULL) {
+                mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) {
 
 }
 
@@ -90,6 +90,8 @@ DynamicsWorld::~DynamicsWorld() {
         delete[] mSplitAngularVelocities;
         delete[] mConstrainedLinearVelocities;
         delete[] mConstrainedAngularVelocities;
+        delete[] mConstrainedPositions;
+        delete[] mConstrainedOrientations;
     }
 
 #ifdef IS_PROFILING_ACTIVE
@@ -122,7 +124,7 @@ void DynamicsWorld::update() {
     while(mTimer.isPossibleToTakeStep()) {
 
         // Remove all contact manifolds
-        mContactManifolds.clear();
+        mCollisionDetection.mContactManifolds.clear();
 
         // Reset all the contact manifolds lists of each body
         resetContactManifoldListsOfBodies();
@@ -148,10 +150,10 @@ void DynamicsWorld::update() {
         // Solve the position correction for constraints
         solvePositionCorrection();
 
-        if (mIsSleepingEnabled) updateSleepingBodies();
+        // Update the state (positions and velocities) of the bodies
+        updateBodiesState();
 
-        // Update the AABBs of the bodies
+        if (mIsSleepingEnabled) updateSleepingBodies();
-        updateRigidBodiesAABB();
     }
 
     // Reset the external force and torque applied to the bodies
@@ -183,10 +185,6 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
             Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
             Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
 
-            // Update the linear and angular velocity of the body
-            bodies[b]->mLinearVelocity = newLinVelocity;
-            bodies[b]->mAngularVelocity = newAngVelocity;
-
             // Add the split impulse velocity from Contact Solver (only used
             // to update the position)
             if (mContactSolver.isSplitImpulseActive()) {
@@ -199,38 +197,45 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
             const Vector3& currentPosition = bodies[b]->getTransform().getPosition();
             const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
 
-            // Compute the new position of the body
+            // Update the new constrained position and orientation of the body
-            Vector3 newPosition = currentPosition + newLinVelocity * dt;
+            mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * dt;
-            Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) *
+            mConstrainedOrientations[indexArray] = currentOrientation +
-                    currentOrientation * decimal(0.5) * dt;
+                                                   Quaternion(0, newAngVelocity) *
-
+                                                   currentOrientation * decimal(0.5) * dt;
-            // Update the world-space center of mass
-            // TODO : IMPLEMENT THIS
-
-            // Update the Transform of the body
-            Transform newTransform(newPosition, newOrientation.getUnit());
-            bodies[b]->setTransform(newTransform);
         }
     }
 }
 
-// Update the AABBs of the bodies
+// Update the postion/orientation of the bodies
-void DynamicsWorld::updateRigidBodiesAABB() {
+void DynamicsWorld::updateBodiesState() {
 
-    PROFILE("DynamicsWorld::updateRigidBodiesAABB()");
+    PROFILE("DynamicsWorld::updateBodiesState()");
 
-    // For each rigid body of the world
+    // For each island of the world
-    set<RigidBody*>::iterator it;
+    for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
-    for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
 
-        // If the body has moved
+        // For each body of the island
-        if ((*it)->mHasMoved) {
+        RigidBody** bodies = mIslands[islandIndex]->getBodies();
 
-            // Update the transform of the body due to the change of its center of mass
+        for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
-            (*it)->updateTransformWithCenterOfMass();
 
-            // Update the AABB of the rigid body
+            uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
-            (*it)->updateAABB();
+
+            // Update the linear and angular velocity of the body
+            bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index];
+            bodies[b]->mAngularVelocity = mConstrainedAngularVelocities[index];
+
+            // Update the position of the center of mass of the body
+            bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index];
+
+            // Update the orientation of the body
+            bodies[b]->mTransform.setOrientation(mConstrainedOrientations[index].getUnit());
+
+            // Update the transform of the body (using the new center of mass and new orientation)
+            bodies[b]->updateTransformWithCenterOfMass();
+
+            // Update the broad-phase state of the body
+            bodies[b]->updateBroadPhaseState();
         }
     }
 }
@@ -263,14 +268,19 @@ void DynamicsWorld::initVelocityArrays() {
             delete[] mSplitAngularVelocities;
         }
         mNbBodiesCapacity = nbBodies;
+        // TODO : Use better memory allocation here
         mSplitLinearVelocities = new Vector3[mNbBodiesCapacity];
         mSplitAngularVelocities = new Vector3[mNbBodiesCapacity];
         mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity];
         mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity];
+        mConstrainedPositions = new Vector3[mNbBodiesCapacity];
+        mConstrainedOrientations = new Quaternion[mNbBodiesCapacity];
         assert(mSplitLinearVelocities != NULL);
         assert(mSplitAngularVelocities != NULL);
         assert(mConstrainedLinearVelocities != NULL);
         assert(mConstrainedAngularVelocities != NULL);
+        assert(mConstrainedPositions != NULL);
+        assert(mConstrainedOrientations != NULL);
     }
 
     // Reset the velocities arrays
@@ -381,6 +391,8 @@ void DynamicsWorld::solveContactsAndConstraints() {
                                                   mConstrainedAngularVelocities);
     mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
                                                      mConstrainedAngularVelocities);
+    mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions,
+                                                    mConstrainedOrientations);
 
     // ---------- Solve velocity constraints for joints and contacts ---------- //
 
@@ -440,10 +452,6 @@ void DynamicsWorld::solvePositionCorrection() {
 
     // ---------- Get the position/orientation of the rigid bodies ---------- //
 
-    // TODO : Use better memory allocation here
-    mConstrainedPositions = std::vector<Vector3>(mRigidBodies.size());
-    mConstrainedOrientations = std::vector<Quaternion>(mRigidBodies.size());
-
     // For each island of the world
     for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
 
@@ -467,27 +475,11 @@ void DynamicsWorld::solvePositionCorrection() {
             // Solve the position constraints
             mConstraintSolver.solvePositionConstraints(mIslands[islandIndex]);
         }
-
-        // ---------- Update the position/orientation of the rigid bodies ---------- //
-
-        for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
-
-            uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
-
-            // Update the position of the center of mass of the body
-            bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index];
-
-            // Update the orientation of the body
-            bodies[b]->mTransform.setOrientation(mConstrainedOrientations[index].getUnit());
-
-            // Update the Transform of the body (using the new center of mass and new orientation)
-            bodies[b]->updateTransformWithCenterOfMass();
-        }
     }
 }
 
 // Create a rigid body into the physics world
-RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass) {
+RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
 
     // Compute the body ID
     bodyindex bodyID = computeNextAvailableBodyID();
@@ -497,17 +489,13 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma
 
     // Create the rigid body
     RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform,
-                                                                                mass, bodyID);
+                                                                                *this, bodyID);
     assert(rigidBody != NULL);
 
     // Add the rigid body to the physics world
     mBodies.insert(rigidBody);
     mRigidBodies.insert(rigidBody);
 
-    // TODO : DELETE THIS
-    // Add the rigid body to the collision detection
-    //mCollisionDetection.addProxyCollisionShape(rigidBody);
-
     // Return the pointer to the rigid body
     return rigidBody;
 }
@@ -515,17 +503,12 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma
 // Destroy a rigid body and all the joints which it belongs
 void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
 
-    // TODO : DELETE THIS
+    // Remove all the collision shapes of the body
-    // Remove the body from the collision detection
+    rigidBody->removeAllCollisionShapes();
-    //mCollisionDetection.removeProxyCollisionShape(rigidBody);
 
     // Add the body ID to the list of free IDs
     mFreeBodiesIDs.push_back(rigidBody->getID());
 
-    // TODO : DELETE THIS
-    // Remove the collision shape from the world
-    //removeCollisionShape(rigidBody->getCollisionShape());
-
     // Destroy all the joints in which the rigid body to be destroyed is involved
     JointListElement* element;
     for (element = rigidBody->mJointsList; element != NULL; element = element->next) {
@@ -665,29 +648,6 @@ void DynamicsWorld::addJointToBody(Joint* joint) {
     joint->mBody2->mJointsList = jointListElement2;
 }
 
-// Add a contact manifold to the linked list of contact manifolds of the two bodies involed
-// in the corresponding contact
-void DynamicsWorld::addContactManifoldToBody(ContactManifold* contactManifold,
-                                             CollisionBody* body1, CollisionBody* body2) {
-
-    assert(contactManifold != NULL);
-
-    // Add the contact manifold at the beginning of the linked
-    // list of contact manifolds of the first body
-    void* allocatedMemory1 = mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
-    ContactManifoldListElement* listElement1 = new (allocatedMemory1)
-                                                  ContactManifoldListElement(contactManifold,
-                                                                     body1->mContactManifoldsList);
-    body1->mContactManifoldsList = listElement1;
-
-    // Add the joint at the beginning of the linked list of joints of the second body
-    void* allocatedMemory2 = mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
-    ContactManifoldListElement* listElement2 = new (allocatedMemory2)
-                                                  ContactManifoldListElement(contactManifold,
-                                                                     body2->mContactManifoldsList);
-    body2->mContactManifoldsList = listElement2;
-}
-
 // Reset all the contact manifolds linked list of each body
 void DynamicsWorld::resetContactManifoldListsOfBodies() {
 
@@ -736,8 +696,8 @@ void DynamicsWorld::computeIslands() {
     for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
         (*it)->mIsAlreadyInIsland = false;
     }
-    for (std::vector<ContactManifold*>::iterator it = mContactManifolds.begin();
+    for (std::vector<ContactManifold*>::iterator it = mCollisionDetection.mContactManifolds.begin();
-         it != mContactManifolds.end(); ++it) {
+         it != mCollisionDetection.mContactManifolds.end(); ++it) {
         (*it)->mIsAlreadyInIsland = false;
     }
     for (std::set<Joint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
@@ -770,7 +730,8 @@ void DynamicsWorld::computeIslands() {
 
         // Create the new island
         void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island));
-        mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies,mContactManifolds.size(),
+        mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies,
+                                                                  mCollisionDetection.mContactManifolds.size(),
                                                                   mJoints.size(), mMemoryAllocator);
 
         // While there are still some bodies to visit in the stack
@@ -919,42 +880,6 @@ void DynamicsWorld::updateSleepingBodies() {
     }
 }
 
-// Notify the world about a new narrow-phase contact
-void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair,
-                                     const ContactPointInfo* contactInfo) {
-
-    // Create a new contact
-    ContactPoint* contact = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(
-                                                                                    *contactInfo);
-    assert(contact != NULL);
-
-    // Get the corresponding overlapping pair
-    pair<bodyindex, bodyindex> indexPair = broadPhasePair->getBodiesIndexPair();
-    OverlappingPair* overlappingPair = mOverlappingPairs.find(indexPair)->second;
-    assert(overlappingPair != NULL);
-
-    // If it is the first contact since the pair are overlapping
-    if (overlappingPair->getNbContactPoints() == 0) {
-
-        // Trigger a callback event
-        if (mEventListener != NULL) mEventListener->beginContact(*contactInfo);
-    }
-
-    // Add the contact to the contact cache of the corresponding overlapping pair
-    overlappingPair->addContact(contact);
-
-    // Add the contact manifold to the world
-    mContactManifolds.push_back(overlappingPair->getContactManifold());
-
-    // Add the contact manifold into the list of contact manifolds
-    // of the two bodies involved in the contact
-    addContactManifoldToBody(overlappingPair->getContactManifold(), overlappingPair->mBody1,
-                             overlappingPair->mBody2);
-
-    // Trigger a callback event for the new contact
-    if (mEventListener != NULL) mEventListener->newContact(*contactInfo);
-}
-
 // Enable/Disable the sleeping technique
 void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
     mIsSleepingEnabled = isSleepingEnabled;

src/engine/DynamicsWorld.h

@@ -72,10 +72,6 @@ class DynamicsWorld : public CollisionWorld {
         /// All the rigid bodies of the physics world
         std::set<RigidBody*> mRigidBodies;
 
-        /// All the contact constraints
-        // TODO : Remove this variable (we will use the ones in the island now)
-        std::vector<ContactManifold*> mContactManifolds;
-
         /// All the joints of the world
         std::set<Joint*> mJoints;
 
@@ -100,10 +96,10 @@ class DynamicsWorld : public CollisionWorld {
         Vector3* mSplitAngularVelocities;
 
         /// Array of constrained rigid bodies position (for position error correction)
-        std::vector<Vector3> mConstrainedPositions;
+        Vector3* mConstrainedPositions;
 
         /// Array of constrained rigid bodies orientation (for position error correction)
-        std::vector<Quaternion> mConstrainedOrientations;
+        Quaternion* mConstrainedOrientations;
 
         /// Map body to their index in the constrained velocities array
         std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
@@ -130,9 +126,6 @@ class DynamicsWorld : public CollisionWorld {
         /// becomes smaller than the sleep velocity.
         decimal mTimeBeforeSleep;
 
-        /// Pointer to an event listener object
-        EventListener* mEventListener;
-
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -154,11 +147,6 @@ class DynamicsWorld : public CollisionWorld {
         void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
                                                 Vector3 newAngVelocity);
 
-        /// Add a contact manifold to the linked list of contact manifolds of the two bodies
-        /// involed in the corresponding contact.
-        void addContactManifoldToBody(ContactManifold* contactManifold,
-                                      CollisionBody *body1, CollisionBody *body2);
-
         /// Compute and set the interpolation factor to all bodies
         void setInterpolationFactorToAllBodies();
 
@@ -180,16 +168,12 @@ class DynamicsWorld : public CollisionWorld {
         /// Compute the islands of awake bodies.
         void computeIslands();
 
+        /// Update the postion/orientation of the bodies
+        void updateBodiesState();
+
         /// Put bodies to sleep if needed.
         void updateSleepingBodies();
 
-        /// Update the overlapping pair
-        virtual void updateOverlappingPair(const BroadPhasePair* pair);
-
-        /// Notify the world about a new narrow-phase contact
-        virtual void notifyNewContact(const BroadPhasePair* pair,
-                                      const ContactPointInfo* contactInfo);
-
     public :
 
         // -------------------- Methods -------------------- //
@@ -226,7 +210,7 @@ class DynamicsWorld : public CollisionWorld {
         void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
 
         /// Create a rigid body into the physics world.
-        RigidBody* createRigidBody(const Transform& transform, decimal mass);
+        RigidBody* createRigidBody(const Transform& transform);
 
         /// Destroy a rigid body and all the joints which it belongs
         void destroyRigidBody(RigidBody* rigidBody);
@@ -359,21 +343,6 @@ inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool
     mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive);
 }
 
-// Update the overlapping pair
-inline void DynamicsWorld::updateOverlappingPair(const BroadPhasePair* pair) {
-
-    // TODO : CHECK WHERE TO CALL THIS METHOD
-
-    // Get the pair of body index
-    std::pair<bodyindex, bodyindex> indexPair = pair->getBodiesIndexPair();
-
-    // Get the corresponding overlapping pair
-    OverlappingPair* overlappingPair = mOverlappingPairs[indexPair];
-
-    // Update the contact cache of the overlapping pair
-    overlappingPair->update();
-}
-
 // Return the gravity vector of the world
 inline Vector3 DynamicsWorld::getGravity() const {
     return mGravity;
@@ -410,13 +379,15 @@ inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator()
 }
 
 // Return a reference to the contact manifolds of the world
+// TODO : DELETE THIS METHOD AND USE EVENT LISTENER IN EXAMPLES INSTEAD
 inline const std::vector<ContactManifold*>& DynamicsWorld::getContactManifolds() const {
-    return mContactManifolds;
+    return mCollisionDetection.mContactManifolds;
 }
 
 // Return the number of contact manifolds in the world
+// TODO : DELETE THIS METHOD AND USE EVENT LISTENER IN EXAMPLES INSTEAD
 inline uint DynamicsWorld::getNbContactManifolds() const {
-    return mContactManifolds.size();
+    return mCollisionDetection.mContactManifolds.size();
 }
 
 // Return the current physics time (in seconds)

src/engine/OverlappingPair.cpp

@@ -30,9 +30,10 @@ using namespace reactphysics3d;
 
 
 // Constructor
-OverlappingPair::OverlappingPair(CollisionBody* body1, CollisionBody* body2,
+OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
-                                 MemoryAllocator &memoryAllocator)
+                                 MemoryAllocator& memoryAllocator)
-                : mBody1(body1), mBody2(body2), mContactManifold(body1, body2, memoryAllocator),
+                : mShape1(shape1), mShape2(shape2),
+                  mContactManifold(shape1->getBody(), shape2->getBody(), memoryAllocator),
                   mCachedSeparatingAxis(1.0, 1.0, 1.0) {
     
 }

src/engine/OverlappingPair.h

@@ -133,7 +133,8 @@ inline void OverlappingPair::addContact(ContactPoint* contact) {
 
 // Update the contact manifold
 inline void OverlappingPair::update() {
-    mContactManifold.update(mShape1->getBody()->getTransform(), mShape2->getBody()->getTransform());
+    mContactManifold.update(mShape1->getBody()->getTransform() *mShape1->getLocalToBodyTransform(),
+                            mShape2->getBody()->getTransform() *mShape2->getLocalToBodyTransform());
 }
 
 // Return the cached separating axis