Some optimizations

src/collision/shapes/CollisionShape.cpp

@@ -59,7 +59,7 @@ void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const {
     Vector3 maxBounds;
     getLocalBounds(minBounds, maxBounds);
 
-    const Vector3 translation = transform.getPosition();
+    const Vector3& translation = transform.getPosition();
     Matrix3x3 matrix = transform.getOrientation().getMatrix();
     Vector3 resultMin;
     Vector3 resultMax;

src/collision/shapes/ConcaveMeshShape.cpp

@@ -35,6 +35,7 @@ using namespace reactphysics3d;
 
 // Constructor
 ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh, const Vector3& scaling)
+                // TODO : Do not use the default base allocator here
                  : ConcaveShape(CollisionShapeName::TRIANGLE_MESH), mDynamicAABBTree(MemoryManager::getBaseAllocator()),
                    mScaling(scaling) {
     mTriangleMesh = triangleMesh;

src/components/ProxyShapeComponents.h

@@ -195,6 +195,7 @@ class ProxyShapeComponents : public Components {
 
         friend class BroadPhaseSystem;
         friend class CollisionDetectionSystem;
+        friend class DynamicsSystem;
 };
 
 // Return the body entity of a given proxy-shape

src/systems/CollisionDetectionSystem.cpp

@@ -91,7 +91,7 @@ void CollisionDetectionSystem::computeCollisionDetection() {
     computeBroadPhase();
 
     // Compute the middle-phase collision detection
-    computeMiddlePhase(mOverlappingPairs, mNarrowPhaseInput);
+    computeMiddlePhase(mNarrowPhaseInput);
     
     // Compute the narrow-phase collision detection
     computeNarrowPhase();
@@ -204,7 +204,7 @@ void CollisionDetectionSystem::updateOverlappingPairs(const List<Pair<int32, int
 }
 
 // Compute the middle-phase collision detection
-void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairs& overlappingPairs, NarrowPhaseInput& narrowPhaseInput) {
+void CollisionDetectionSystem::computeMiddlePhase(NarrowPhaseInput& narrowPhaseInput) {
 
     RP3D_PROFILE("CollisionDetectionSystem::computeMiddlePhase()", mProfiler);
 
@@ -212,21 +212,21 @@ void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairs& overlappingP
     narrowPhaseInput.reserveMemory();
 
     // Remove the obsolete last frame collision infos and mark all the others as obsolete
-    overlappingPairs.clearObsoleteLastFrameCollisionInfos();
+    mOverlappingPairs.clearObsoleteLastFrameCollisionInfos();
 
     // For each possible convex vs convex pair of bodies
-    for (uint64 i=0; i < overlappingPairs.getNbConvexVsConvexPairs(); i++) {
+    for (uint64 i=0; i < mOverlappingPairs.getNbConvexVsConvexPairs(); i++) {
 
-        assert(mProxyShapesComponents.getBroadPhaseId(overlappingPairs.mProxyShapes1[i]) != -1);
-        assert(mProxyShapesComponents.getBroadPhaseId(overlappingPairs.mProxyShapes2[i]) != -1);
-        assert(mProxyShapesComponents.getBroadPhaseId(overlappingPairs.mProxyShapes1[i]) != mProxyShapesComponents.getBroadPhaseId(overlappingPairs.mProxyShapes2[i]));
+        assert(mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mProxyShapes1[i]) != -1);
+        assert(mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mProxyShapes2[i]) != -1);
+        assert(mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mProxyShapes1[i]) != mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mProxyShapes2[i]));
 
         // Check that at least one body is enabled (active and awake) and not static
         // TODO : Do not test this every frame
         if (mOverlappingPairs.mIsActive[i]) {
 
-            const Entity proxyShape1Entity = overlappingPairs.mProxyShapes1[i];
-            const Entity proxyShape2Entity = overlappingPairs.mProxyShapes2[i];
+            const Entity proxyShape1Entity = mOverlappingPairs.mProxyShapes1[i];
+            const Entity proxyShape2Entity = mOverlappingPairs.mProxyShapes2[i];
 
             const uint proxyShape1Index = mProxyShapesComponents.getEntityIndex(proxyShape1Entity);
             const uint proxyShape2Index = mProxyShapesComponents.getEntityIndex(proxyShape2Entity);
@@ -234,11 +234,11 @@ void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairs& overlappingP
             CollisionShape* collisionShape1 = mProxyShapesComponents.mCollisionShapes[proxyShape1Index];
             CollisionShape* collisionShape2 = mProxyShapesComponents.mCollisionShapes[proxyShape2Index];
 
-            NarrowPhaseAlgorithmType algorithmType = overlappingPairs.mNarrowPhaseAlgorithmType[i];
+            NarrowPhaseAlgorithmType algorithmType = mOverlappingPairs.mNarrowPhaseAlgorithmType[i];
 
             // No middle-phase is necessary, simply create a narrow phase info
             // for the narrow-phase collision detection
-            narrowPhaseInput.addNarrowPhaseTest(overlappingPairs.mPairIds[i], i, proxyShape1Entity, proxyShape2Entity, collisionShape1, collisionShape2,
+            narrowPhaseInput.addNarrowPhaseTest(mOverlappingPairs.mPairIds[i], i, proxyShape1Entity, proxyShape2Entity, collisionShape1, collisionShape2,
                                                       mProxyShapesComponents.mLocalToWorldTransforms[proxyShape1Index],
                                                       mProxyShapesComponents.mLocalToWorldTransforms[proxyShape2Index],
                                                       algorithmType, mMemoryManager.getSingleFrameAllocator());
@@ -246,12 +246,12 @@ void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairs& overlappingP
     }
 
     // For each possible convex vs concave pair of bodies
-    const uint64 convexVsConcaveStartIndex = overlappingPairs.getConvexVsConcavePairsStartIndex();
-    for (uint64 i=convexVsConcaveStartIndex; i < convexVsConcaveStartIndex + overlappingPairs.getNbConvexVsConcavePairs(); i++) {
+    const uint64 convexVsConcaveStartIndex = mOverlappingPairs.getConvexVsConcavePairsStartIndex();
+    for (uint64 i=convexVsConcaveStartIndex; i < convexVsConcaveStartIndex + mOverlappingPairs.getNbConvexVsConcavePairs(); i++) {
 
-        assert(mProxyShapesComponents.getBroadPhaseId(overlappingPairs.mProxyShapes1[i]) != -1);
-        assert(mProxyShapesComponents.getBroadPhaseId(overlappingPairs.mProxyShapes2[i]) != -1);
-        assert(mProxyShapesComponents.getBroadPhaseId(overlappingPairs.mProxyShapes1[i]) != mProxyShapesComponents.getBroadPhaseId(overlappingPairs.mProxyShapes2[i]));
+        assert(mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mProxyShapes1[i]) != -1);
+        assert(mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mProxyShapes2[i]) != -1);
+        assert(mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mProxyShapes1[i]) != mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mProxyShapes2[i]));
 
         // Check that at least one body is enabled (active and awake) and not static
         if (mOverlappingPairs.mIsActive[i]) {
@@ -283,20 +283,23 @@ void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List<uint64>&
         const Entity proxyShape1Entity = mOverlappingPairs.mProxyShapes1[pairIndex];
         const Entity proxyShape2Entity = mOverlappingPairs.mProxyShapes2[pairIndex];
 
+        const uint proxyShape1Index = mProxyShapesComponents.getEntityIndex(proxyShape1Entity);
+        const uint proxyShape2Index = mProxyShapesComponents.getEntityIndex(proxyShape2Entity);
+
         assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1);
         assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1);
         assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity));
 
