diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp
index e8108bb9..e0107f28 100644
--- a/src/body/CollisionBody.cpp
+++ b/src/body/CollisionBody.cpp
@@ -58,7 +58,7 @@ CollisionBody::~CollisionBody() {
 /// 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
 /// 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 local-space of the body.
 /// This method will return a pointer to the proxy collision shape that links the body with
 /// the collision shape you have added.
 ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisionShape,
@@ -102,11 +102,16 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
     // If the the first proxy shape is the one to remove
     if (current == proxyShape) {
         mProxyCollisionShapes = current->mNext;
-        mWorld.mCollisionDetection.removeProxyCollisionShape(current);
+
+        if (mIsActive) {
+            mWorld.mCollisionDetection.removeProxyCollisionShape(current);
+        }
+
         mWorld.removeCollisionShape(proxyShape->mCollisionShape);
         current->ProxyShape::~ProxyShape();
         mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
         mNbCollisionShapes--;
+        assert(mNbCollisionShapes >= 0);
         return;
     }
 
@@ -119,7 +124,11 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
             // Remove the proxy collision shape
             ProxyShape* elementToRemove = current->mNext;
             current->mNext = elementToRemove->mNext;
-            mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
+
+            if (mIsActive) {
+                mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
+            }
+
             mWorld.removeCollisionShape(proxyShape->mCollisionShape);
             elementToRemove->ProxyShape::~ProxyShape();
             mWorld.mMemoryAllocator.release(elementToRemove, sizeof(ProxyShape));
@@ -137,6 +146,8 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
 // Remove all the collision shapes
 void CollisionBody::removeAllCollisionShapes() {
 
+    // TODO : Remove all the contact manifolds at the end of this call
+
     ProxyShape* current = mProxyCollisionShapes;
 
     // Look for the proxy shape that contains the collision shape in parameter
@@ -144,7 +155,11 @@ void CollisionBody::removeAllCollisionShapes() {
 
         // Remove the proxy collision shape
         ProxyShape* nextElement = current->mNext;
-        mWorld.mCollisionDetection.removeProxyCollisionShape(current);
+
+        if (mIsActive) {
+            mWorld.mCollisionDetection.removeProxyCollisionShape(current);
+        }
+
         mWorld.removeCollisionShape(current->mCollisionShape);
         current->ProxyShape::~ProxyShape();
         mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
@@ -196,8 +211,6 @@ void CollisionBody::setIsActive(bool isActive) {
 
     Body::setIsActive(isActive);
 
-    // TODO : Implement this
-
     // If we have to activate the body
     if (isActive) {
 
diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h
index 98e4858b..7678ed59 100644
--- a/src/body/CollisionBody.h
+++ b/src/body/CollisionBody.h
@@ -144,8 +144,8 @@ class CollisionBody : public Body {
         void setTransform(const Transform& transform);
 
         /// Add a collision shape to the body.
-        ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
-                                            const Transform& transform = Transform::identity());
+        virtual ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
+                                      const Transform& transform);
 
         /// Remove a collision shape from the body
         virtual void removeCollisionShape(const ProxyShape* proxyShape);
diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp
index 21ae595c..9dcd2c13 100644
--- a/src/body/RigidBody.cpp
+++ b/src/body/RigidBody.cpp
@@ -176,7 +176,8 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
 /// parameter is the transformation that transform the local-space of the collision shape into
 /// the local-space of the body. By default, the second parameter is the identity transform.
 ProxyShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape,
-                                               decimal mass, const Transform& transform) {
+                                         const Transform& transform,
+                                         decimal mass) {
 
     assert(mass > decimal(0.0));
 
diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h
index 6375f8f6..a8242c3b 100644
--- a/src/body/RigidBody.h
+++ b/src/body/RigidBody.h
@@ -204,9 +204,9 @@ class RigidBody : public CollisionBody {
         void applyTorque(const Vector3& torque);
 
         /// Add a collision shape to the body.
-        ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
-                                            decimal mass,
-                                            const Transform& transform = Transform::identity());
+        virtual ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
+                                              const Transform& transform,
+                                              decimal mass);
 
         /// Remove a collision shape from the body
         virtual void removeCollisionShape(const ProxyShape* proxyCollisionShape);
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index b2f10fa9..261c27aa 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -142,14 +142,6 @@ void CollisionDetection::computeNarrowPhase() {
         if (narrowPhaseAlgorithm.testCollision(shape1, shape2, contactInfo)) {
             assert(contactInfo != NULL);
 
-            /*
-            // Set the bodies of the contact
-            contactInfo->body1 = dynamic_cast<RigidBody*>(body1);
-            contactInfo->body2 = dynamic_cast<RigidBody*>(body2);
-            assert(contactInfo->body1 != NULL);
-            assert(contactInfo->body2 != NULL);
-            */
-
             // Create a new contact
             createContact(pair, contactInfo);
 
@@ -256,7 +248,8 @@ void CollisionDetection::addContactManifoldToBody(ContactManifold* contactManifo
                                                                      body1->mContactManifoldsList);
     body1->mContactManifoldsList = listElement1;
 
-    // Add the joint at the beginning of the linked list of joints of the second body
+    // Add the contact manifold at the beginning of the linked
+    // list of the contact manifolds of the second body
     void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
     ContactManifoldListElement* listElement2 = new (allocatedMemory2)
                                                   ContactManifoldListElement(contactManifold,