More optimizations

include/reactphysics3d/body/RigidBody.h

@@ -65,8 +65,8 @@ class RigidBody : public CollisionBody {
         /// Compute the local-space inertia tensor and total mass of the body using its colliders
         void computeMassAndInertiaTensorLocal(Vector3& inertiaTensorLocal, decimal& totalMass) const;
 
-        /// Return the inverse of the inertia tensor in world coordinates.
+        /// Compute the inverse of the inertia tensor in world coordinates.
-        static const Matrix3x3 getWorldInertiaTensorInverse(PhysicsWorld& world, Entity bodyEntity);
+        static void computeWorldInertiaTensorInverse(const Matrix3x3& orientation, const Vector3& inverseInertiaTensorLocal, Matrix3x3& outInverseInertiaTensorWorld);
 
     public :
 

include/reactphysics3d/components/RigidBodyComponents.h

@@ -109,9 +109,12 @@ class RigidBodyComponents : public Components {
         /// Array with the inertia tensor of each component
         Vector3* mLocalInertiaTensors;
 
-        /// Array with the inverse of the inertia tensor of each component
+        /// Array with the inverse of the local inertia tensor of each component
         Vector3* mInverseInertiaTensorsLocal;
 
+        /// Array with the inverse of the world inertia tensor of each component
+        Matrix3x3* mInverseInertiaTensorsWorld;
+
         /// Array with the constrained linear velocity of each component
         Vector3* mConstrainedLinearVelocities;
 
@@ -261,6 +264,9 @@ class RigidBodyComponents : public Components {
         /// Return the inverse local inertia tensor of an entity
         const Vector3& getInertiaTensorLocalInverse(Entity bodyEntity);
 
+        /// Return the inverse world inertia tensor of an entity
+        const Matrix3x3& getInertiaTensorWorldInverse(Entity bodyEntity);
+
         /// Set the external force of an entity
         void setExternalForce(Entity bodyEntity, const Vector3& externalForce);
 
@@ -523,6 +529,14 @@ inline const Vector3& RigidBodyComponents::getInertiaTensorLocalInverse(Entity b
    return mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]];
 }
 
+// Return the inverse world inertia tensor of an entity
+inline const Matrix3x3& RigidBodyComponents::getInertiaTensorWorldInverse(Entity bodyEntity) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]];
+}
+
 // Set the external force of an entity
 inline void RigidBodyComponents::setExternalForce(Entity bodyEntity, const Vector3& externalForce) {
 

include/reactphysics3d/engine/PhysicsWorld.h

@@ -301,6 +301,9 @@ class PhysicsWorld {
         /// Add the joint to the list of joints of the two bodies involved in the joint
         void addJointToBodies(Entity body1, Entity body2, Entity joint);
 
+        /// Update the world inverse inertia tensors of rigid bodies
+        void updateBodiesInverseWorldInertiaTensors();
+
         /// Destructor
         ~PhysicsWorld();
 

src/body/RigidBody.cpp

@@ -886,16 +886,14 @@ void RigidBody::updateOverlappingPairs() {
     }
 }
 
-/// Return the inverse of the inertia tensor in world coordinates.
+/// Compute the inverse of the inertia tensor in world coordinates.
-const Matrix3x3 RigidBody::getWorldInertiaTensorInverse(PhysicsWorld& world, Entity bodyEntity) {
+void RigidBody::computeWorldInertiaTensorInverse(const Matrix3x3& orientation, const Vector3& inverseInertiaTensorLocal, Matrix3x3& outInverseInertiaTensorWorld) {
 
-    Matrix3x3 orientation = world.mTransformComponents.getTransform(bodyEntity).getOrientation().getMatrix();
-    const Vector3& inverseInertiaLocalTensor = world.mRigidBodyComponents.getInertiaTensorLocalInverse(bodyEntity);
     Matrix3x3 orientationTranspose = orientation.getTranspose();
-    orientationTranspose[0] *= inverseInertiaLocalTensor.x;
+    orientationTranspose[0] *= inverseInertiaTensorLocal.x;
-    orientationTranspose[1] *= inverseInertiaLocalTensor.y;
+    orientationTranspose[1] *= inverseInertiaTensorLocal.y;
-    orientationTranspose[2] *= inverseInertiaLocalTensor.z;
+    orientationTranspose[2] *= inverseInertiaTensorLocal.z;
-    return orientation * orientationTranspose;
+    outInverseInertiaTensorWorld = orientation * orientationTranspose;
 }
 
 // Set whether or not the body is allowed to go to sleep

src/components/RigidBodyComponents.cpp

@@ -40,7 +40,7 @@ RigidBodyComponents::RigidBodyComponents(MemoryAllocator& allocator)
                                 sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
                                 sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) +
                                 sizeof(decimal) + sizeof(decimal) + sizeof(Vector3) +
-                                sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
+                                sizeof(Vector3) + + sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(Vector3) +
                                 sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
                                 sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) +
                                 sizeof(bool) + sizeof(bool) + sizeof(List<Entity>) + sizeof(List<uint>)) {
@@ -78,7 +78,8 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
     decimal* newInverseMasses = reinterpret_cast<decimal*>(newMasses + nbComponentsToAllocate);
     Vector3* newInertiaTensorLocal = reinterpret_cast<Vector3*>(newInverseMasses + nbComponentsToAllocate);
     Vector3* newInertiaTensorLocalInverses = reinterpret_cast<Vector3*>(newInertiaTensorLocal + nbComponentsToAllocate);
-    Vector3* newConstrainedLinearVelocities = reinterpret_cast<Vector3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
+    Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast<Matrix3x3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
+    Vector3* newConstrainedLinearVelocities = reinterpret_cast<Vector3*>(newInertiaTensorWorldInverses + nbComponentsToAllocate);
     Vector3* newConstrainedAngularVelocities = reinterpret_cast<Vector3*>(newConstrainedLinearVelocities + nbComponentsToAllocate);
     Vector3* newSplitLinearVelocities = reinterpret_cast<Vector3*>(newConstrainedAngularVelocities + nbComponentsToAllocate);
     Vector3* newSplitAngularVelocities = reinterpret_cast<Vector3*>(newSplitLinearVelocities + nbComponentsToAllocate);
@@ -111,6 +112,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
         memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal));
         memcpy(newInertiaTensorLocal, mLocalInertiaTensors, mNbComponents * sizeof(Vector3));
         memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Vector3));
