From a871bfdd6ad178de2d566b5e642d9a6f5ba127c6 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 20 Jul 2020 00:33:50 +0200 Subject: [PATCH] More optimizations --- include/reactphysics3d/body/RigidBody.h | 4 +- .../components/RigidBodyComponents.h | 16 ++++++- include/reactphysics3d/engine/PhysicsWorld.h | 3 ++ src/body/RigidBody.cpp | 14 +++--- src/components/RigidBodyComponents.cpp | 12 ++++- src/engine/PhysicsWorld.cpp | 12 +++++ src/systems/ContactSolverSystem.cpp | 46 +++++++++++++++---- src/systems/DynamicsSystem.cpp | 4 +- src/systems/SolveBallAndSocketJointSystem.cpp | 15 ++++-- src/systems/SolveFixedJointSystem.cpp | 15 ++++-- src/systems/SolveHingeJointSystem.cpp | 15 ++++-- src/systems/SolveSliderJointSystem.cpp | 15 ++++-- 12 files changed, 126 insertions(+), 45 deletions(-) diff --git a/include/reactphysics3d/body/RigidBody.h b/include/reactphysics3d/body/RigidBody.h index bc2353e0..b2481e2b 100644 --- a/include/reactphysics3d/body/RigidBody.h +++ b/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. - static const Matrix3x3 getWorldInertiaTensorInverse(PhysicsWorld& world, Entity bodyEntity); + /// Compute the inverse of the inertia tensor in world coordinates. + static void computeWorldInertiaTensorInverse(const Matrix3x3& orientation, const Vector3& inverseInertiaTensorLocal, Matrix3x3& outInverseInertiaTensorWorld); public : diff --git a/include/reactphysics3d/components/RigidBodyComponents.h b/include/reactphysics3d/components/RigidBodyComponents.h index 8097aaf3..a78f071b 100644 --- a/include/reactphysics3d/components/RigidBodyComponents.h +++ b/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) { diff --git a/include/reactphysics3d/engine/PhysicsWorld.h b/include/reactphysics3d/engine/PhysicsWorld.h index c4c841ef..58fe99e7 100644 --- a/include/reactphysics3d/engine/PhysicsWorld.h +++ b/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(); diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index be893ff4..76db1b04 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -886,16 +886,14 @@ void RigidBody::updateOverlappingPairs() { } } -/// Return the inverse of the inertia tensor in world coordinates. -const Matrix3x3 RigidBody::getWorldInertiaTensorInverse(PhysicsWorld& world, Entity bodyEntity) { +/// Compute the inverse of the inertia tensor in world coordinates. +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[1] *= inverseInertiaLocalTensor.y; - orientationTranspose[2] *= inverseInertiaLocalTensor.z; - return orientation * orientationTranspose; + orientationTranspose[0] *= inverseInertiaTensorLocal.x; + orientationTranspose[1] *= inverseInertiaTensorLocal.y; + orientationTranspose[2] *= inverseInertiaTensorLocal.z; + outInverseInertiaTensorWorld = orientation * orientationTranspose; } // Set whether or not the body is allowed to go to sleep diff --git a/src/components/RigidBodyComponents.cpp b/src/components/RigidBodyComponents.cpp index 4653daab..6d2a389a 100644 --- a/src/components/RigidBodyComponents.cpp +++ b/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) + sizeof(List)) { @@ -78,7 +78,8 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { decimal* newInverseMasses = reinterpret_cast(newMasses + nbComponentsToAllocate); Vector3* newInertiaTensorLocal = reinterpret_cast(newInverseMasses + nbComponentsToAllocate); Vector3* newInertiaTensorLocalInverses = reinterpret_cast(newInertiaTensorLocal + nbComponentsToAllocate); - Vector3* newConstrainedLinearVelocities = reinterpret_cast(newInertiaTensorLocalInverses + nbComponentsToAllocate); + Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast(newInertiaTensorLocalInverses + nbComponentsToAllocate); + Vector3* newConstrainedLinearVelocities = reinterpret_cast(newInertiaTensorWorldInverses + nbComponentsToAllocate); Vector3* newConstrainedAngularVelocities = reinterpret_cast(newConstrainedLinearVelocities + nbComponentsToAllocate); Vector3* newSplitLinearVelocities = reinterpret_cast(newConstrainedAngularVelocities + nbComponentsToAllocate); Vector3* newSplitAngularVelocities = reinterpret_cast(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(); diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp index 0ef91ca8..04fd5598 100644 --- a/src/engine/PhysicsWorld.cpp +++ b/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) { diff --git a/src/systems/ContactSolverSystem.cpp b/src/systems/ContactSolverSystem.cpp index 76a79c74..b5854a54 100644 --- a/src/systems/ContactSolverSystem.cpp +++ b/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].inverseInertiaTensorBody2 = RigidBody::getWorldInertiaTensorInverse(mWorld, externalManifold.bodyEntity2); + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = mRigidBodyComponents.mInverseInertiaTensorsWorld[rigidBodyIndex1]; + 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; } } } diff --git a/src/systems/DynamicsSystem.cpp b/src/systems/DynamicsSystem.cpp index fae61e82..b30506d6 100644 --- a/src/systems/DynamicsSystem.cpp +++ b/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 * - RigidBody::getWorldInertiaTensorInverse(mWorld, mRigidBodyComponents.mBodiesEntities[i]) * mRigidBodyComponents.mExternalTorques[i]; + mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep * mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * + mRigidBodyComponents.mExternalTorques[i]; } // Apply gravity force diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp index f481d319..078a1dd7 100644 --- a/src/systems/SolveBallAndSocketJointSystem.cpp +++ b/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.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); + mBallAndSocketJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity); + 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 - mBallAndSocketJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity); - mBallAndSocketJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); + // Recompute the world inverse inertia tensors + const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix(); + 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 diff --git a/src/systems/SolveFixedJointSystem.cpp b/src/systems/SolveFixedJointSystem.cpp index 7143bb43..59a07089 100644 --- a/src/systems/SolveFixedJointSystem.cpp +++ b/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.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); + mFixedJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity); + 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 - mFixedJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity); - mFixedJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); + // Recompute the world inverse inertia tensors + const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix(); + 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 diff --git a/src/systems/SolveHingeJointSystem.cpp b/src/systems/SolveHingeJointSystem.cpp index a1582750..a4f9946e 100644 --- a/src/systems/SolveHingeJointSystem.cpp +++ b/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.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); + mHingeJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity); + 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 - mHingeJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity); - mHingeJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); + // Recompute the world inverse inertia tensors + const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix(); + 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 diff --git a/src/systems/SolveSliderJointSystem.cpp b/src/systems/SolveSliderJointSystem.cpp index d68645f7..ac44ef7d 100644 --- a/src/systems/SolveSliderJointSystem.cpp +++ b/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.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); + mSliderJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity); + 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 - mSliderJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity); - mSliderJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity); + // Recompute the world inverse inertia tensors + const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix(); + 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