-        CollisionShape* collisionShape1 = mProxyShapesComponents.getCollisionShape(proxyShape1Entity);
-        CollisionShape* collisionShape2 = mProxyShapesComponents.getCollisionShape(proxyShape2Entity);
+        CollisionShape* collisionShape1 = mProxyShapesComponents.mCollisionShapes[proxyShape1Index];
+        CollisionShape* collisionShape2 = mProxyShapesComponents.mCollisionShapes[proxyShape2Index];
 
         NarrowPhaseAlgorithmType algorithmType = mOverlappingPairs.mNarrowPhaseAlgorithmType[pairIndex];
 
         // No middle-phase is necessary, simply create a narrow phase info
         // for the narrow-phase collision detection
         narrowPhaseInput.addNarrowPhaseTest(pairId, pairIndex, proxyShape1Entity, proxyShape2Entity, collisionShape1, collisionShape2,
-                                                  mProxyShapesComponents.getLocalToWorldTransform(proxyShape1Entity),
-                                                  mProxyShapesComponents.getLocalToWorldTransform(proxyShape2Entity),
+                                                  mProxyShapesComponents.mLocalToWorldTransforms[proxyShape1Index],
+                                                  mProxyShapesComponents.mLocalToWorldTransforms[proxyShape2Index],
                                                   algorithmType, mMemoryManager.getSingleFrameAllocator());
 
     }