+        memcpy(newInertiaTensorWorldInverses, mInverseInertiaTensorsWorld, mNbComponents * sizeof(Matrix3x3));
         memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3));
         memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3));
         memcpy(newSplitLinearVelocities, mSplitLinearVelocities, mNbComponents * sizeof(Vector3));
@@ -146,6 +148,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
     mInverseMasses = newInverseMasses;
     mLocalInertiaTensors = newInertiaTensorLocal;
     mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses;
+    mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses;
     mConstrainedLinearVelocities = newConstrainedLinearVelocities;
     mConstrainedAngularVelocities = newConstrainedAngularVelocities;
     mSplitLinearVelocities = newSplitLinearVelocities;
@@ -183,6 +186,7 @@ void RigidBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, const
     mInverseMasses[index] = decimal(1.0);
     new (mLocalInertiaTensors + index) Vector3(1.0, 1.0, 1.0);
     new (mInverseInertiaTensorsLocal + index) Vector3(1.0, 1.0, 1.0);
+    new (mInverseInertiaTensorsWorld + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
     new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0);
     new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0);
     new (mSplitLinearVelocities + index) Vector3(0, 0, 0);
@@ -228,6 +232,7 @@ void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex
     mInverseMasses[destIndex] = mInverseMasses[srcIndex];
     new (mLocalInertiaTensors + destIndex) Vector3(mLocalInertiaTensors[srcIndex]);
     new (mInverseInertiaTensorsLocal + destIndex) Vector3(mInverseInertiaTensorsLocal[srcIndex]);
+    new (mInverseInertiaTensorsWorld + destIndex) Matrix3x3(mInverseInertiaTensorsWorld[srcIndex]);
     new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]);
     new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]);
     new (mSplitLinearVelocities + destIndex) Vector3(mSplitLinearVelocities[srcIndex]);
