diff --git a/include/reactphysics3d/body/RigidBody.h b/include/reactphysics3d/body/RigidBody.h index bbc583b0..3e262dcb 100644 --- a/include/reactphysics3d/body/RigidBody.h +++ b/include/reactphysics3d/body/RigidBody.h @@ -212,6 +212,24 @@ class RigidBody : public CollisionBody { friend class Joint; }; +/// Compute the inverse of the inertia tensor in world coordinates. +RP3D_FORCE_INLINE void RigidBody::computeWorldInertiaTensorInverse(const Matrix3x3& orientation, const Vector3& inverseInertiaTensorLocal, Matrix3x3& outInverseInertiaTensorWorld) { + + outInverseInertiaTensorWorld[0][0] = orientation[0][0] * inverseInertiaTensorLocal.x; + outInverseInertiaTensorWorld[0][1] = orientation[1][0] * inverseInertiaTensorLocal.x; + outInverseInertiaTensorWorld[0][2] = orientation[2][0] * inverseInertiaTensorLocal.x; + + outInverseInertiaTensorWorld[1][0] = orientation[0][1] * inverseInertiaTensorLocal.y; + outInverseInertiaTensorWorld[1][1] = orientation[1][1] * inverseInertiaTensorLocal.y; + outInverseInertiaTensorWorld[1][2] = orientation[2][1] * inverseInertiaTensorLocal.y; + + outInverseInertiaTensorWorld[2][0] = orientation[0][2] * inverseInertiaTensorLocal.z; + outInverseInertiaTensorWorld[2][1] = orientation[1][2] * inverseInertiaTensorLocal.z; + outInverseInertiaTensorWorld[2][2] = orientation[2][2] * inverseInertiaTensorLocal.z; + + outInverseInertiaTensorWorld = orientation * outInverseInertiaTensorWorld; +} + } #endif diff --git a/include/reactphysics3d/components/JointComponents.h b/include/reactphysics3d/components/JointComponents.h index e535dbd1..00c2b372 100644 --- a/include/reactphysics3d/components/JointComponents.h +++ b/include/reactphysics3d/components/JointComponents.h @@ -157,6 +157,11 @@ class JointComponents : public Components { friend class BroadPhaseSystem; friend class ConstraintSolverSystem; friend class PhysicsWorld; + friend class SolveBallAndSocketJointSystem; + friend class SolveFixedJointSystem; + friend class SolveHingeJointSystem; + friend class SolveSliderJointSystem; + }; // Return the entity of the first body of a joint diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 86d7cb62..752e5f52 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -888,16 +888,6 @@ void RigidBody::resetOverlappingPairs() { askForBroadPhaseCollisionCheck(); } -/// Compute the inverse of the inertia tensor in world coordinates. -void RigidBody::computeWorldInertiaTensorInverse(const Matrix3x3& orientation, const Vector3& inverseInertiaTensorLocal, Matrix3x3& outInverseInertiaTensorWorld) { - - Matrix3x3 orientationTranspose = orientation.getTranspose(); - 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 /** * @param isAllowedToSleep True if the body is allowed to sleep diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp index 078a1dd7..e46707c4 100644 --- a/src/systems/SolveBallAndSocketJointSystem.cpp +++ b/src/systems/SolveBallAndSocketJointSystem.cpp @@ -48,13 +48,15 @@ SolveBallAndSocketJointSystem::SolveBallAndSocketJointSystem(PhysicsWorld& world void SolveBallAndSocketJointSystem::initBeforeSolve() { // For each joint - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mBallAndSocketJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity)); assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity)); @@ -65,13 +67,14 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { } // For each joint - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -82,9 +85,10 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { } // For each joint - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Compute the corresponding skew-symmetric matrices const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i]; @@ -93,8 +97,8 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { Matrix3x3 skewSymmetricMatrixU2 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -122,13 +126,14 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { const decimal biasFactor = (BETA / mTimeStep); // For each joint - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i]; const Vector3& r2World = mBallAndSocketJointComponents.mR2World[i]; @@ -138,7 +143,7 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { // Compute the bias "b" of the constraint mBallAndSocketJointComponents.mBiasVector[i].setToZero(); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mBallAndSocketJointComponents.mBiasVector[i] = biasFactor * (x2 + r2World - x1 - r1World); } } @@ -159,12 +164,14 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { void SolveBallAndSocketJointSystem::warmstart() { // For each joint component - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mBallAndSocketJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -202,12 +209,14 @@ void SolveBallAndSocketJointSystem::warmstart() { void SolveBallAndSocketJointSystem::solveVelocityConstraint() { // For each joint component - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mBallAndSocketJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -249,16 +258,18 @@ void SolveBallAndSocketJointSystem::solveVelocityConstraint() { void SolveBallAndSocketJointSystem::solvePositionConstraint() { // For each joint component - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbEnabledJoints = mBallAndSocketJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Recompute the world inverse inertia tensors const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix(); @@ -271,16 +282,17 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { } // For each joint component - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Compute the vector from body center to the anchor point in world-space mBallAndSocketJointComponents.mR1World[i] = mRigidBodyComponents.getConstrainedOrientation(body1Entity) * @@ -290,16 +302,17 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { } // For each joint component - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -330,16 +343,17 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { } // For each joint component - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); diff --git a/src/systems/SolveFixedJointSystem.cpp b/src/systems/SolveFixedJointSystem.cpp index 59a07089..054d78e5 100644 --- a/src/systems/SolveFixedJointSystem.cpp +++ b/src/systems/SolveFixedJointSystem.cpp @@ -48,13 +48,15 @@ SolveFixedJointSystem::SolveFixedJointSystem(PhysicsWorld& world, RigidBodyCompo void SolveFixedJointSystem::initBeforeSolve() { // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mFixedJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity)); assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity)); @@ -65,13 +67,14 @@ void SolveFixedJointSystem::initBeforeSolve() { } // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -82,13 +85,14 @@ void SolveFixedJointSystem::initBeforeSolve() { } // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Compute the corresponding skew-symmetric matrices Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR1World[i]); @@ -118,13 +122,14 @@ void SolveFixedJointSystem::initBeforeSolve() { const decimal biasFactor = BETA / mTimeStep; // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Get the bodies positions and orientations const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity); @@ -135,19 +140,20 @@ void SolveFixedJointSystem::initBeforeSolve() { // Compute the bias "b" of the constraint for the 3 translation constraints mFixedJointComponents.mBiasTranslation[i].setToZero(); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mFixedJointComponents.mBiasTranslation[i] = biasFactor * (x2 + r2World - x1 - r1World); } } // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation contraints (3x3 matrix) mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mI1[i] + mFixedJointComponents.mI2[i]; @@ -158,13 +164,14 @@ void SolveFixedJointSystem::initBeforeSolve() { } // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Compute the bias "b" for the 3 rotation constraints mFixedJointComponents.mBiasRotation[i].setToZero(); @@ -172,7 +179,7 @@ void SolveFixedJointSystem::initBeforeSolve() { const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { const Quaternion qError = orientationBody2 * mFixedJointComponents.mInitOrientationDifferenceInv[i] * orientationBody1.getInverse(); mFixedJointComponents.mBiasRotation[i] = biasFactor * decimal(2.0) * qError.getVectorV(); } @@ -182,7 +189,7 @@ void SolveFixedJointSystem::initBeforeSolve() { if (!mIsWarmStartingActive) { // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { // Reset the accumulated impulses mFixedJointComponents.mImpulseTranslation[i].setToZero(); @@ -195,13 +202,15 @@ void SolveFixedJointSystem::initBeforeSolve() { void SolveFixedJointSystem::warmstart() { // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mFixedJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -253,13 +262,15 @@ void SolveFixedJointSystem::warmstart() { void SolveFixedJointSystem::solveVelocityConstraint() { // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mFixedJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -334,17 +345,19 @@ void SolveFixedJointSystem::solveVelocityConstraint() { void SolveFixedJointSystem::solvePositionConstraint() { // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbEnabledJoints = mFixedJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Recompute the world inverse inertia tensors const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix(); @@ -357,39 +370,41 @@ void SolveFixedJointSystem::solvePositionConstraint() { } // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Get the bodies positions and orientations const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity); const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity); // Compute the vector from body center to the anchor point in world-space - mFixedJointComponents.mR1World[i] = q1 * mFixedJointComponents.getLocalAnchorPointBody1(jointEntity); - mFixedJointComponents.mR2World[i] = q2 * mFixedJointComponents.getLocalAnchorPointBody2(jointEntity); + mFixedJointComponents.mR1World[i] = q1 * mFixedJointComponents.mLocalAnchorPointBody1[i]; + mFixedJointComponents.mR2World[i] = q2 * mFixedJointComponents.mLocalAnchorPointBody2[i]; } // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -422,17 +437,18 @@ void SolveFixedJointSystem::solvePositionConstraint() { } // For each joint - for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Vector3& r1World = mFixedJointComponents.mR1World[i]; const Vector3& r2World = mFixedJointComponents.mR2World[i]; diff --git a/src/systems/SolveHingeJointSystem.cpp b/src/systems/SolveHingeJointSystem.cpp index ee8864ea..45b66aed 100644 --- a/src/systems/SolveHingeJointSystem.cpp +++ b/src/systems/SolveHingeJointSystem.cpp @@ -48,13 +48,15 @@ SolveHingeJointSystem::SolveHingeJointSystem(PhysicsWorld& world, RigidBodyCompo void SolveHingeJointSystem::initBeforeSolve() { // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mHingeJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity)); assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity)); @@ -65,13 +67,14 @@ void SolveHingeJointSystem::initBeforeSolve() { } // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -84,13 +87,14 @@ void SolveHingeJointSystem::initBeforeSolve() { const decimal biasFactor = (BETA / mTimeStep); // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -109,19 +113,20 @@ void SolveHingeJointSystem::initBeforeSolve() { // Compute the bias "b" of the rotation constraints mHingeJointComponents.mBiasRotation[i].setToZero(); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mHingeJointComponents.mBiasRotation[i] = biasFactor * Vector2(a1.dot(b2), a1.dot(c2)); } } // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Compute the corresponding skew-symmetric matrices Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR1World[i]); @@ -148,13 +153,14 @@ void SolveHingeJointSystem::initBeforeSolve() { } // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Get the bodies positions and orientations const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity); @@ -162,19 +168,20 @@ void SolveHingeJointSystem::initBeforeSolve() { // Compute the bias "b" of the translation constraints mHingeJointComponents.mBiasTranslation[i].setToZero(); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mHingeJointComponents.mBiasTranslation[i] = biasFactor * (x2 + mHingeJointComponents.mR2World[i] - x1 - mHingeJointComponents.mR1World[i]); } } // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Matrix3x3& i1 = mHingeJointComponents.mI1[i]; const Matrix3x3& i2 = mHingeJointComponents.mI2[i]; @@ -202,7 +209,7 @@ void SolveHingeJointSystem::initBeforeSolve() { if (!mIsWarmStartingActive) { // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { // Reset all the accumulated impulses mHingeJointComponents.mImpulseTranslation[i].setToZero(); @@ -214,13 +221,14 @@ void SolveHingeJointSystem::initBeforeSolve() { } // For each joint - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -261,13 +269,13 @@ void SolveHingeJointSystem::initBeforeSolve() { // Compute the bias "b" of the lower limit constraint mHingeJointComponents.mBLowerLimit[i] = decimal(0.0); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mHingeJointComponents.mBLowerLimit[i] = biasFactor * lowerLimitError; } // Compute the bias "b" of the upper limit constraint mHingeJointComponents.mBUpperLimit[i] = decimal(0.0); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mHingeJointComponents.mBUpperLimit[i] = biasFactor * upperLimitError; } } @@ -279,13 +287,15 @@ void SolveHingeJointSystem::initBeforeSolve() { void SolveHingeJointSystem::warmstart() { // For each joint component - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mHingeJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -357,13 +367,15 @@ void SolveHingeJointSystem::warmstart() { void SolveHingeJointSystem::solveVelocityConstraint() { // For each joint component - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mHingeJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -528,16 +540,18 @@ void SolveHingeJointSystem::solveVelocityConstraint() { void SolveHingeJointSystem::solvePositionConstraint() { // For each joint component - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbEnabledJoints = mHingeJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; // Get the bodies entities - Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Recompute the world inverse inertia tensors const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix(); @@ -550,16 +564,17 @@ void SolveHingeJointSystem::solvePositionConstraint() { } // For each joint component - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity); const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity); @@ -570,16 +585,17 @@ void SolveHingeJointSystem::solvePositionConstraint() { } // For each joint component - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Compute the corresponding skew-symmetric matrices Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR1World[i]); @@ -607,16 +623,17 @@ void SolveHingeJointSystem::solvePositionConstraint() { } // For each joint component - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -730,16 +747,17 @@ void SolveHingeJointSystem::solvePositionConstraint() { } // For each joint component - for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity); Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity); diff --git a/src/systems/SolveSliderJointSystem.cpp b/src/systems/SolveSliderJointSystem.cpp index ac44ef7d..914ffcf8 100644 --- a/src/systems/SolveSliderJointSystem.cpp +++ b/src/systems/SolveSliderJointSystem.cpp @@ -48,13 +48,15 @@ SolveSliderJointSystem::SolveSliderJointSystem(PhysicsWorld& world, RigidBodyCom void SolveSliderJointSystem::initBeforeSolve() { // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mSliderJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity)); assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity)); @@ -65,13 +67,14 @@ void SolveSliderJointSystem::initBeforeSolve() { } // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -82,11 +85,12 @@ void SolveSliderJointSystem::initBeforeSolve() { } // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); // Compute the two orthogonal vectors to the slider axis in world-space @@ -95,7 +99,7 @@ void SolveSliderJointSystem::initBeforeSolve() { } // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { mSliderJointComponents.mN1[i] = mSliderJointComponents.mSliderAxisWorld[i].getOneUnitOrthogonalVector(); mSliderJointComponents.mN2[i] = mSliderJointComponents.mSliderAxisWorld[i].cross(mSliderJointComponents.mN1[i]); @@ -104,13 +108,14 @@ void SolveSliderJointSystem::initBeforeSolve() { const decimal biasFactor = (BETA / mTimeStep); // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -147,7 +152,7 @@ void SolveSliderJointSystem::initBeforeSolve() { // Compute the bias "b" of the translation constraint mSliderJointComponents.mBiasTranslation[i].setToZero(); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mSliderJointComponents.mBiasTranslation[i].x = u.dot(mSliderJointComponents.mN1[i]); mSliderJointComponents.mBiasTranslation[i].y = u.dot(mSliderJointComponents.mN2[i]); mSliderJointComponents.mBiasTranslation[i] *= biasFactor; @@ -173,20 +178,20 @@ void SolveSliderJointSystem::initBeforeSolve() { // Compute the bias "b" of the lower limit constraint mSliderJointComponents.mBLowerLimit[i] = decimal(0.0); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mSliderJointComponents.mBLowerLimit[i] = biasFactor * lowerLimitError; } // Compute the bias "b" of the upper limit constraint mSliderJointComponents.mBUpperLimit[i] = decimal(0.0); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mSliderJointComponents.mBUpperLimit[i] = biasFactor * upperLimitError; } } } // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { // Compute the cross products used in the Jacobians mSliderJointComponents.mR2CrossN1[i] = mSliderJointComponents.mR2[i].cross(mSliderJointComponents.mN1[i]); @@ -195,13 +200,14 @@ void SolveSliderJointSystem::initBeforeSolve() { } // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Vector3& r2CrossN1 = mSliderJointComponents.mR2CrossN1[i]; const Vector3& r2CrossN2 = mSliderJointComponents.mR2CrossN2[i]; @@ -250,20 +256,21 @@ void SolveSliderJointSystem::initBeforeSolve() { } // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); // Compute the bias "b" of the rotation constraint mSliderJointComponents.mBiasRotation[i].setToZero(); - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { const Quaternion qError = orientationBody2 * mSliderJointComponents.mInitOrientationDifferenceInv[i] * orientationBody1.getInverse(); mSliderJointComponents.mBiasRotation[i] = biasFactor * decimal(2.0) * qError.getVectorV(); } @@ -286,7 +293,7 @@ void SolveSliderJointSystem::initBeforeSolve() { if (!mIsWarmStartingActive) { // For each joint - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { // Reset all the accumulated impulses mSliderJointComponents.mImpulseTranslation[i].setToZero(); @@ -302,13 +309,15 @@ void SolveSliderJointSystem::initBeforeSolve() { void SolveSliderJointSystem::warmstart() { // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mSliderJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -380,13 +389,15 @@ void SolveSliderJointSystem::warmstart() { void SolveSliderJointSystem::solveVelocityConstraint() { // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbJoints = mSliderJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -444,13 +455,14 @@ void SolveSliderJointSystem::solveVelocityConstraint() { } // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -482,13 +494,14 @@ void SolveSliderJointSystem::solveVelocityConstraint() { } // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -607,17 +620,19 @@ void SolveSliderJointSystem::solveVelocityConstraint() { void SolveSliderJointSystem::solvePositionConstraint() { // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + const uint32 nbEnabledJoints = mSliderJointComponents.getNbEnabledComponents(); + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Recompute the world inverse inertia tensors const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix(); @@ -630,17 +645,18 @@ void SolveSliderJointSystem::solvePositionConstraint() { } // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity); const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity); @@ -651,17 +667,18 @@ void SolveSliderJointSystem::solvePositionConstraint() { } // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -776,17 +793,18 @@ void SolveSliderJointSystem::solvePositionConstraint() { } // For each joint component - for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < nbEnabledJoints; i++) { const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; + const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies entities - const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); - const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; + const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);