@@ -314,6 +317,7 @@ void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List<uint64>&
         computeConvexVsConcaveMiddlePhase(pairIndex, mMemoryManager.getSingleFrameAllocator(), narrowPhaseInput);
     }
 }
+
 // Compute the concave vs convex middle-phase algorithm for a given pair of bodies
 void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(uint64 pairIndex, MemoryAllocator& allocator, NarrowPhaseInput& narrowPhaseInput) {
 
@@ -325,32 +329,29 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(uint64 pairInde
     const uint proxyShape1Index = mProxyShapesComponents.getEntityIndex(proxyShape1);
     const uint proxyShape2Index = mProxyShapesComponents.getEntityIndex(proxyShape2);
 
-    ProxyShape* shape1 = mProxyShapesComponents.mProxyShapes[proxyShape1Index];
-    ProxyShape* shape2 = mProxyShapesComponents.mProxyShapes[proxyShape2Index];
+    Transform& shape1LocalToWorldTransform = mProxyShapesComponents.mLocalToWorldTransforms[proxyShape1Index];
+    Transform& shape2LocalToWorldTransform = mProxyShapesComponents.mLocalToWorldTransforms[proxyShape2Index];
 
-    ConvexShape* convexShape;
-    ConcaveShape* concaveShape;
-
-    const bool isShape1Convex = mOverlappingPairs.mIsShape1Convex[pairIndex];
+    Transform convexToConcaveTransform;
 
     // Collision shape 1 is convex, collision shape 2 is concave
+    ConvexShape* convexShape;
+    ConcaveShape* concaveShape;
+    const bool isShape1Convex = mOverlappingPairs.mIsShape1Convex[pairIndex];
     if (isShape1Convex) {
-        convexShape = static_cast<ConvexShape*>(shape1->getCollisionShape());
-        concaveShape = static_cast<ConcaveShape*>(shape2->getCollisionShape());
+        convexShape = static_cast<ConvexShape*>(mProxyShapesComponents.mCollisionShapes[proxyShape1Index]);
+        concaveShape = static_cast<ConcaveShape*>(mProxyShapesComponents.mCollisionShapes[proxyShape2Index]);
+        convexToConcaveTransform = shape2LocalToWorldTransform.getInverse() * shape1LocalToWorldTransform;
     }
     else {  // Collision shape 2 is convex, collision shape 1 is concave
-        convexShape = static_cast<ConvexShape*>(shape2->getCollisionShape());
-        concaveShape = static_cast<ConcaveShape*>(shape1->getCollisionShape());
+        convexShape = static_cast<ConvexShape*>(mProxyShapesComponents.mCollisionShapes[proxyShape1Index]);
+        concaveShape = static_cast<ConcaveShape*>(mProxyShapesComponents.mCollisionShapes[proxyShape2Index]);
+        convexToConcaveTransform = shape1LocalToWorldTransform.getInverse() * shape2LocalToWorldTransform;
     }
 
     assert(mOverlappingPairs.mNarrowPhaseAlgorithmType[pairIndex] != NarrowPhaseAlgorithmType::None);
 
-    Transform& shape1LocalToWorldTransform = mProxyShapesComponents.mLocalToWorldTransforms[proxyShape1Index];
-    Transform& shape2LocalToWorldTransform = mProxyShapesComponents.mLocalToWorldTransforms[proxyShape2Index];
-
     // Compute the convex shape AABB in the local-space of the convex shape
-    const Transform convexToConcaveTransform = (isShape1Convex ? shape2LocalToWorldTransform : shape1LocalToWorldTransform).getInverse() *
-                                               (isShape1Convex ? shape1LocalToWorldTransform : shape2LocalToWorldTransform);
     AABB aabb;
     convexShape->computeAABB(aabb, convexToConcaveTransform);
 
@@ -1360,7 +1361,7 @@ void CollisionDetectionSystem::testOverlap(OverlapCallback& callback) {
     computeBroadPhase();
 
     // Compute the middle-phase collision detection
-    computeMiddlePhase(mOverlappingPairs, narrowPhaseInput);
+    computeMiddlePhase(narrowPhaseInput);
 
     // Compute the narrow-phase collision detection and report overlapping shapes
     computeNarrowPhaseOverlapSnapshot(narrowPhaseInput, &callback);
@@ -1444,7 +1445,7 @@ void CollisionDetectionSystem::testCollision(CollisionCallback& callback) {
     computeBroadPhase();
 
     // Compute the middle-phase collision detection
-    computeMiddlePhase(mOverlappingPairs, narrowPhaseInput);
+    computeMiddlePhase(narrowPhaseInput);
 
     // Compute the narrow-phase collision detection and report contacts
     computeNarrowPhaseCollisionSnapshot(narrowPhaseInput, callback);

src/systems/CollisionDetectionSystem.h

@@ -176,7 +176,7 @@ class CollisionDetectionSystem {
         void computeBroadPhase();
 
         /// Compute the middle-phase collision detection
-        void computeMiddlePhase(OverlappingPairs& overlappingPairs, NarrowPhaseInput& narrowPhaseInput);
+        void computeMiddlePhase(NarrowPhaseInput& narrowPhaseInput);
 
         // Compute the middle-phase collision detection
         void computeMiddlePhaseCollisionSnapshot(List<uint64>& convexPairs, List<uint64>& concavePairs, NarrowPhaseInput& narrowPhaseInput);

src/systems/ContactSolverSystem.cpp

@@ -124,19 +124,21 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
         assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity1));
         assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity2));
 
-        // Get the position of the two bodies
-        const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(externalManifold.bodyEntity1);
-        const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(externalManifold.bodyEntity2);
+        const uint rigidBodyIndex1 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity1);
+        const uint rigidBodyIndex2 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity2);
 