@@ -272,6 +277,7 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) {
     decimal inverseMass1 = mInverseMasses[index1];
     Vector3 inertiaTensorLocal1 = mLocalInertiaTensors[index1];
     Vector3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1];
+    Matrix3x3 inertiaTensorWorldInverse1 = mInverseInertiaTensorsWorld[index1];
     Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]);
     Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]);
     Vector3 splitLinearVelocity1(mSplitLinearVelocities[index1]);
@@ -307,6 +313,7 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) {
     mInverseMasses[index2] = inverseMass1;
     mLocalInertiaTensors[index2] = inertiaTensorLocal1;
     mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1;
+    mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1;
     new (mConstrainedLinearVelocities + index2) Vector3(constrainedLinearVelocity1);
     new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1);
     new (mSplitLinearVelocities + index2) Vector3(splitLinearVelocity1);
@@ -345,6 +352,7 @@ void RigidBodyComponents::destroyComponent(uint32 index) {
     mExternalTorques[index].~Vector3();
     mLocalInertiaTensors[index].~Vector3();
     mInverseInertiaTensorsLocal[index].~Vector3();
+    mInverseInertiaTensorsWorld[index].~Matrix3x3();
     mConstrainedLinearVelocities[index].~Vector3();
     mConstrainedAngularVelocities[index].~Vector3();
     mSplitLinearVelocities[index].~Vector3();

src/engine/PhysicsWorld.cpp

@@ -341,6 +341,9 @@ void PhysicsWorld::update(decimal timeStep) {
     // Report the contacts to the user
     mCollisionDetection.reportContactsAndTriggers();
 
+    // Recompute the inverse inertia tensors of rigid bodies
+    updateBodiesInverseWorldInertiaTensors();
+
     // Disable the joints for pair of sleeping bodies
     disableJointsOfSleepingBodies();
 
@@ -379,6 +382,15 @@ void PhysicsWorld::update(decimal timeStep) {
     mMemoryManager.resetFrameAllocator();
 }
 
+// Update the world inverse inertia tensors of rigid bodies
+void PhysicsWorld::updateBodiesInverseWorldInertiaTensors() {
+
+    for (uint i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
+        const Matrix3x3 orientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation().getMatrix();
+
+        RigidBody::computeWorldInertiaTensorInverse(orientation, mRigidBodyComponents.mInverseInertiaTensorsLocal[i], mRigidBodyComponents.mInverseInertiaTensorsWorld[i]);
+    }
+}
 
 // Solve the contacts and constraints
 void PhysicsWorld::solveContactsAndConstraints(decimal timeStep) {

src/systems/ContactSolverSystem.cpp

@@ -147,8 +147,8 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
         new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
         mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody1 = rigidBodyIndex1;
         mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody2 = rigidBodyIndex2;
-        mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = RigidBody::getWorldInertiaTensorInverse(mWorld, externalManifold.bodyEntity1);
+        mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = mRigidBodyComponents.mInverseInertiaTensorsWorld[rigidBodyIndex1];
-        mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = RigidBody::getWorldInertiaTensorInverse(mWorld, externalManifold.bodyEntity2);
+        mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = mRigidBodyComponents.mInverseInertiaTensorsWorld[rigidBodyIndex2];
         mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex1];
         mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex2];
         mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints;
@@ -668,14 +668,20 @@ void ContactSolverSystem::solve() {
         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;
+        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;
 
         // 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.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+        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;
 
         // ------ Second friction constraint at the center of the contact manifold ----- //
 
@@ -716,13 +722,21 @@ void ContactSolverSystem::solve() {
         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;
+
+        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;
 
         // 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.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+
+        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;
 
         // ------ Twist friction constraint at the center of the contact manifol ------ //
 
@@ -745,10 +759,16 @@ 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].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
+        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;
 
         // Update the velocities of the body 1 by applying the impulse P
-        mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+        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;
 
         // --------- Rolling resistance constraint at the center of the contact manifold --------- //
 
@@ -766,10 +786,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].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
+            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;
 
             // Update the velocities of the body 2 by applying the impulse P
-            mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
+            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;
         }
     }
 }

