diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp
index 9c2cd841..a310ab26 100644
--- a/src/body/CollisionBody.cpp
+++ b/src/body/CollisionBody.cpp
@@ -36,7 +36,6 @@ CollisionBody::CollisionBody(const Transform& transform, CollisionWorld& world,
               : Body(id), mType(DYNAMIC), mTransform(transform), mProxyCollisionShapes(NULL),
                 mNbCollisionShapes(0), mContactManifoldsList(NULL), mWorld(world) {
 
-    mIsCollisionEnabled = true;
     mInterpolationFactor = 0.0;
 
     // Initialize the old transform
@@ -305,3 +304,26 @@ bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
 
     return isHit;
 }
+
+// Compute and return the AABB of the body by merging all proxy shapes AABBs
+AABB CollisionBody::getAABB() const {
+
+    AABB bodyAABB;
+
+    if (mProxyCollisionShapes == NULL) return bodyAABB;
+
+    mProxyCollisionShapes->getCollisionShape()->computeAABB(bodyAABB, mTransform * mProxyCollisionShapes->getLocalToBodyTransform());
+
+    // For each proxy shape of the body
+    for (ProxyShape* shape = mProxyCollisionShapes->mNext; shape != NULL; shape = shape->mNext) {
+
+        // Compute the world-space AABB of the collision shape
+        AABB aabb;
+        shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->getLocalToBodyTransform());
+
+        // Merge the proxy shape AABB with the current body AABB
+        bodyAABB.mergeWithAABB(aabb);
+    }
+
+    return bodyAABB;
+}
diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h
index 39baf710..7a656b96 100644
--- a/src/body/CollisionBody.h
+++ b/src/body/CollisionBody.h
@@ -79,9 +79,6 @@ class CollisionBody : public Body {
         /// Interpolation factor used for the state interpolation
         decimal mInterpolationFactor;
 
-        /// True if the body can collide with others bodies
-        bool mIsCollisionEnabled;
-
         /// First element of the linked list of proxy collision shapes of this body
         ProxyShape* mProxyCollisionShapes;
 
@@ -166,7 +163,7 @@ class CollisionBody : public Body {
         void enableCollision(bool isCollisionEnabled);
 
         /// Return the first element of the linked list of contact manifolds involving this body
-        const ContactManifoldListElement* getContactManifoldsLists() const;
+        const ContactManifoldListElement* getContactManifoldsList() const;
 
         /// Return true if a point is inside the collision body
         bool testPointInside(const Vector3& worldPoint) const;
@@ -174,6 +171,27 @@ class CollisionBody : public Body {
         /// Raycast method with feedback information
         bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
 
+        /// Compute and return the AABB of the body by merging all proxy shapes AABBs
+        AABB getAABB() const;
+
+        /// Return the linked list of proxy shapes of that body
+        ProxyShape* getProxyShapesList();
+
+        /// Return the linked list of proxy shapes of that body
+        const ProxyShape* getProxyShapesList() const;
+
+        /// Return the world-space coordinates of a point given the local-space coordinates of the body
+        Vector3 getWorldPoint(const Vector3& localPoint) const;
+
+        /// Return the world-space vector of a vector given in local-space coordinates of the body
+        Vector3 getWorldVector(const Vector3& localVector) const;
+
+        /// Return the body local-space coordinates of a point given in the world-space coordinates
+        Vector3 getLocalPoint(const Vector3& worldPoint) const;
+
+        /// Return the body local-space coordinates of a vector given in the world-space coordinates
+        Vector3 getLocalVector(const Vector3& worldVector) const;
+
         // -------------------- Friendship -------------------- //
 
         friend class CollisionWorld;
@@ -226,16 +244,6 @@ inline void CollisionBody::setTransform(const Transform& transform) {
     updateBroadPhaseState();
 }
 
-// Return true if the body can collide with others bodies
-inline bool CollisionBody::isCollisionEnabled() const {
-    return mIsCollisionEnabled;
-}
-
-// Enable/disable the collision with this body
-inline void CollisionBody::enableCollision(bool isCollisionEnabled) {
-    mIsCollisionEnabled = isCollisionEnabled;
-}
-
 // Update the old transform with the current one.
 /// This is used to compute the interpolated position and orientation of the body
 inline void CollisionBody::updateOldTransform() {
@@ -243,10 +251,40 @@ inline void CollisionBody::updateOldTransform() {
 }
 
 // Return the first element of the linked list of contact manifolds involving this body
-inline const ContactManifoldListElement* CollisionBody::getContactManifoldsLists() const {
+inline const ContactManifoldListElement* CollisionBody::getContactManifoldsList() const {
     return mContactManifoldsList;
 }
 
+// Return the linked list of proxy shapes of that body
+inline ProxyShape* CollisionBody::getProxyShapesList() {
+    return mProxyCollisionShapes;
+}
+
+// Return the linked list of proxy shapes of that body
+inline const ProxyShape* CollisionBody::getProxyShapesList() const {
+    return mProxyCollisionShapes;
+}
+
+// Return the world-space coordinates of a point given the local-space coordinates of the body
+inline Vector3 CollisionBody::getWorldPoint(const Vector3& localPoint) const {
+    return mTransform * localPoint;
+}
+
+// Return the world-space vector of a vector given in local-space coordinates of the body
+inline Vector3 CollisionBody::getWorldVector(const Vector3& localVector) const {
+    return mTransform.getOrientation() * localVector;
+}
+
+// Return the body local-space coordinates of a point given in the world-space coordinates
+inline Vector3 CollisionBody::getLocalPoint(const Vector3& worldPoint) const {
+    return mTransform.getInverse() * worldPoint;
+}
+
+// Return the body local-space coordinates of a vector given in the world-space coordinates
+inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const {
+    return mTransform.getOrientation().getInverse() * worldVector;
+}
+
 }
 
  #endif
diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h
index a8242c3b..e1415d65 100644
--- a/src/body/RigidBody.h
+++ b/src/body/RigidBody.h
@@ -191,6 +191,9 @@ class RigidBody : public CollisionBody {
         /// Return the first element of the linked list of joints involving this body
         const JointListElement* getJointsList() const;
 
+        /// Return the first element of the linked list of joints involving this body
+        JointListElement* getJointsList();
+
         /// Set the variable to know whether or not the body is sleeping
         virtual void setIsSleeping(bool isSleeping);
 
@@ -344,6 +347,11 @@ inline const JointListElement* RigidBody::getJointsList() const {
     return mJointsList;
 }
 
+// Return the first element of the linked list of joints involving this body
+inline JointListElement* RigidBody::getJointsList() {
+    return mJointsList;
+}
+
 // Set the variable to know whether or not the body is sleeping
 inline void RigidBody::setIsSleeping(bool isSleeping) {
 
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index cbb652b5..ba0c77a9 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -51,7 +51,7 @@ CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryAllocator& m
 
 // Destructor
 CollisionDetection::~CollisionDetection() {
-    
+
 }
 
 // Compute the collision detection
@@ -66,6 +66,79 @@ void CollisionDetection::computeCollisionDetection() {
     computeNarrowPhase();
 }
 
+// Compute the collision detection
+void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback,
+                                                    const std::set<uint>& shapes1,
+                                                    const std::set<uint>& shapes2) {
+
+    // Compute the broad-phase collision detection
+    computeBroadPhase();
+
+    // Delete all the contact points in the currently overlapping pairs
+    clearContactPoints();
+
+    // Compute the narrow-phase collision detection among given sets of shapes
+    computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2);
+}
+
+// Report collision between two sets of shapes
+void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback,
+                                                      const std::set<uint>& shapes1,
+                                                      const std::set<uint>& shapes2) {
+
+    // For each possible collision pair of bodies
+    map<overlappingpairid, OverlappingPair*>::iterator it;
+    for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
+
+        OverlappingPair* pair = it->second;
+
+        ProxyShape* shape1 = pair->getShape1();
+        ProxyShape* shape2 = pair->getShape2();
+
+        assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
+
+        // If both shapes1 and shapes2 sets are non-empty, we check that
+        // shape1 is among on set and shape2 is among the other one
+        if (!shapes1.empty() && !shapes2.empty() &&
+            (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) &&
+            (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) {
+            continue;
+        }
+        if (!shapes1.empty() && shapes2.empty() &&
+            shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0)
+        {
+            continue;
+        }
+        if (!shapes2.empty() && shapes1.empty() &&
+            shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0)
+        {
+            continue;
+        }
+
+        // For each contact manifold of the overlapping pair
+        ContactManifold* manifold = pair->getContactManifold();
+        for (int i=0; i<manifold->getNbContactPoints(); i++) {
+
+            ContactPoint* contactPoint = manifold->getContactPoint(i);
+
+            // Create the contact info object for the contact
+            ContactPointInfo* contactInfo = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
+                                         ContactPointInfo(manifold->getShape1(), manifold->getShape2(),
+                                                          contactPoint->getNormal(),
+                                                          contactPoint->getPenetrationDepth(),
+                                                          contactPoint->getLocalPointOnBody1(),
+                                                          contactPoint->getLocalPointOnBody2());
+
+            // Notify the collision callback about this new contact
+            if (callback != NULL) callback->notifyContact(*contactInfo);
+
+            // Delete and remove the contact info from the memory allocator
+            contactInfo->ContactPointInfo::~ContactPointInfo();
+             mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
+        }
+    }
+}
+
 // Compute the broad-phase collision detection
 void CollisionDetection::computeBroadPhase() {
 
@@ -130,7 +203,7 @@ void CollisionDetection::computeNarrowPhase() {
         if (body1->isSleeping() && body2->isSleeping()) continue;
         
         // Select the narrow phase algorithm to use according to the two collision shapes
-        NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm(
+        NarrowPhaseAlgorithm& narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(
                                                         shape1->getCollisionShape(),
                                                         shape2->getCollisionShape());
         
@@ -142,9 +215,120 @@ void CollisionDetection::computeNarrowPhase() {
         if (narrowPhaseAlgorithm.testCollision(shape1, shape2, contactInfo)) {
             assert(contactInfo != NULL);
 
+            // If it is the first contact since the pair are overlapping
+            if (pair->getNbContactPoints() == 0) {
+
+                // Trigger a callback event
+                if (mWorld->mEventListener != NULL) mWorld->mEventListener->beginContact(*contactInfo);
+            }
+
             // Create a new contact
             createContact(pair, contactInfo);
 
+            // Trigger a callback event for the new contact
+            if (mWorld->mEventListener != NULL) mWorld->mEventListener->newContact(*contactInfo);
+
+            // Delete and remove the contact info from the memory allocator
+            contactInfo->ContactPointInfo::~ContactPointInfo();
+            mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
+        }
+    }
+}
+
+// Compute the narrow-phase collision detection
+void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
+                                                         const std::set<uint>& shapes1,
+                                                         const std::set<uint>& shapes2) {
+
+    // For each possible collision pair of bodies
+    map<overlappingpairid, OverlappingPair*>::iterator it;
+    for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
+        ContactPointInfo* contactInfo = NULL;
+
+        OverlappingPair* pair = it->second;
+
+        ProxyShape* shape1 = pair->getShape1();
+        ProxyShape* shape2 = pair->getShape2();
+
+        assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
+
+        bool test1 = shapes1.count(shape1->mBroadPhaseID) == 0;
+        bool test2 = shapes2.count(shape2->mBroadPhaseID) == 0;
+        bool test3 = shapes1.count(shape2->mBroadPhaseID) == 0;
+        bool test4 = shapes2.count(shape1->mBroadPhaseID) == 0;
+        bool test5 = !shapes1.empty() && !shapes2.empty();
+
+        // If both shapes1 and shapes2 sets are non-empty, we check that
+        // shape1 is among on set and shape2 is among the other one
+        if (!shapes1.empty() && !shapes2.empty() &&
+            (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) &&
+            (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) {
+            ++it;
+            continue;
+        }
+        if (!shapes1.empty() && shapes2.empty() &&
+            shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0)
+        {
+            ++it;
+            continue;
+        }
+        if (!shapes2.empty() && shapes1.empty() &&
+            shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0)
+        {
+            ++it;
+            continue;
+        }
+
+        // Check that the two shapes are overlapping. If the shapes are not overlapping
+        // anymore, we remove the overlapping pair.
+        if (!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
+
+            std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
+            ++it;
+
+            // Destroy the overlapping pair
+            itToRemove->second->OverlappingPair::~OverlappingPair();
+            mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
+            mOverlappingPairs.erase(itToRemove);
+            continue;
+        }
+        else {
+            ++it;
+        }
+
+        CollisionBody* const body1 = shape1->getBody();
+        CollisionBody* const body2 = shape2->getBody();
+
+        // Update the contact cache of the overlapping 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;
+        bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
+        if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
+
+        // Check if the two bodies are sleeping, if so, we do no test collision between them
+        if (body1->isSleeping() && body2->isSleeping()) continue;
+
+        // Select the narrow phase algorithm to use according to the two collision shapes
+        NarrowPhaseAlgorithm& narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(
+                                                        shape1->getCollisionShape(),
+                                                        shape2->getCollisionShape());
+
+        // Notify the narrow-phase algorithm about the overlapping pair we are going to test
+        narrowPhaseAlgorithm.setCurrentOverlappingPair(pair);
+
+        // Use the narrow-phase collision detection algorithm to check
+        // if there really is a collision
+        if (narrowPhaseAlgorithm.testCollision(shape1, shape2, contactInfo)) {
+            assert(contactInfo != NULL);
+
+            // Create a new contact
+            createContact(pair, contactInfo);
+
+            // Notify the collision callback about this new contact
+            if (callback != NULL) callback->notifyContact(*contactInfo);
+
             // Delete and remove the contact info from the memory allocator
             contactInfo->ContactPointInfo::~ContactPointInfo();
             mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
@@ -210,13 +394,6 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair,
                                 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);
 
@@ -225,9 +402,6 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair,
     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
@@ -253,3 +427,13 @@ void CollisionDetection::addContactManifoldToBody(ContactManifold* contactManifo
                                                                      body2->mContactManifoldsList);
     body2->mContactManifoldsList = listElement2;
 }
+
+// Delete all the contact points in the currently overlapping pairs
+void CollisionDetection::clearContactPoints() {
+
+    // For each overlapping pair
+    std::map<overlappingpairid, OverlappingPair*>::iterator it;
+    for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
+        it->second->clearContactPoints();
+    }
+}
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index 8d619621..5e851ffc 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -45,6 +45,7 @@ namespace reactphysics3d {
 // Declarations
 class BroadPhaseAlgorithm;
 class CollisionWorld;
+class CollisionCallback;
 
 // Class CollisionDetection
 /**
@@ -95,7 +96,7 @@ class CollisionDetection {
         void computeNarrowPhase();
 
         /// Select the narrow phase algorithm to use given two collision shapes
-        NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(const CollisionShape* collisionShape1,
+        NarrowPhaseAlgorithm& selectNarrowPhaseAlgorithm(const CollisionShape* collisionShape1,
                                                          const CollisionShape* collisionShape2);
 
         /// Create a new contact
@@ -105,6 +106,9 @@ class CollisionDetection {
         /// involed in the corresponding contact.
         void addContactManifoldToBody(ContactManifold* contactManifold,
                                       CollisionBody *body1, CollisionBody *body2);
+
+        /// Delete all the contact points in the currently overlapping pairs
+        void clearContactPoints();
    
     public :
 
@@ -138,12 +142,35 @@ class CollisionDetection {
         /// Compute the collision detection
         void computeCollisionDetection();
 
+        /// Compute the collision detection
+        void testCollisionBetweenShapes(CollisionCallback* callback,
+                                        const std::set<uint>& shapes1,
+                                        const std::set<uint>& shapes2);
+
+        /// Report collision between two sets of shapes
+        void reportCollisionBetweenShapes(CollisionCallback* callback,
+                                          const std::set<uint>& shapes1,
+                                          const std::set<uint>& shapes2) ;
+
         /// Ray casting method
         void raycast(RaycastCallback* raycastCallback, const Ray& ray) const;
 
+        /// Test if the AABBs of two bodies overlap
+        bool testAABBOverlap(const CollisionBody* body1,
+                             const CollisionBody* body2) const;
+
+        /// Test if the AABBs of two proxy shapes overlap
+        bool testAABBOverlap(const ProxyShape* shape1,
+                             const ProxyShape* shape2) const;
+
         /// Allow the broadphase to notify the collision detection about an overlapping pair.
         void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
 
+        // Compute the narrow-phase collision detection
+        void computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
+                                             const std::set<uint>& shapes1,
+                                             const std::set<uint>& shapes2);
+
         // -------------------- Friendship -------------------- //
 
         friend class DynamicsWorld;
@@ -151,7 +178,7 @@ class CollisionDetection {
 };
 
 // Select the narrow-phase collision algorithm to use given two collision shapes
-inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm(
+inline NarrowPhaseAlgorithm& CollisionDetection::selectNarrowPhaseAlgorithm(
                              const CollisionShape* collisionShape1,
                              const CollisionShape* collisionShape2) {
     
@@ -210,6 +237,18 @@ inline void CollisionDetection::raycast(RaycastCallback* raycastCallback,
     mBroadPhaseAlgorithm.raycast(ray, rayCastTest);
 }
 
+// Test if the AABBs of two proxy shapes overlap
+inline bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1,
+                                                const ProxyShape* shape2) const {
+
+    // If one of the shape's body is not active, we return no overlap
+    if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) {
+        return false;
+    }
+
+    return mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2);
+}
+
 }
 
 #endif
diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h
index fa38a68c..3f498178 100644
--- a/src/collision/ProxyShape.h
+++ b/src/collision/ProxyShape.h
@@ -64,6 +64,12 @@ class ProxyShape {
         /// Return the collision shape margin
         decimal getMargin() const;
 
+        /// Return the next proxy shape in the linked list of proxy shapes
+        ProxyShape* getNext();
+
+        /// Return the next proxy shape in the linked list of proxy shapes
+        const ProxyShape* getNext() const;
+
     public:
 
         // -------------------- Methods -------------------- //
@@ -110,6 +116,8 @@ class ProxyShape {
         friend class BroadPhaseAlgorithm;
         friend class DynamicAABBTree;
         friend class CollisionDetection;
+        friend class CollisionWorld;
+        friend class DynamicsWorld;
         friend class EPAAlgorithm;
         friend class GJKAlgorithm;
         friend class ConvexMeshShape;
@@ -174,6 +182,16 @@ inline bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
     return mCollisionShape->raycast(ray, raycastInfo, this);
 }
 
+// Return the next proxy shape in the linked list of proxy shapes
+inline ProxyShape* ProxyShape::getNext() {
+    return mNext;
+}
+
+// Return the next proxy shape in the linked list of proxy shapes
+inline const ProxyShape* ProxyShape::getNext() const {
+    return mNext;
+}
+
 }
 
 #endif
diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h
index 6e4e8737..da9c6579 100644
--- a/src/collision/broadphase/BroadPhaseAlgorithm.h
+++ b/src/collision/broadphase/BroadPhaseAlgorithm.h
@@ -156,7 +156,7 @@ class BroadPhaseAlgorithm {
         void computeOverlappingPairs();
 
         /// Return true if the two broad-phase collision shapes are overlapping
-        bool testOverlappingShapes(ProxyShape* shape1, ProxyShape* shape2) const;
+        bool testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const;
 
         /// Ray casting method
         void raycast(const Ray& ray, RaycastTest& raycastTest) const;
@@ -173,8 +173,8 @@ inline bool BroadPair::smallerThan(const BroadPair& pair1, const BroadPair& pair
 }
 
 // Return true if the two broad-phase collision shapes are overlapping
-inline bool BroadPhaseAlgorithm::testOverlappingShapes(ProxyShape* shape1,
-                                                       ProxyShape* shape2) const {
+inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1,
+                                                       const ProxyShape* shape2) const {
     // Get the two AABBs of the collision shapes
     const AABB& aabb1 = mDynamicAABBTree.getFatAABB(shape1->mBroadPhaseID);
     const AABB& aabb2 = mDynamicAABBTree.getFatAABB(shape2->mBroadPhaseID);
diff --git a/src/collision/shapes/AABB.cpp b/src/collision/shapes/AABB.cpp
index 6f4fc7ef..a18e1c2b 100644
--- a/src/collision/shapes/AABB.cpp
+++ b/src/collision/shapes/AABB.cpp
@@ -53,6 +53,17 @@ AABB::~AABB() {
 
 }
 
+// Merge the AABB in parameter with the current one
+void AABB::mergeWithAABB(const AABB& aabb) {
+    mMinCoordinates.x = std::min(mMinCoordinates.x, aabb.mMinCoordinates.x);
+    mMinCoordinates.y = std::min(mMinCoordinates.y, aabb.mMinCoordinates.y);
+    mMinCoordinates.z = std::min(mMinCoordinates.z, aabb.mMinCoordinates.z);
+
+    mMaxCoordinates.x = std::max(mMaxCoordinates.x, aabb.mMaxCoordinates.x);
+    mMaxCoordinates.y = std::max(mMaxCoordinates.y, aabb.mMaxCoordinates.y);
+    mMaxCoordinates.z = std::max(mMaxCoordinates.z, aabb.mMaxCoordinates.z);
+}
+
 // Replace the current AABB with a new AABB that is the union of two AABBs in parameters
 void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) {
     mMinCoordinates.x = std::min(aabb1.mMinCoordinates.x, aabb2.mMinCoordinates.x);
diff --git a/src/collision/shapes/AABB.h b/src/collision/shapes/AABB.h
index 5a557e95..24517b74 100644
--- a/src/collision/shapes/AABB.h
+++ b/src/collision/shapes/AABB.h
@@ -88,6 +88,9 @@ class AABB {
         /// Return the volume of the AABB
         decimal getVolume() const;
 
+        /// Merge the AABB in parameter with the current one
+        void mergeWithAABB(const AABB& aabb);
+
         /// Replace the current AABB with a new AABB that is the union of two AABBs in parameters
         void mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2);
 
diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp
index c15f70d1..a0813339 100644
--- a/src/engine/CollisionWorld.cpp
+++ b/src/engine/CollisionWorld.cpp
@@ -166,4 +166,124 @@ void CollisionWorld::removeCollisionShape(CollisionShape* collisionShape) {
     }
 }
 
+// Reset all the contact manifolds linked list of each body
+void CollisionWorld::resetContactManifoldListsOfBodies() {
+
+    // For each rigid body of the world
+    for (std::set<CollisionBody*>::iterator it = mBodies.begin(); it != mBodies.end(); ++it) {
+
+        // Reset the contact manifold list of the body
+        (*it)->resetContactManifoldsList();
+    }
+}
+
+// Test if the AABBs of two bodies overlap
+bool CollisionWorld::testAABBOverlap(const CollisionBody* body1,
+                                     const CollisionBody* body2) const {
+
+    // If one of the body is not active, we return no overlap
+    if (!body1->isActive() || !body2->isActive()) return false;
+
+    // Compute the AABBs of both bodies
+    AABB body1AABB = body1->getAABB();
+    AABB body2AABB = body2->getAABB();
+
+    // Return true if the two AABBs overlap
+    return body1AABB.testCollision(body2AABB);
+}
+
+// Test and report collisions between a given shape and all the others
+// shapes of the world.
+void CollisionWorld::testCollision(const ProxyShape* shape,
+                                   CollisionCallback* callback) {
+
+    // Reset all the contact manifolds lists of each body
+    resetContactManifoldListsOfBodies();
+
+    // Create the sets of shapes
+    std::set<uint> shapes;
+    shapes.insert(shape->mBroadPhaseID);
+    std::set<uint> emptySet;
+
+    // Perform the collision detection and report contacts
+    mCollisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet);
+}
+
+// Test and report collisions between two given shapes
+void CollisionWorld::testCollision(const ProxyShape* shape1,
+                                   const ProxyShape* shape2,
+                                   CollisionCallback* callback) {
+
+    // Reset all the contact manifolds lists of each body
+    resetContactManifoldListsOfBodies();
+
+    // Create the sets of shapes
+    std::set<uint> shapes1;
+    shapes1.insert(shape1->mBroadPhaseID);
+    std::set<uint> shapes2;
+    shapes2.insert(shape2->mBroadPhaseID);
+
+    // Perform the collision detection and report contacts
+    mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
+}
+
+// Test and report collisions between a body and all the others bodies of the
+// world
+void CollisionWorld::testCollision(const CollisionBody* body,
+                                   CollisionCallback* callback) {
+
+    // Reset all the contact manifolds lists of each body
+    resetContactManifoldListsOfBodies();
+
+    // Create the sets of shapes
+    std::set<uint> shapes1;
+
+    // For each shape of the body
+    for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL;
+         shape = shape->getNext()) {
+        shapes1.insert(shape->mBroadPhaseID);
+    }
+
+    std::set<uint> emptySet;
+
+    // Perform the collision detection and report contacts
+    mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet);
+}
+
+// Test and report collisions between two bodies
+void CollisionWorld::testCollision(const CollisionBody* body1,
+                                   const CollisionBody* body2,
+                                   CollisionCallback* callback) {
+
+    // Reset all the contact manifolds lists of each body
+    resetContactManifoldListsOfBodies();
+
+    // Create the sets of shapes
+    std::set<uint> shapes1;
+    for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL;
+         shape = shape->getNext()) {
+        shapes1.insert(shape->mBroadPhaseID);
+    }
+
+    std::set<uint> shapes2;
+    for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL;
+         shape = shape->getNext()) {
+        shapes2.insert(shape->mBroadPhaseID);
+    }
+
+    // Perform the collision detection and report contacts
+    mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
+}
+
+// Test and report collisions between all shapes of the world
+void CollisionWorld::testCollision(CollisionCallback* callback) {
+
+    // Reset all the contact manifolds lists of each body
+    resetContactManifoldListsOfBodies();
+
+    std::set<uint> emptySet;
+
+    // Perform the collision detection and report contacts
+    mCollisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet);
+}
 
diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h
index 8a33da30..69495c37 100644
--- a/src/engine/CollisionWorld.h
+++ b/src/engine/CollisionWorld.h
@@ -45,6 +45,9 @@
 /// Namespace reactphysics3d
 namespace reactphysics3d {
 
+// Declarations
+class CollisionCallback;
+
 // Class CollisionWorld
 /**
  * This class represent a world where it is possible to move bodies
@@ -95,6 +98,9 @@ class CollisionWorld {
         /// Create a new collision shape in the world.
         CollisionShape* createCollisionShape(const CollisionShape& collisionShape);
 
+        /// Reset all the contact manifolds linked list of each body
+        void resetContactManifoldListsOfBodies();
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -120,6 +126,37 @@ class CollisionWorld {
         /// Ray cast method
         void raycast(const Ray& ray, RaycastCallback* raycastCallback) const;
 
+        /// Test if the AABBs of two bodies overlap
+        bool testAABBOverlap(const CollisionBody* body1,
+                             const CollisionBody* body2) const;
+
+        /// Test if the AABBs of two proxy shapes overlap
+        bool testAABBOverlap(const ProxyShape* shape1,
+                             const ProxyShape* shape2) const;
+
+        /// Test and report collisions between a given shape and all the others
+        /// shapes of the world
+        virtual void testCollision(const ProxyShape* shape,
+                                   CollisionCallback* callback);
+
+        /// Test and report collisions between two given shapes
+        virtual void testCollision(const ProxyShape* shape1,
+                                   const ProxyShape* shape2,
+                                   CollisionCallback* callback);
+
+        /// Test and report collisions between a body and all the others bodies of the
+        /// world
+        virtual void testCollision(const CollisionBody* body,
+                                   CollisionCallback* callback);
+
+        /// Test and report collisions between two bodies
+        virtual void testCollision(const CollisionBody* body1,
+                                   const CollisionBody* body2,
+                                   CollisionCallback* callback);
+
+        /// Test and report collisions between all shapes of the world
+        virtual void testCollision(CollisionCallback* callback);
+
         // -------------------- Friendship -------------------- //
 
         friend class CollisionDetection;
@@ -144,6 +181,33 @@ inline void CollisionWorld::raycast(const Ray& ray,
     mCollisionDetection.raycast(raycastCallback, ray);
 }
 
+// Test if the AABBs of two proxy shapes overlap
+inline bool CollisionWorld::testAABBOverlap(const ProxyShape* shape1,
+                                            const ProxyShape* shape2) const {
+
+    return mCollisionDetection.testAABBOverlap(shape1, shape2);
+}
+
+// Class CollisionCallback
+/**
+ * This class can be used to register a callback for collision test queries.
+ * You should implement your own class inherited from this one and implement
+ * the notifyRaycastHit() method. This method will be called for each ProxyShape
+ * that is hit by the ray.
+ */
+class CollisionCallback {
+
+    public:
+
+        /// Destructor
+        virtual ~CollisionCallback() {
+
+        }
+
+        /// This method will be called for contact
+        virtual void notifyContact(const ContactPointInfo& contactPointInfo)=0;
+};
+
 }
 
  #endif
diff --git a/src/engine/ContactManifold.cpp b/src/engine/ContactManifold.cpp
index b2b9987e..4249afe2 100644
--- a/src/engine/ContactManifold.cpp
+++ b/src/engine/ContactManifold.cpp
@@ -30,9 +30,9 @@
 using namespace reactphysics3d;
 
 // Constructor
-ContactManifold::ContactManifold(CollisionBody* body1, CollisionBody* body2,
+ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
                                  MemoryAllocator& memoryAllocator)
-                : mBody1(body1), mBody2(body2), mNbContactPoints(0), mFrictionImpulse1(0.0),
+                : mShape1(shape1), mShape2(shape2), mNbContactPoints(0), mFrictionImpulse1(0.0),
                   mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
                   mMemoryAllocator(memoryAllocator) {
     
diff --git a/src/engine/ContactManifold.h b/src/engine/ContactManifold.h
index 7d6c5d19..86e8de87 100644
--- a/src/engine/ContactManifold.h
+++ b/src/engine/ContactManifold.h
@@ -29,6 +29,7 @@
 // Libraries
 #include <vector>
 #include "body/CollisionBody.h"
+#include "collision/ProxyShape.h"
 #include "constraint/ContactPoint.h"
 #include "memory/MemoryAllocator.h"
 
@@ -88,11 +89,11 @@ class ContactManifold {
 
         // -------------------- Attributes -------------------- //
 
-        /// Pointer to the first body of the contact
-        CollisionBody* mBody1;
+        /// Pointer to the first proxy shape of the contact
+        ProxyShape* mShape1;
 
-        /// Pointer to the second body of the contact
-        CollisionBody* mBody2;
+        /// Pointer to the second proxy shape of the contact
+        ProxyShape* mShape2;
 
         /// Contact points in the manifold
         ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
@@ -129,12 +130,6 @@ class ContactManifold {
         /// Private assignment operator
         ContactManifold& operator=(const ContactManifold& contactManifold);
 
-        /// Return a pointer to the first body of the contact manifold
-        CollisionBody* getBody1() const;
-
-        /// Return a pointer to the second body of the contact manifold
-        CollisionBody* getBody2() const;
-
         /// Return the index of maximum area
         int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const;
 
@@ -155,12 +150,24 @@ class ContactManifold {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactManifold(CollisionBody* body1, CollisionBody* body2,
+        ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
                         MemoryAllocator& memoryAllocator);
 
         /// Destructor
         ~ContactManifold();
 
+        /// Return a pointer to the first proxy shape of the contact
+        ProxyShape* getShape1() const;
+
+        /// Return a pointer to the second proxy shape of the contact
+        ProxyShape* getShape2() const;
+
+        /// Return a pointer to the first body of the contact manifold
+        CollisionBody* getBody1() const;
+
+        /// Return a pointer to the second body of the contact manifold
+        CollisionBody* getBody2() const;
+
         /// Add a contact point to the manifold
         void addContactPoint(ContactPoint* contact);
 
@@ -213,14 +220,24 @@ class ContactManifold {
         friend class CollisionBody;
 };
 
+// Return a pointer to the first proxy shape of the contact
+inline ProxyShape* ContactManifold::getShape1() const {
+    return mShape1;
+}
+
+// Return a pointer to the second proxy shape of the contact
+inline ProxyShape* ContactManifold::getShape2() const {
+    return mShape2;
+}
+
 // Return a pointer to the first body of the contact manifold
 inline CollisionBody* ContactManifold::getBody1() const {
-    return mBody1;
+    return mShape1->getBody();
 }
 
 // Return a pointer to the second body of the contact manifold
 inline CollisionBody* ContactManifold::getBody2() const {
-    return mBody2;
+    return mShape2->getBody();
 }
 
 // Return the number of contact points in the manifold
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index e6d0ae16..3ade5ce2 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -34,8 +34,6 @@
 using namespace reactphysics3d;
 using namespace std;
 
-// TODO : Check if we really need to store the contact manifolds also in mContactManifolds.
-
 // Constructor
 DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
               : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityEnabled(true),
@@ -621,17 +619,6 @@ void DynamicsWorld::addJointToBody(Joint* joint) {
     joint->mBody2->mJointsList = jointListElement2;
 }
 
-// Reset all the contact manifolds linked list of each body
-void DynamicsWorld::resetContactManifoldListsOfBodies() {
-
-    // For each rigid body of the world
-    for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
-
-        // Reset the contact manifold list of the body
-        (*it)->resetContactManifoldsList();
-    }
-}
-
 // Compute the islands of awake bodies.
 /// An island is an isolated group of rigid bodies that have constraints (joints or contacts)
 /// between each other. This method computes the islands at each time step as follows: For each
@@ -867,3 +854,93 @@ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
         }
     }
 }
+
+// Test and report collisions between a given shape and all the others
+// shapes of the world.
+/// This method should be called after calling the
+/// DynamicsWorld::update() method that will compute the collisions.
+void DynamicsWorld::testCollision(const ProxyShape* shape,
+                                   CollisionCallback* callback) {
+
+    // Create the sets of shapes
+    std::set<uint> shapes;
+    shapes.insert(shape->mBroadPhaseID);
+    std::set<uint> emptySet;
+
+    // Perform the collision detection and report contacts
+    mCollisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet);
+}
+
+// Test and report collisions between two given shapes.
+/// This method should be called after calling the
+/// DynamicsWorld::update() method that will compute the collisions.
+void DynamicsWorld::testCollision(const ProxyShape* shape1,
+                                   const ProxyShape* shape2,
+                                   CollisionCallback* callback) {
+
+    // Create the sets of shapes
+    std::set<uint> shapes1;
+    shapes1.insert(shape1->mBroadPhaseID);
+    std::set<uint> shapes2;
+    shapes2.insert(shape2->mBroadPhaseID);
+
+    // Perform the collision detection and report contacts
+    mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
+}
+
+// Test and report collisions between a body and all the others bodies of the
+// world.
+/// This method should be called after calling the
+/// DynamicsWorld::update() method that will compute the collisions.
+void DynamicsWorld::testCollision(const CollisionBody* body,
+                                   CollisionCallback* callback) {
+
+    // Create the sets of shapes
+    std::set<uint> shapes1;
+
+    // For each shape of the body
+    for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL;
+         shape = shape->getNext()) {
+        shapes1.insert(shape->mBroadPhaseID);
+    }
+
+    std::set<uint> emptySet;
+
+    // Perform the collision detection and report contacts
+    mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet);
+}
+
+// Test and report collisions between two bodies.
+/// This method should be called after calling the
+/// DynamicsWorld::update() method that will compute the collisions.
+void DynamicsWorld::testCollision(const CollisionBody* body1,
+                                   const CollisionBody* body2,
+                                   CollisionCallback* callback) {
+
+    // Create the sets of shapes
+    std::set<uint> shapes1;
+    for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL;
+         shape = shape->getNext()) {
+        shapes1.insert(shape->mBroadPhaseID);
+    }
+
+    std::set<uint> shapes2;
+    for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL;
+         shape = shape->getNext()) {
+        shapes2.insert(shape->mBroadPhaseID);
+    }
+
+    // Perform the collision detection and report contacts
+    mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
+}
+
+// Test and report collisions between all shapes of the world.
+/// This method should be called after calling the
+/// DynamicsWorld::update() method that will compute the collisions.
+void DynamicsWorld::testCollision(CollisionCallback* callback) {
+
+    std::set<uint> emptySet;
+
+    // Perform the collision detection and report contacts
+    mCollisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet);
+}
diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h
index 7fd53cd6..83e5d6d2 100644
--- a/src/engine/DynamicsWorld.h
+++ b/src/engine/DynamicsWorld.h
@@ -224,9 +224,6 @@ class DynamicsWorld : public CollisionWorld {
         /// Add the joint to the list of joints of the two bodies involved in the joint
         void addJointToBody(Joint* joint);
 
-        /// Reset all the contact manifolds linked list of each body
-        void resetContactManifoldListsOfBodies();
-
         /// Return the gravity vector of the world
         Vector3 getGravity() const;
 
@@ -278,6 +275,29 @@ class DynamicsWorld : public CollisionWorld {
         /// Set an event listener object to receive events callbacks.
         void setEventListener(EventListener* eventListener);
 
+        /// Test and report collisions between a given shape and all the others
+        /// shapes of the world
+        virtual void testCollision(const ProxyShape* shape,
+                                   CollisionCallback* callback);
+
+        /// Test and report collisions between two given shapes
+        virtual void testCollision(const ProxyShape* shape1,
+                                   const ProxyShape* shape2,
+                                   CollisionCallback* callback);
+
+        /// Test and report collisions between a body and all
+        /// the others bodies of the world
+        virtual void testCollision(const CollisionBody* body,
+                                   CollisionCallback* callback);
+
+        /// Test and report collisions between two bodies
+        virtual void testCollision(const CollisionBody* body1,
+                                   const CollisionBody* body2,
+                                   CollisionCallback* callback);
+
+        /// Test and report collisions between all shapes of the world
+        virtual void testCollision(CollisionCallback* callback);
+
         // -------------------- Friendship -------------------- //
 
         friend class RigidBody;
diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp
index 915c40dd..7f81ab20 100644
--- a/src/engine/OverlappingPair.cpp
+++ b/src/engine/OverlappingPair.cpp
@@ -33,7 +33,7 @@ using namespace reactphysics3d;
 OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
                                  MemoryAllocator& memoryAllocator)
                 : mShape1(shape1), mShape2(shape2),
-                  mContactManifold(shape1->getBody(), shape2->getBody(), memoryAllocator),
+                  mContactManifold(shape1, shape2, memoryAllocator),
                   mCachedSeparatingAxis(1.0, 1.0, 1.0) {
     
 }
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index fb0e9f25..0397f93b 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -106,6 +106,9 @@ class OverlappingPair {
         /// Return the contact manifold
         ContactManifold* getContactManifold();
 
+        /// Clear the contact points of the contact manifold
+        void clearContactPoints();
+
         /// Return the pair of bodies index
         static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
 
@@ -183,6 +186,11 @@ inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body
     return indexPair;
 }
 
+// Clear the contact points of the contact manifold
+inline void OverlappingPair::clearContactPoints() {
+   mContactManifold.clear();
+}
+
 }
 
 #endif