-        // Initialize the internal contact manifold structure using the external
-        // contact manifold
+        // Get the position of the two bodies
+        const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[rigidBodyIndex1];
+        const Vector3& x2 = mRigidBodyComponents.mCentersOfMassWorld[rigidBodyIndex2];
+
+        // Initialize the internal contact manifold structure using the external contact manifold
         new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
-        mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1->getEntity());
-        mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2->getEntity());
+        mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody1 = rigidBodyIndex1;
+        mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody2 = rigidBodyIndex2;
         mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = RigidBody::getInertiaTensorInverseWorld(mWorld, externalManifold.bodyEntity1);
         mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = RigidBody::getInertiaTensorInverseWorld(mWorld, externalManifold.bodyEntity2);
-        mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.getMassInverse(body1->getEntity());
-        mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.getMassInverse(body2->getEntity());
+        mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex1];
+        mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex2];
         mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints;
         mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
         mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
@@ -146,15 +148,15 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
         mContactConstraints[mNbContactManifolds].frictionPointBody2.setToZero();
 
         // Get the velocities of the bodies
-        const Vector3& v1 = mRigidBodyComponents.getLinearVelocity(externalManifold.bodyEntity1);
-        const Vector3& w1 = mRigidBodyComponents.getAngularVelocity(externalManifold.bodyEntity1);
-        const Vector3& v2 = mRigidBodyComponents.getLinearVelocity(externalManifold.bodyEntity2);
-        const Vector3& w2 = mRigidBodyComponents.getAngularVelocity(externalManifold.bodyEntity2);
+        const Vector3& v1 = mRigidBodyComponents.mLinearVelocities[rigidBodyIndex1];
+        const Vector3& w1 = mRigidBodyComponents.mAngularVelocities[rigidBodyIndex1];
+        const Vector3& v2 = mRigidBodyComponents.mLinearVelocities[rigidBodyIndex2];
+        const Vector3& w2 = mRigidBodyComponents.mAngularVelocities[rigidBodyIndex2];
 
         // For each  contact point of the contact manifold
         assert(externalManifold.nbContactPoints > 0);
         uint contactPointsStartIndex = externalManifold.contactPointsIndex;
-        uint nbContactPoints = externalManifold.nbContactPoints;
+        uint nbContactPoints = static_cast<uint>(externalManifold.nbContactPoints);
         for (uint c=contactPointsStartIndex; c < contactPointsStartIndex + nbContactPoints; c++) {
 
             ContactPoint& externalContact = (*mAllContactPoints)[c];
@@ -350,22 +352,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].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x;
-                mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y;
-                mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z;
+                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.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
-                mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
-                mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
+                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;
 
                 // Update the velocities of the body 2 by applying the impulse P
