Optimizations

include/reactphysics3d/collision/ContactManifold.h

@@ -101,13 +101,19 @@ class ContactManifold {
         /// True if the contact manifold has already been added into an island
         bool isAlreadyInIsland;
 
+        /// Index of the first body of the manifold in the mRigidBodyComponents array (only if body 1 is a RigidBody)
+        uint32 rigidBody1Index;
+
+        /// Index of the second body of the manifold in the mRigidBodyComponents array (only if body 2 is a RigidBody)
+        uint32 rigidBody2Index;
+
     public:
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
         ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity colliderEntity1, Entity colliderEntity2,
-                        uint contactPointsIndex, uint8 nbContactPoints);
+                        uint contactPointsIndex, uint8 nbContactPoints, uint32 rigidBody1Index, uint32 rigidBody2Index);
 
         // -------------------- Friendship -------------------- //
 

include/reactphysics3d/engine/Material.h

@@ -28,6 +28,7 @@
 
 // Libraries
 #include <cassert>
+#include <cmath>
 #include <reactphysics3d/configuration.h>
 
 namespace reactphysics3d {
@@ -44,8 +45,8 @@ class Material {
 
         // -------------------- Attributes -------------------- //
 
-        /// Friction coefficient (positive value)
-        decimal mFrictionCoefficient;
+        /// Square root of the friction coefficient
+        decimal mFrictionCoefficientSqrt;
 
         /// Rolling resistance factor (positive value)
         decimal mRollingResistance;
@@ -59,14 +60,7 @@ class Material {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        Material(decimal frictionCoefficient, decimal rollingResistance, decimal bounciness,
-                 decimal massDensity = decimal(1.0));
-
-        /// Copy-constructor
-        Material(const Material& material);
-
-        /// Destructor
-        ~Material() = default;
+        Material(decimal frictionCoefficient, decimal rollingResistance, decimal bounciness, decimal massDensity = decimal(1.0));
 
     public :
 
@@ -84,6 +78,9 @@ class Material {
         /// Set the friction coefficient.
         void setFrictionCoefficient(decimal frictionCoefficient);
 
+        /// Return the square root friction coefficient
+        decimal getFrictionCoefficientSqrt() const;
+
         /// Return the rolling resistance factor
         decimal getRollingResistance() const;
 
@@ -99,9 +96,6 @@ class Material {
         /// Return a string representation for the material
         std::string to_string() const;
 
-        /// Overloaded assignment operator
-        Material& operator=(const Material& material);
-
         // ---------- Friendship ---------- //
 
         friend class Collider;
@@ -131,7 +125,7 @@ RP3D_FORCE_INLINE void Material::setBounciness(decimal bounciness) {
  * @return Friction coefficient (positive value)
  */
 RP3D_FORCE_INLINE decimal Material::getFrictionCoefficient() const {
-    return mFrictionCoefficient;
+    return mFrictionCoefficientSqrt * mFrictionCoefficientSqrt;
 }
 
 // Set the friction coefficient.
@@ -142,7 +136,12 @@ RP3D_FORCE_INLINE decimal Material::getFrictionCoefficient() const {
  */
 RP3D_FORCE_INLINE void Material::setFrictionCoefficient(decimal frictionCoefficient) {
     assert(frictionCoefficient >= decimal(0.0));
-    mFrictionCoefficient = frictionCoefficient;
+    mFrictionCoefficientSqrt = std::sqrt(frictionCoefficient);
+}
+
+// Return the square root friction coefficient
+RP3D_FORCE_INLINE decimal Material::getFrictionCoefficientSqrt() const {
+    return mFrictionCoefficientSqrt;
 }
 
 // Return the rolling resistance factor. If this value is larger than zero,
@@ -184,27 +183,13 @@ RP3D_FORCE_INLINE std::string Material::to_string() const {
 
     std::stringstream ss;
 
-    ss << "frictionCoefficient=" << mFrictionCoefficient << std::endl;
+    ss << "frictionCoefficient=" << (mFrictionCoefficientSqrt * mFrictionCoefficientSqrt) << std::endl;
     ss << "rollingResistance=" << mRollingResistance << std::endl;
     ss << "bounciness=" << mBounciness << std::endl;
 
     return ss.str();
 }
 
-// Overloaded assignment operator
-RP3D_FORCE_INLINE Material& Material::operator=(const Material& material) {
-
-    // Check for self-assignment
-    if (this != &material) {
-        mFrictionCoefficient = material.mFrictionCoefficient;
-        mBounciness = material.mBounciness;
-        mRollingResistance = material.mRollingResistance;
-    }
-
-    // Return this material
-    return *this;
-}
-
 }
 
 #endif

include/reactphysics3d/mathematics/Matrix3x3.h

@@ -95,6 +95,9 @@ class Matrix3x3 {
         /// Return the inverse matrix
         Matrix3x3 getInverse() const;
 
+        /// Return the inverse matrix
+        Matrix3x3 getInverseWithDeterminant(decimal determinant) const;
+
         /// Return the matrix with absolute values
         Matrix3x3 getAbsoluteMatrix() const;
 

src/collision/ContactManifold.cpp

@@ -32,9 +32,9 @@ using namespace reactphysics3d;
 
 // Constructor
 ContactManifold::ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity colliderEntity1, Entity colliderEntity2,
-                                 uint contactPointsIndex, uint8 nbContactPoints)
+                                 uint contactPointsIndex, uint8 nbContactPoints, uint32 rigidBody1Index, uint32 rigidBody2Index)
                 :contactPointsIndex(contactPointsIndex), bodyEntity1(bodyEntity1), bodyEntity2(bodyEntity2),
                  colliderEntity1(colliderEntity1), colliderEntity2(colliderEntity2), nbContactPoints(nbContactPoints), frictionImpulse1(0), frictionImpulse2(0),
-                 frictionTwistImpulse(0), isAlreadyInIsland(false) {
+                 frictionTwistImpulse(0), isAlreadyInIsland(false), rigidBody1Index(rigidBody1Index), rigidBody2Index(rigidBody2Index) {
 
 }

src/engine/Material.cpp

@@ -30,14 +30,7 @@ using namespace reactphysics3d;
 
 // Constructor
 Material::Material(decimal frictionCoefficient, decimal rollingResistance, decimal bounciness, decimal massDensity)
-         : mFrictionCoefficient(frictionCoefficient), mRollingResistance(rollingResistance), mBounciness(bounciness),
-           mMassDensity(massDensity) {
-
-}
-
-// Copy-constructor
-Material::Material(const Material& material)
-         : mFrictionCoefficient(material.mFrictionCoefficient), mRollingResistance(material.mRollingResistance),
-           mBounciness(material.mBounciness) {
+         : mFrictionCoefficientSqrt(std::sqrt(frictionCoefficient)),
+           mRollingResistance(rollingResistance), mBounciness(bounciness), mMassDensity(massDensity) {
 
 }

src/mathematics/Matrix3x3.cpp

@@ -66,5 +66,26 @@ Matrix3x3 Matrix3x3::getInverse() const {
     return (invDeterminant * tempMatrix);
 }
 
+// Return the inverse matrix
+Matrix3x3 Matrix3x3::getInverseWithDeterminant(decimal determinant) const {
+
+    // Check if the determinant is equal to zero
+    assert(determinant != decimal(0.0));
+
+    decimal invDeterminant = decimal(1.0) / determinant;
+
+    Matrix3x3 tempMatrix((mRows[1][1]*mRows[2][2]-mRows[2][1]*mRows[1][2]),
+                         -(mRows[0][1]*mRows[2][2]-mRows[2][1]*mRows[0][2]),
+                         (mRows[0][1]*mRows[1][2]-mRows[0][2]*mRows[1][1]),
+                            -(mRows[1][0]*mRows[2][2]-mRows[2][0]*mRows[1][2]),
+                         (mRows[0][0]*mRows[2][2]-mRows[2][0]*mRows[0][2]),
+                         -(mRows[0][0]*mRows[1][2]-mRows[1][0]*mRows[0][2]),
+                            (mRows[1][0]*mRows[2][1]-mRows[2][0]*mRows[1][1]),
+                         -(mRows[0][0]*mRows[2][1]-mRows[2][0]*mRows[0][1]),
+                         (mRows[0][0]*mRows[1][1]-mRows[0][1]*mRows[1][0]));
+
+    // Return the inverse matrix
+    return (invDeterminant * tempMatrix);
+}
 
 

src/systems/CollisionDetectionSystem.cpp

@@ -167,7 +167,8 @@ void CollisionDetectionSystem::updateOverlappingPairs(const List<Pair<int32, int
     RP3D_PROFILE("CollisionDetectionSystem::updateOverlappingPairs()", mProfiler);
 
     // For each overlapping pair of nodes
-    for (uint i=0; i < overlappingNodes.size(); i++) {
+    const uint nbOverlappingNodes = overlappingNodes.size();
+    for (uint i=0; i < nbOverlappingNodes; i++) {
 
         Pair<int32, int32> nodePair = overlappingNodes[i];
 
@@ -241,7 +242,8 @@ void CollisionDetectionSystem::computeMiddlePhase(NarrowPhaseInput& narrowPhaseI
     mOverlappingPairs.clearObsoleteLastFrameCollisionInfos();
 
     // For each possible convex vs convex pair of bodies
-    for (uint64 i=0; i < mOverlappingPairs.getNbConvexVsConvexPairs(); i++) {
+    const uint64 nbConvexVsConvexPairs = mOverlappingPairs.getNbConvexVsConvexPairs();
+    for (uint64 i=0; i < nbConvexVsConvexPairs; i++) {
 
         assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders1[i]) != -1);
         assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders2[i]) != -1);
@@ -278,7 +280,8 @@ void CollisionDetectionSystem::computeMiddlePhase(NarrowPhaseInput& narrowPhaseI
 
     // For each possible convex vs concave pair of bodies
     const uint64 convexVsConcaveStartIndex = mOverlappingPairs.getConvexVsConcavePairsStartIndex();
-    for (uint64 i=convexVsConcaveStartIndex; i < convexVsConcaveStartIndex + mOverlappingPairs.getNbConvexVsConcavePairs(); i++) {
+    const uint64 nbConvexVsConcavePairs = mOverlappingPairs.getNbConvexVsConcavePairs();
+    for (uint64 i=convexVsConcaveStartIndex; i < convexVsConcaveStartIndex + nbConvexVsConcavePairs; i++) {
 
         assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders1[i]) != -1);
         assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders2[i]) != -1);
@@ -307,7 +310,8 @@ void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List<uint64>&
     mOverlappingPairs.clearObsoleteLastFrameCollisionInfos();
 
     // For each possible convex vs convex pair of bodies
-    for (uint64 p=0; p < convexPairs.size(); p++) {
+    const uint64 nbConvexPairs = convexPairs.size();
+    for (uint64 p=0; p < nbConvexPairs; p++) {
 
         const uint64 pairId = convexPairs[p];
 
@@ -339,7 +343,8 @@ void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List<uint64>&
     }
 
     // For each possible convex vs concave pair of bodies
-    for (uint p=0; p < concavePairs.size(); p++) {
+    const uint nbConcavePairs = concavePairs.size();
+    for (uint p=0; p < nbConcavePairs; p++) {
 
         const uint64 pairId = concavePairs[p];
         const uint64 pairIndex = mOverlappingPairs.mMapPairIdToPairIndex[pairId];
@@ -407,7 +412,8 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(uint64 pairInde
     const bool reportContacts = !isCollider1Trigger && !isCollider2Trigger;
 
     // For each overlapping triangle
-    for (uint i=0; i < shapeIds.size(); i++)
+    const uint nbShapeIds = shapeIds.size();
+    for (uint i=0; i < nbShapeIds; i++)
     {
         // Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the
         // destructor of the corresponding NarrowPhaseInfo.
@@ -716,7 +722,8 @@ void CollisionDetectionSystem::createContacts() {
     mCurrentContactPoints->reserve(mCurrentContactManifolds->size());
 
     // For each contact pair
-    for (uint p=0; p < (*mCurrentContactPairs).size(); p++) {
+    const uint nbCurrentContactPairs = (*mCurrentContactPairs).size();
+    for (uint p=0; p < nbCurrentContactPairs; p++) {
 
         ContactPair& contactPair = (*mCurrentContactPairs)[p];
 
@@ -725,10 +732,12 @@ void CollisionDetectionSystem::createContacts() {
         contactPair.contactPointsIndex = mCurrentContactPoints->size();
 
         // Add the associated contact pair to both bodies of the pair (used to create islands later)
-        if (mRigidBodyComponents.hasComponent(contactPair.body1Entity)) {
+        uint32 rigidBody1Index = 0;
+        uint32 rigidBody2Index = 0;
+        if (mRigidBodyComponents.hasComponentGetIndex(contactPair.body1Entity, rigidBody1Index)) {
            mRigidBodyComponents.addContacPair(contactPair.body1Entity, p);
         }
-        if (mRigidBodyComponents.hasComponent(contactPair.body2Entity)) {
+        if (mRigidBodyComponents.hasComponentGetIndex(contactPair.body2Entity, rigidBody2Index)) {
            mRigidBodyComponents.addContacPair(contactPair.body2Entity, p);
         }
 
@@ -742,12 +751,9 @@ void CollisionDetectionSystem::createContacts() {
             const int8 nbContactPoints = static_cast<int8>(potentialManifold.nbPotentialContactPoints);
             contactPair.nbToTalContactPoints += nbContactPoints;
 
-            // We create a new contact manifold
-            ContactManifold contactManifold(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity,
-                                            contactPair.collider2Entity, contactPointsIndex, nbContactPoints);
-
-            // Add the contact manifold
-            mCurrentContactManifolds->add(contactManifold);
+            // Create and add the contact manifold
+            mCurrentContactManifolds->emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity,
+                                              contactPair.collider2Entity, contactPointsIndex, nbContactPoints, rigidBody1Index, rigidBody2Index);
 
             assert(potentialManifold.nbPotentialContactPoints > 0);
 
@@ -756,11 +762,8 @@ void CollisionDetectionSystem::createContacts() {
 
                 ContactPointInfo& potentialContactPoint = mPotentialContactPoints[potentialManifold.potentialContactPointsIndices[c]];
 
-                // Create a new contact point
-                ContactPoint contactPoint(potentialContactPoint, mWorld->mConfig.persistentContactDistanceThreshold);
-
-                // Add the contact point
-                mCurrentContactPoints->add(contactPoint);
+                // Create and add the contact point
+                mCurrentContactPoints->emplace(potentialContactPoint, mWorld->mConfig.persistentContactDistanceThreshold);
             }
         }
     }
@@ -812,7 +815,8 @@ void CollisionDetectionSystem::createSnapshotContacts(List<ContactPair>& contact
     contactPoints.reserve(contactManifolds.size());
 
     // For each contact pair
-    for (uint p=0; p < contactPairs.size(); p++) {
+    const uint nbContactPairs = contactPairs.size();
+    for (uint p=0; p < nbContactPairs; p++) {
 
         ContactPair& contactPair = contactPairs[p];
         assert(contactPair.nbPotentialContactManifolds > 0);
@@ -831,12 +835,9 @@ void CollisionDetectionSystem::createSnapshotContacts(List<ContactPair>& contact
             const uint8 nbContactPoints = potentialManifold.nbPotentialContactPoints;
             contactPair.nbToTalContactPoints += nbContactPoints;
 
-            // We create a new contact manifold
-            ContactManifold contactManifold(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity,
-                                            contactPair.collider2Entity, contactPointsIndex, nbContactPoints);
-
-            // Add the contact manifold
-            contactManifolds.add(contactManifold);
+            // Create and add the contact manifold
+            contactManifolds.emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity,
+                                     contactPair.collider2Entity, contactPointsIndex, nbContactPoints, 0, 0);
 
             assert(potentialManifold.nbPotentialContactPoints > 0);
 
@@ -992,10 +993,12 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
 
     RP3D_PROFILE("CollisionDetectionSystem::processPotentialContacts()", mProfiler);
 
+    const uint nbObjects = narrowPhaseInfoBatch.getNbObjects();
+
     if (updateLastFrameInfo) {
 
         // For each narrow phase info object
-        for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) {
+        for(uint i=0; i < nbObjects; i++) {
 
             narrowPhaseInfoBatch.narrowPhaseInfos[i].lastFrameCollisionInfo->wasColliding = narrowPhaseInfoBatch.narrowPhaseInfos[i].isColliding;
 
@@ -1005,7 +1008,7 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
     }
 
     // For each narrow phase info object
-    for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) {
+    for(uint i=0; i < nbObjects; i++) {
 
         const uint64 pairId = narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId;
         const uint64 pairIndex = mOverlappingPairs.mMapPairIdToPairIndex[pairId];
@@ -1175,7 +1178,8 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List<ContactPair>
     RP3D_PROFILE("CollisionDetectionSystem::reducePotentialContactManifolds()", mProfiler);
 
     // Reduce the number of potential contact manifolds in a contact pair
-    for (uint i=0; i < contactPairs->size(); i++) {
+    const uint nbContactPairs = contactPairs->size();
+    for (uint i=0; i < nbContactPairs; i++) {
 
         ContactPair& contactPair = (*contactPairs)[i];
 
@@ -1205,7 +1209,7 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List<ContactPair>
     }
 
     // Reduce the number of potential contact points in the manifolds
-    for (uint i=0; i < contactPairs->size(); i++) {
+    for (uint i=0; i < nbContactPairs; i++) {
 
         const ContactPair& pairContact = (*contactPairs)[i];
 

src/systems/ContactSolverSystem.cpp

@@ -90,7 +90,8 @@ void ContactSolverSystem::init(List<ContactManifold>* contactManifolds, List<Con
     assert(mContactConstraints != nullptr);
 
     // For each island of the world
-    for (uint i = 0; i < mIslands.getNbIslands(); i++) {
+    const uint nbIslands = mIslands.getNbIslands();
+    for (uint i = 0; i < nbIslands; i++) {
 
         if (mIslands.nbContactManifolds[i] > 0) {
             initializeForIsland(i);
@@ -117,25 +118,24 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
     assert(mIslands.nbContactManifolds[islandIndex] > 0);
 
     // For each contact manifold of the island
-    uint contactManifoldsIndex = mIslands.contactManifoldsIndices[islandIndex];
-    uint nbContactManifolds = mIslands.nbContactManifolds[islandIndex];
+    const uint contactManifoldsIndex = mIslands.contactManifoldsIndices[islandIndex];
+    const uint nbContactManifolds = mIslands.nbContactManifolds[islandIndex];
     for (uint m=contactManifoldsIndex; m < contactManifoldsIndex + nbContactManifolds; m++) {
 
         ContactManifold& externalManifold = (*mAllContactManifolds)[m];
 
         assert(externalManifold.nbContactPoints > 0);
 
-        const uint rigidBodyIndex1 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity1);
-        const uint rigidBodyIndex2 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity2);
+        const uint rigidBodyIndex1 = externalManifold.rigidBody1Index;
+        const uint rigidBodyIndex2 = externalManifold.rigidBody2Index;
 
         // Get the two bodies of the contact
-        RigidBody* body1 = mRigidBodyComponents.mRigidBodies[rigidBodyIndex1];
-        RigidBody* body2 = mRigidBodyComponents.mRigidBodies[rigidBodyIndex2];
         assert(body1 != nullptr);
         assert(body2 != nullptr);
         assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity1));
         assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity2));
 
+        // TODO OPTI : Do not use pointers to colliders here, maybe we call totally remove those pointers
         Collider* collider1 = mColliderComponents.getCollider(externalManifold.colliderEntity1);
         Collider* collider2 = mColliderComponents.getCollider(externalManifold.colliderEntity2);
 
@@ -152,7 +152,9 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
         mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex1];
         mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex2];
         mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints;
+        // TODO OPTI : Do not compute this using colliders pointers
         mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(collider1, collider2);
+        // TODO OPTI : Do not compute this using colliders pointers
         mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(collider1, collider2);
         mContactConstraints[mNbContactManifolds].externalContactManifold = &externalManifold;
         mContactConstraints[mNbContactManifolds].normal.setToZero();
@@ -165,17 +167,21 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
         const Vector3& v2 = mRigidBodyComponents.mLinearVelocities[rigidBodyIndex2];
         const Vector3& w2 = mRigidBodyComponents.mAngularVelocities[rigidBodyIndex2];
 
+        // TODO OPTI : Maybe use collider index here
+        const Transform& collider1LocalToWorldTransform = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity1);
+        const Transform& collider2LocalToWorldTransform = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity2);
+
         // For each  contact point of the contact manifold
         assert(externalManifold.nbContactPoints > 0);
-        uint contactPointsStartIndex = externalManifold.contactPointsIndex;
-        uint nbContactPoints = static_cast<uint>(externalManifold.nbContactPoints);
+        const uint contactPointsStartIndex = externalManifold.contactPointsIndex;
+        const uint nbContactPoints = static_cast<uint>(externalManifold.nbContactPoints);
         for (uint c=contactPointsStartIndex; c < contactPointsStartIndex + nbContactPoints; c++) {
 
             ContactPoint& externalContact = (*mAllContactPoints)[c];
 
             // Get the contact point on the two bodies
-            Vector3 p1 = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity1) * externalContact.getLocalPointOnShape1();
-            Vector3 p2 = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity2) * externalContact.getLocalPointOnShape2();
+            const Vector3 p1 = collider1LocalToWorldTransform * externalContact.getLocalPointOnShape1();
+            const Vector3 p2 = collider2LocalToWorldTransform * externalContact.getLocalPointOnShape2();
 
             new (mContactPoints + mNbContactPoints) ContactPointSolver();
             mContactPoints[mNbContactPoints].externalContact = &externalContact;
@@ -270,20 +276,20 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
         mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold.frictionTwistImpulse;
 
         // Compute the inverse K matrix for the rolling resistance constraint
-        bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
-        bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
+        const bool isBody1DynamicType = mRigidBodyComponents.mBodyTypes[rigidBodyIndex1] == BodyType::DYNAMIC;
+        const bool isBody2DynamicType = mRigidBodyComponents.mBodyTypes[rigidBodyIndex2] == BodyType::DYNAMIC;
         mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero();
         if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) {
 
             mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2;
-            decimal det = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getDeterminant();
+            const decimal det = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getDeterminant();
 
             // If the matrix is not inversible
             if (approxEqual(det, decimal(0.0))) {
                mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero();
             }
             else {
-               mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse();
+               mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverseWithDeterminant(det);
             }
         }
 
@@ -352,10 +358,12 @@ void ContactSolverSystem::warmStart() {
 
         for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
 
-
             // If it is not a new contact (this contact was already existing at last time step)
             if (mContactPoints[contactPointIndex].isRestingContact) {
 
+                const uint32 rigidBody1Index = mContactConstraints[c].rigidBodyComponentIndexBody1;
+                const uint32 rigidBody2Index = mContactConstraints[c].rigidBodyComponentIndexBody2;
+
                 atLeastOneRestingContactPoint = true;
 
                 // --------- Penetration --------- //
@@ -364,22 +372,22 @@ void ContactSolverSystem::warmStart() {
                 Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse,
                                            mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse,
                                            mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse);
-                mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x;
-                mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y;
-                mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z;
+                mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x;
+                mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y;
+                mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z;
 
-                mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
-                mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
-                mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
+                mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
+                mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
+                mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
 
                 // Update the velocities of the body 2 by applying the impulse P
-                mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x;
-                mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y;
-                mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z;
+                mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x;
+                mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y;
+                mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z;
 
-                mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
-                mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
-                mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
+                mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
+                mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
+                mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
             }
             else {  // If it is a new contact point
 
@@ -418,13 +426,16 @@ void ContactSolverSystem::warmStart() {
                                         mContactConstraints[c].r2CrossT1.y * mContactConstraints[c].friction1Impulse,
                                         mContactConstraints[c].r2CrossT1.z * mContactConstraints[c].friction1Impulse);
 
-            // Update the velocities of the body 1 by applying the impulse P
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+            const uint32 rigidBody1Index = mContactConstraints[c].rigidBodyComponentIndexBody1;
+            const uint32 rigidBody2Index = mContactConstraints[c].rigidBodyComponentIndexBody2;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+            mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+
+            // Update the velocities of the body 1 by applying the impulse P
+            mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
             // ------ Second friction constraint at the center of the contact manifold ----- //
 
@@ -440,18 +451,18 @@ void ContactSolverSystem::warmStart() {
             angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * mContactConstraints[c].friction2Impulse;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
+            mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
+            mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
+            mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
 
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
 
             // Update the velocities of the body 2 by applying the impulse P
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
+            mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
+            mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
+            mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
 
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
             // ------ Twist friction constraint at the center of the contact manifold ------ //
 
@@ -465,10 +476,10 @@ void ContactSolverSystem::warmStart() {
             angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
 
             // Update the velocities of the body 2 by applying the impulse P
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
             // ------ Rolling resistance at the center of the contact manifold ------ //
 
@@ -476,10 +487,10 @@ void ContactSolverSystem::warmStart() {
             angularImpulseBody2 = mContactConstraints[c].rollingResistanceImpulse;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
         }
         else {  // If it is a new contact manifold
 
@@ -508,11 +519,14 @@ void ContactSolverSystem::solve() {
 
         decimal sumPenetrationImpulse = 0.0;
 
+        const uint32 rigidBody1Index = mContactConstraints[c].rigidBodyComponentIndexBody1;
+        const uint32 rigidBody2Index = mContactConstraints[c].rigidBodyComponentIndexBody2;
+
         // Get the constrained velocities
-        const Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1];
-        const Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1];
-        const Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2];
-        const Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2];
+        const Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index];
+        const Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index];
+        const Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index];
+        const Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index];
 
         for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
 
@@ -554,22 +568,22 @@ void ContactSolverSystem::solve() {
                                   mContactPoints[contactPointIndex].normal.z * deltaLambda);
 
             // Update the velocities of the body 1 by applying the impulse P
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
+            mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
+            mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
+            mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
 
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda;
 
             // Update the velocities of the body 2 by applying the impulse P
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
+            mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
+            mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
+            mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
 
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda;
 
             sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse;
 
@@ -577,10 +591,10 @@ void ContactSolverSystem::solve() {
             if (mIsSplitImpulseActive) {
 
                 // Split impulse (position correction)
-                const Vector3& v1Split = mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1];
-                const Vector3& w1Split = mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1];
-                const Vector3& v2Split = mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2];
-                const Vector3& w2Split = mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2];
+                const Vector3& v1Split = mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index];
+                const Vector3& w1Split = mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index];
+                const Vector3& v2Split = mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index];
+                const Vector3& w2Split = mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index];
 
                 //Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1);
                 Vector3 deltaVSplit(v2Split.x + w2Split.y * mContactPoints[contactPointIndex].r2.z - w2Split.z * mContactPoints[contactPointIndex].r2.y - v1Split.x -
@@ -605,22 +619,22 @@ void ContactSolverSystem::solve() {
                                       mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit);
 
                 // Update the velocities of the body 1 by applying the impulse P
-                mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
-                mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
-                mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
+                mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
+                mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
+                mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
 
-                mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit;
-                mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit;
-                mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit;
+                mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit;
+                mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit;
+                mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit;
 
                 // Update the velocities of the body 1 by applying the impulse P
-                mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
-                mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
-                mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
+                mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
+                mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
+                mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
 
-                mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit;
-                mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit;
-                mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit;
+                mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit;
+                mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit;
+                mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit;
             }
 
             contactPointIndex++;
@@ -664,24 +678,24 @@ void ContactSolverSystem::solve() {
 
 
         // Update the velocities of the body 1 by applying the impulse P
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
+        mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
+        mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
+        mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
 
         Vector3 angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x += angularVelocity1.x;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y += angularVelocity1.y;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z += angularVelocity1.z;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x += angularVelocity1.x;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y += angularVelocity1.y;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z += angularVelocity1.z;
 
         // Update the velocities of the body 2 by applying the impulse P
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
+        mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
+        mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
+        mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
 
         Vector3 angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += angularVelocity2.x;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += angularVelocity2.y;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += angularVelocity2.z;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z;
 
         // ------ Second friction constraint at the center of the contact manifold ----- //
 
@@ -719,24 +733,24 @@ void ContactSolverSystem::solve() {
         angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda;
 
         // Update the velocities of the body 1 by applying the impulse P
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
+        mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
+        mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
+        mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
 
         angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x += angularVelocity1.x;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y += angularVelocity1.y;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z += angularVelocity1.z;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x += angularVelocity1.x;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y += angularVelocity1.y;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z += angularVelocity1.z;
 
         // Update the velocities of the body 2 by applying the impulse P
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
+        mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
+        mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
+        mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
 
         angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += angularVelocity2.x;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += angularVelocity2.y;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += angularVelocity2.z;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z;
 
         // ------ Twist friction constraint at the center of the contact manifol ------ //
 
@@ -760,15 +774,15 @@ void ContactSolverSystem::solve() {
 
         // Update the velocities of the body 1 by applying the impulse P
         angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= angularVelocity1.x;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= angularVelocity1.y;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= angularVelocity1.z;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= angularVelocity1.x;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= angularVelocity1.y;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= angularVelocity1.z;
 
         // Update the velocities of the body 1 by applying the impulse P
         angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += angularVelocity2.x;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += angularVelocity2.y;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += angularVelocity2.z;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y;
+        mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z;
 
         // --------- Rolling resistance constraint at the center of the contact manifold --------- //
 
@@ -787,15 +801,15 @@ void ContactSolverSystem::solve() {
 
             // Update the velocities of the body 1 by applying the impulse P
             angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= angularVelocity1.x;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= angularVelocity1.y;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= angularVelocity1.z;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= angularVelocity1.x;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= angularVelocity1.y;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= angularVelocity1.z;
 
             // Update the velocities of the body 2 by applying the impulse P
             angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += angularVelocity2.x;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += angularVelocity2.y;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += angularVelocity2.z;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y;
+            mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z;
         }
     }
 }
@@ -813,12 +827,11 @@ decimal ContactSolverSystem::computeMixedRestitutionFactor(Collider* collider1,
 decimal ContactSolverSystem::computeMixedFrictionCoefficient(Collider* collider1, Collider* collider2) const {
 
     // Use the geometric mean to compute the mixed friction coefficient
-    return std::sqrt(collider1->getMaterial().getFrictionCoefficient() *
-                collider2->getMaterial().getFrictionCoefficient());
+    return collider1->getMaterial().getFrictionCoefficientSqrt() * collider2->getMaterial().getFrictionCoefficientSqrt();
 }
 
 // Compute th mixed rolling resistance factor between two colliders
-RP3D_FORCE_INLINE decimal ContactSolverSystem::computeMixedRollingResistance(Collider* collider1, Collider* collider2) const {
+decimal ContactSolverSystem::computeMixedRollingResistance(Collider* collider1, Collider* collider2) const {
     return decimal(0.5f) * (collider1->getMaterial().getRollingResistance() + collider2->getMaterial().getRollingResistance());
 }
 
@@ -851,8 +864,7 @@ void ContactSolverSystem::storeImpulses() {
 
 // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
 // for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal.
-void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity,
-                                           ContactManifoldSolver& contact) const {
+void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity, ContactManifoldSolver& contact) const {
 
     RP3D_PROFILE("ContactSolver::computeFrictionVectors()", mProfiler);