diff --git a/test/main.cpp b/test/main.cpp
index 78564a99..e66f8b40 100644
--- a/test/main.cpp
+++ b/test/main.cpp
@@ -33,6 +33,7 @@
 #include "tests/mathematics/TestMatrix3x3.h"
 #include "tests/collision/TestPointInside.h"
 #include "tests/collision/TestRaycast.h"
+#include "tests/collision/TestCollisionWorld.h"
 
 using namespace reactphysics3d;
 
@@ -53,6 +54,7 @@ int main() {
 
     testSuite.addTest(new TestPointInside("IsPointInside"));
     testSuite.addTest(new TestRaycast("Raycasting"));
+    testSuite.addTest(new TestCollisionWorld("CollisionWorld"));
 
     // Run the tests
     testSuite.run();
diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h
new file mode 100644
index 00000000..465d1493
--- /dev/null
+++ b/test/tests/collision/TestCollisionWorld.h
@@ -0,0 +1,269 @@
+/********************************************************************************
+* 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 TEST_COLLISION_WORLD_H
+#define TEST_COLLISION_WORLD_H
+
+// Libraries
+#include "reactphysics3d.h"
+
+/// Reactphysics3D namespace
+namespace reactphysics3d {
+
+// Class
+class WorldCollisionCallback : public CollisionCallback
+{
+    public:
+
+        bool boxCollideWithSphere1;
+        bool boxCollideWithCylinder;
+        bool sphere1CollideWithCylinder;
+        bool sphere1CollideWithSphere2;
+
+        CollisionBody* boxBody;
+        CollisionBody* sphere1Body;
+        CollisionBody* sphere2Body;
+        CollisionBody* cylinderBody;
+
+        WorldCollisionCallback()
+        {
+            reset();
+        }
+
+        void reset()
+        {
+            boxCollideWithSphere1 = false;
+            boxCollideWithCylinder = false;
+            sphere1CollideWithCylinder = false;
+            sphere1CollideWithSphere2 = false;
+        }
+
+
+        // This method will be called for contact
+        virtual void notifyContact(const ContactPointInfo& contactPointInfo) {
+
+            if (isContactBetweenBodies(boxBody, sphere1Body, contactPointInfo)) {
+                boxCollideWithSphere1 = true;
+            }
+            else if (isContactBetweenBodies(boxBody, cylinderBody, contactPointInfo)) {
+                boxCollideWithCylinder = true;
+            }
+            else if (isContactBetweenBodies(sphere1Body, cylinderBody, contactPointInfo)) {
+                sphere1CollideWithCylinder = true;
+            }
+            else if (isContactBetweenBodies(sphere1Body, sphere2Body, contactPointInfo)) {
+                sphere1CollideWithSphere2 = true;
+            }
+        }
+
+        bool isContactBetweenBodies(const CollisionBody* body1, const CollisionBody* body2,
+                                     const ContactPointInfo& contactPointInfo) {
+            return (contactPointInfo.shape1->getBody()->getID() == body1->getID() &&
+                    contactPointInfo.shape2->getBody()->getID() == body2->getID()) ||
+                   (contactPointInfo.shape2->getBody()->getID() == body1->getID() &&
+                    contactPointInfo.shape1->getBody()->getID() == body2->getID());
+        }
+};
+
+// Class TestCollisionWorld
+/**
+ * Unit test for the CollisionWorld class.
+ */
+class TestCollisionWorld : public Test {
+
+    private :
+
+        // ---------- Atributes ---------- //
+
+        // Physics world
+        CollisionWorld* mWorld;
+
+        // Bodies
+        CollisionBody* mBoxBody;
+        CollisionBody* mSphere1Body;
+        CollisionBody* mSphere2Body;
+        CollisionBody* mCylinderBody;
+
+        // Shapes
+        ProxyShape* mBoxShape;
+        ProxyShape* mSphere1Shape;
+        ProxyShape* mSphere2Shape;
+        ProxyShape* mCylinderShape;
+
+        // Collision callback class
+        WorldCollisionCallback mCollisionCallback;
+
+    public :
+
+        // ---------- Methods ---------- //
+
+        /// Constructor
+        TestCollisionWorld(const std::string& name) : Test(name) {
+
+            // Create the world
+            mWorld = new CollisionWorld();
+
+            // Create the bodies
+            Transform boxTransform(Vector3(10, 0, 0), Quaternion::identity());
+            mBoxBody = mWorld->createCollisionBody(boxTransform);
+            BoxShape boxShape(Vector3(3, 3, 3));
+            mBoxShape = mBoxBody->addCollisionShape(boxShape, Transform::identity());
+
+            SphereShape sphereShape(3.0);
+            Transform sphere1Transform(Vector3(10,5, 0), Quaternion::identity());
+            mSphere1Body = mWorld->createCollisionBody(sphere1Transform);
+            mSphere1Shape = mSphere1Body->addCollisionShape(sphereShape, Transform::identity());
+
+            Transform sphere2Transform(Vector3(30, 10, 10), Quaternion::identity());
+            mSphere2Body = mWorld->createCollisionBody(sphere2Transform);
+            mSphere2Shape = mSphere2Body->addCollisionShape(sphereShape, Transform::identity());
+
+            Transform cylinderTransform(Vector3(10, -5, 0), Quaternion::identity());
+            mCylinderBody = mWorld->createCollisionBody(cylinderTransform);
+            CylinderShape cylinderShape(2, 5);
+            mCylinderShape = mCylinderBody->addCollisionShape(cylinderShape, Transform::identity());
+
+            mCollisionCallback.boxBody = mBoxBody;
+            mCollisionCallback.sphere1Body = mSphere1Body;
+            mCollisionCallback.sphere2Body = mSphere2Body;
+            mCollisionCallback.cylinderBody = mCylinderBody;
+        }
+
+        /// Run the tests
+        void run() {
+
+            testCollisions();
+        }
+
+        void testCollisions() {
+
+            mCollisionCallback.reset();
+            mWorld->testCollision(&mCollisionCallback);
+            test(mCollisionCallback.boxCollideWithSphere1);
+            test(mCollisionCallback.boxCollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithSphere2);
+
+            test(mWorld->testAABBOverlap(mBoxBody, mSphere1Body));
+            test(mWorld->testAABBOverlap(mBoxBody, mCylinderBody));
+            test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody));
+            test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body));
+
+            test(mWorld->testAABBOverlap(mBoxShape, mSphere1Shape));
+            test(mWorld->testAABBOverlap(mBoxShape, mCylinderShape));
+            test(!mWorld->testAABBOverlap(mSphere1Shape, mCylinderShape));
+            test(!mWorld->testAABBOverlap(mSphere1Shape, mSphere2Shape));
+
+            mCollisionCallback.reset();
+            mWorld->testCollision(mCylinderBody, &mCollisionCallback);
+            test(!mCollisionCallback.boxCollideWithSphere1);
+            test(mCollisionCallback.boxCollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithSphere2);
+
+            mCollisionCallback.reset();
+            mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback);
+            test(mCollisionCallback.boxCollideWithSphere1);
+            test(!mCollisionCallback.boxCollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithSphere2);
+
+            mCollisionCallback.reset();
+            mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback);
+            test(!mCollisionCallback.boxCollideWithSphere1);
+            test(mCollisionCallback.boxCollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithSphere2);
+
+            mCollisionCallback.reset();
+            mWorld->testCollision(mCylinderShape, &mCollisionCallback);
+            test(!mCollisionCallback.boxCollideWithSphere1);
+            test(mCollisionCallback.boxCollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithSphere2);
+
+            mCollisionCallback.reset();
+            mWorld->testCollision(mBoxShape, mCylinderShape, &mCollisionCallback);
+            test(!mCollisionCallback.boxCollideWithSphere1);
+            test(mCollisionCallback.boxCollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithSphere2);
+
+            // Move sphere 1 to collide with sphere 2
+            mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity()));
+
+            mCollisionCallback.reset();
+            mWorld->testCollision(&mCollisionCallback);
+            test(!mCollisionCallback.boxCollideWithSphere1);
+            test(mCollisionCallback.boxCollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithCylinder);
+            test(mCollisionCallback.sphere1CollideWithSphere2);
+
+            mCollisionCallback.reset();
+            mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback);
+            test(!mCollisionCallback.boxCollideWithSphere1);
+            test(!mCollisionCallback.boxCollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithSphere2);
+
+            mCollisionCallback.reset();
+            mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback);
+            test(!mCollisionCallback.boxCollideWithSphere1);
+            test(mCollisionCallback.boxCollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithSphere2);
+
+            // Test collision with inactive bodies
+            mCollisionCallback.reset();
+            mBoxBody->setIsActive(false);
+            mCylinderBody->setIsActive(false);
+            mSphere1Body->setIsActive(false);
+            mSphere2Body->setIsActive(false);
+            mWorld->testCollision(&mCollisionCallback);
+            test(!mCollisionCallback.boxCollideWithSphere1);
+            test(!mCollisionCallback.boxCollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithCylinder);
+            test(!mCollisionCallback.sphere1CollideWithSphere2);
+
+            test(!mWorld->testAABBOverlap(mBoxBody, mSphere1Body));
+            test(!mWorld->testAABBOverlap(mBoxBody, mCylinderBody));
+            test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody));
+            test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body));
+
+            test(!mWorld->testAABBOverlap(mBoxShape, mSphere1Shape));
+            test(!mWorld->testAABBOverlap(mBoxShape, mCylinderShape));
+            test(!mWorld->testAABBOverlap(mSphere1Shape, mCylinderShape));
+            test(!mWorld->testAABBOverlap(mSphere1Shape, mSphere2Shape));
+
+            mBoxBody->setIsActive(true);
+            mCylinderBody->setIsActive(true);
+            mSphere1Body->setIsActive(true);
+            mSphere2Body->setIsActive(true);
+        }
+ };
+
+}
+
+#endif