-                mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x;
-                mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y;
-                mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z;
+                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.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
-                mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
-                mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
+                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;
             }
             else {  // If it is a new contact point
 
@@ -405,12 +407,12 @@ void ContactSolverSystem::warmStart() {
                                         mContactConstraints[c].r2CrossT1.z * mContactConstraints[c].friction1Impulse);
 
             // Update the velocities of the body 1 by applying the impulse P
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
+            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
+            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
             // ------ Second friction constraint at the center of the contact manifold ----- //
 
@@ -426,18 +428,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].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
+            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.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
 
             // Update the velocities of the body 2 by applying the impulse P
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
+            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.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
             // ------ Twist friction constraint at the center of the contact manifold ------ //
 
@@ -451,10 +453,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].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
 
             // Update the velocities of the body 2 by applying the impulse P
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
             // ------ Rolling resistance at the center of the contact manifold ------ //
 
@@ -462,10 +464,10 @@ void ContactSolverSystem::warmStart() {
             angularImpulseBody2 = mContactConstraints[c].rollingResistanceImpulse;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
+            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
         }
         else {  // If it is a new contact manifold
 
@@ -495,10 +497,10 @@ void ContactSolverSystem::solve() {
         decimal sumPenetrationImpulse = 0.0;
 
         // Get the constrained velocities
-        const Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
-        const Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
-        const Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
-        const Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
+        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];
 
         for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
 
@@ -540,22 +542,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].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
+            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.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda;
+            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;
 
             // Update the velocities of the body 2 by applying the impulse P
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
-            mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
+            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.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda;
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda;
+            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;
 
             sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse;
 
@@ -563,10 +565,10 @@ void ContactSolverSystem::solve() {
             if (mIsSplitImpulseActive) {
 
                 // Split impulse (position correction)
-                const Vector3& v1Split = mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
-                const Vector3& w1Split = mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
-                const Vector3& v2Split = mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
-                const Vector3& w2Split = mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
+                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];
 
                 //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 -
@@ -591,22 +593,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].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
-                mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
-                mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
+                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.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit;
-                mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit;
-                mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit;
+                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;
 
                 // Update the velocities of the body 1 by applying the impulse P
-                mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
-                mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
-                mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
+                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.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit;
-                mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit;
-                mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit;
+                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;
             }
 
             contactPointIndex++;
@@ -647,18 +649,18 @@ void ContactSolverSystem::solve() {
                                     mContactConstraints[c].r2CrossT1.z * deltaLambda);
 
         // Update the velocities of the body 1 by applying the impulse P
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
+        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.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
 
         // Update the velocities of the body 2 by applying the impulse P
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
+        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.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
         // ------ Second friction constraint at the center of the contact manifol ----- //
 
@@ -696,16 +698,16 @@ 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].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+        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.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
 
         // Update the velocities of the body 2 by applying the impulse P
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
-        mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+        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.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
         // ------ Twist friction constraint at the center of the contact manifol ------ //
 
@@ -728,10 +730,10 @@ void ContactSolverSystem::solve() {
         angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda;
 
         // Update the velocities of the body 1 by applying the impulse P
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
+        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
 
         // Update the velocities of the body 1 by applying the impulse P
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
 
         // --------- Rolling resistance constraint at the center of the contact manifold --------- //
 
@@ -749,17 +751,16 @@ void ContactSolverSystem::solve() {
             deltaLambdaRolling = mContactConstraints[c].rollingResistanceImpulse - lambdaTempRolling;
 
             // Update the velocities of the body 1 by applying the impulse P
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
+            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
 
             // Update the velocities of the body 2 by applying the impulse P
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
+            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
         }
     }
 }
 
 // Compute the collision restitution factor from the restitution factor of each body