src/systems/DynamicsSystem.cpp

@@ -130,8 +130,8 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) {
         // Integrate the external force to get the new velocity of the body
         mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + timeStep *
                                                               mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i];
-        mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep *
+        mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep * mRigidBodyComponents.mInverseInertiaTensorsWorld[i] *
-                                                 RigidBody::getWorldInertiaTensorInverse(mWorld, mRigidBodyComponents.mBodiesEntities[i]) * mRigidBodyComponents.mExternalTorques[i];
+                                                                mRigidBodyComponents.mExternalTorques[i];
     }
 
     // Apply gravity force

src/systems/SolveBallAndSocketJointSystem.cpp

@@ -60,8 +60,8 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
         assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
 
         // Get the inertia tensor of bodies
-        mBallAndSocketJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
+        mBallAndSocketJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity);
-        mBallAndSocketJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
+        mBallAndSocketJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity);
     }
 
     // For each joint
@@ -260,9 +260,14 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
         const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
         const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
 
-        // Recompute the inverse inertia tensors
+        // Recompute the world inverse inertia tensors
-        mBallAndSocketJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
+        const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
-        mBallAndSocketJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
+        RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity),
+                                                    mBallAndSocketJointComponents.mI1[i]);
+
+        const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
+        RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity),
+                                                    mBallAndSocketJointComponents.mI2[i]);
     }
 
     // For each joint component

src/systems/SolveFixedJointSystem.cpp

@@ -60,8 +60,8 @@ void SolveFixedJointSystem::initBeforeSolve() {
         assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
 
         // Get the inertia tensor of bodies
-        mFixedJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
+        mFixedJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity);
-        mFixedJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
+        mFixedJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity);
     }
 
     // For each joint
@@ -346,9 +346,14 @@ void SolveFixedJointSystem::solvePositionConstraint() {
         const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
         const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
 
-        // Recompute the inverse inertia tensors
+        // Recompute the world inverse inertia tensors
-        mFixedJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
+        const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
-        mFixedJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
+        RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity),
+                                                    mFixedJointComponents.mI1[i]);
+
+        const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
+        RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity),
+                                                    mFixedJointComponents.mI2[i]);
     }
 
     // For each joint

src/systems/SolveHingeJointSystem.cpp

@@ -60,8 +60,8 @@ void SolveHingeJointSystem::initBeforeSolve() {
         assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
 
         // Get the inertia tensor of bodies
-        mHingeJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
+        mHingeJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity);
-        mHingeJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
+        mHingeJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity);
     }
 
     // For each joint
@@ -539,9 +539,14 @@ void SolveHingeJointSystem::solvePositionConstraint() {
         Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
         Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
 
-        // Recompute the inverse inertia tensors
+        // Recompute the world inverse inertia tensors
-        mHingeJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
+        const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
-        mHingeJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
+        RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity),
+                                                    mHingeJointComponents.mI1[i]);
+
+        const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
+        RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity),
+                                                    mHingeJointComponents.mI2[i]);
     }
 
     // For each joint component

src/systems/SolveSliderJointSystem.cpp

@@ -60,8 +60,8 @@ void SolveSliderJointSystem::initBeforeSolve() {
         assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
 
         // Get the inertia tensor of bodies
-        mSliderJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
+        mSliderJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity);
-        mSliderJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
+        mSliderJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity);
     }
 
     // For each joint
@@ -619,9 +619,14 @@ void SolveSliderJointSystem::solvePositionConstraint() {
         const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
         const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
 
-        // Recompute the inverse inertia tensors
+        // Recompute the world inverse inertia tensors
-        mSliderJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
+        const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
-        mSliderJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
+        RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity),
+                                                    mSliderJointComponents.mI1[i]);
+
+        const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
+        RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity),
+                                                    mSliderJointComponents.mI2[i]);
     }
 
     // For each joint component