-decimal ContactSolverSystem::computeMixedRestitutionFactor(RigidBody* body1,
-                                                            RigidBody* body2) const {
+decimal ContactSolverSystem::computeMixedRestitutionFactor(RigidBody* body1, RigidBody* body2) const {
     decimal restitution1 = body1->getMaterial().getBounciness();
     decimal restitution2 = body2->getMaterial().getBounciness();
 
@@ -768,16 +769,14 @@ decimal ContactSolverSystem::computeMixedRestitutionFactor(RigidBody* body1,
 }
 
 // Compute the mixed friction coefficient from the friction coefficient of each body
-decimal ContactSolverSystem::computeMixedFrictionCoefficient(RigidBody *body1,
-                                                              RigidBody *body2) const {
+decimal ContactSolverSystem::computeMixedFrictionCoefficient(RigidBody* body1, RigidBody* body2) const {
     // Use the geometric mean to compute the mixed friction coefficient
     return std::sqrt(body1->getMaterial().getFrictionCoefficient() *
                 body2->getMaterial().getFrictionCoefficient());
 }
 
 // Compute th mixed rolling resistance factor between two bodies
-inline decimal ContactSolverSystem::computeMixedRollingResistance(RigidBody* body1,
-                                                            RigidBody* body2) const {
+inline decimal ContactSolverSystem::computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const {
     return decimal(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance());
 }
 
@@ -825,12 +824,12 @@ void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity,
                             deltaVelocity.z - normalVelocity.z);
 
     // If the velocty difference in the tangential plane is not zero
-    decimal lengthTangenVelocity = tangentVelocity.length();
-    if (lengthTangenVelocity > MACHINE_EPSILON) {
+    decimal lengthTangentVelocity = tangentVelocity.length();
+    if (lengthTangentVelocity > MACHINE_EPSILON) {
 
         // Compute the first friction vector in the direction of the tangent
         // velocity difference
-        contact.frictionVector1 = tangentVelocity / lengthTangenVelocity;
+        contact.frictionVector1 = tangentVelocity / lengthTangentVelocity;
     }
     else {
 

src/systems/ContactSolverSystem.h

@@ -176,10 +176,10 @@ class ContactSolverSystem {
             ContactManifold* externalContactManifold;
 
             /// Index of body 1 in the dynamics components arrays
-            uint32 dynamicsComponentIndexBody1;
+            uint32 rigidBodyComponentIndexBody1;
 
             /// Index of body 2 in the dynamics components arrays
-            uint32 dynamicsComponentIndexBody2;
+            uint32 rigidBodyComponentIndexBody2;
 
             /// Inverse of the mass of body 1
             decimal massInverseBody1;

src/systems/DynamicsSystem.cpp

@@ -77,8 +77,8 @@ void DynamicsSystem::updateBodiesState() {
     for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
 
         // Update the linear and angular velocity of the body
-        mRigidBodyComponents.setLinearVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedLinearVelocities[i]);
-        mRigidBodyComponents.setAngularVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedAngularVelocities[i]);
+        mRigidBodyComponents.mLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i];
+        mRigidBodyComponents.mAngularVelocities[i] = mRigidBodyComponents.mConstrainedAngularVelocities[i];
 
         // Update the position of the center of mass of the body
         mRigidBodyComponents.mCentersOfMassWorld[i] = mRigidBodyComponents.mConstrainedPositions[i];
@@ -98,17 +98,11 @@ void DynamicsSystem::updateBodiesState() {
     }
 
     // Update the local-to-world transform of the proxy-shapes
-    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
+    for (uint32 i=0; i < mProxyShapeComponents.getNbEnabledComponents(); i++) {
 
-        // For all the proxy collision shapes of the body
-        const List<Entity>& proxyShapesEntities = mCollisionBodyComponents.getProxyShapes(mRigidBodyComponents.mBodiesEntities[i]);
-        for (uint j=0; j < proxyShapesEntities.size(); j++) {
-
-            // Update the local-to-world transform of the proxy-shape
-            mProxyShapeComponents.setLocalToWorldTransform(proxyShapesEntities[j],
-                                                            mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]) *
-                                                            mProxyShapeComponents.getLocalToBodyTransform(proxyShapesEntities[j]));
-        }
+        // Update the local-to-world transform of the proxy-shape
+        mProxyShapeComponents.mLocalToWorldTransforms[i] = mTransformComponents.getTransform(mProxyShapeComponents.mBodiesEntities[i]) *
+                                                           mProxyShapeComponents.mLocalToBodyTransforms[i];
     }
 }
 
@@ -141,14 +135,17 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) {
     }
 
     // Apply gravity force
-    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
+    if (mIsGravityEnabled) {
 
-        // If the gravity has to be applied to this rigid body
-        if (mRigidBodyComponents.mIsGravityEnabled[i] && mIsGravityEnabled) {
+        for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
 
-            // Integrate the gravity force
-            mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] + timeStep *
-                                                                  mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mInitMasses[i] * mGravity;
+            // If the gravity has to be applied to this rigid body
+            if (mRigidBodyComponents.mIsGravityEnabled[i]) {
+
+                // Integrate the gravity force
+                mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] + timeStep *
+                                                                       mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mInitMasses[i] * mGravity;
+            }
         }
     }