From 20789713319ee0f858c17090ebaff432a613849b Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 12 Oct 2020 23:56:33 +0200 Subject: [PATCH] Optimization of joints solver --- src/systems/SolveBallAndSocketJointSystem.cpp | 114 +-------- src/systems/SolveFixedJointSystem.cpp | 136 +---------- src/systems/SolveHingeJointSystem.cpp | 185 ++------------- src/systems/SolveSliderJointSystem.cpp | 217 ++---------------- 4 files changed, 66 insertions(+), 586 deletions(-) diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp index e46707c4..c41388c7 100644 --- a/src/systems/SolveBallAndSocketJointSystem.cpp +++ b/src/systems/SolveBallAndSocketJointSystem.cpp @@ -47,6 +47,8 @@ SolveBallAndSocketJointSystem::SolveBallAndSocketJointSystem(PhysicsWorld& world // Initialize before solving the constraint void SolveBallAndSocketJointSystem::initBeforeSolve() { + const decimal biasFactor = (BETA / mTimeStep); + // For each joint const uint32 nbJoints = mBallAndSocketJointComponents.getNbEnabledComponents(); for (uint32 i=0; i < nbJoints; i++) { @@ -64,17 +66,6 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { // Get the inertia tensor of bodies mBallAndSocketJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity); mBallAndSocketJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity); - } - - // For each joint - 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.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -82,13 +73,6 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { // Compute the vector from body center to the anchor point in world-space mBallAndSocketJointComponents.mR1World[i] = orientationBody1 * mBallAndSocketJointComponents.mLocalAnchorPointBody1[i]; mBallAndSocketJointComponents.mR2World[i] = orientationBody2 * mBallAndSocketJointComponents.mLocalAnchorPointBody2[i]; - } - - // For each joint - 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]; @@ -96,10 +80,6 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r1World); Matrix3x3 skewSymmetricMatrixU2 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World); - // Get the bodies entities - const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -121,22 +101,6 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse(); } - } - - const decimal biasFactor = (BETA / mTimeStep); - - // For each joint - 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.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i]; - const Vector3& r2World = mBallAndSocketJointComponents.mR2World[i]; const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity); const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(body2Entity); @@ -146,13 +110,9 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mBallAndSocketJointComponents.mBiasVector[i] = biasFactor * (x2 + r2World - x1 - r1World); } - } - // If warm-starting is not enabled - if (!mIsWarmStartingActive) { - - // For each joint - for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) { + // If warm-starting is not enabled + if (!mIsWarmStartingActive) { // Reset the accumulated impulse mBallAndSocketJointComponents.mImpulse[i].setToZero(); @@ -271,51 +231,23 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; + const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); + const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); + // Recompute the world inverse inertia tensors const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix(); - RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity), + RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody1], mBallAndSocketJointComponents.mI1[i]); const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix(); - RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity), + RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody2], mBallAndSocketJointComponents.mI2[i]); - } - - // For each joint component - 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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - 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) * + mBallAndSocketJointComponents.mR1World[i] = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1] * mBallAndSocketJointComponents.mLocalAnchorPointBody1[i]; - mBallAndSocketJointComponents.mR2World[i] = mRigidBodyComponents.getConstrainedOrientation(body2Entity) * + mBallAndSocketJointComponents.mR2World[i] = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2] * mBallAndSocketJointComponents.mLocalAnchorPointBody2[i]; - } - - // For each joint component - 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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i]; const Vector3& r2World = mBallAndSocketJointComponents.mR2World[i]; @@ -340,30 +272,10 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse(); } - } - - // For each joint component - 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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; - const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i]; - const Vector3& r2World = mBallAndSocketJointComponents.mR2World[i]; - // Compute the constraint error (value of the C(x) function) const Vector3 constraintError = (x2 + r2World - x1 - r1World); @@ -376,10 +288,6 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { const Vector3 linearImpulseBody1 = -lambda; const Vector3 angularImpulseBody1 = lambda.cross(r1World); - // Get the inverse mass and inverse inertia tensors of the bodies - const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; - const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; - // Compute the pseudo velocity of body 1 const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; const Vector3 w1 = mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1; diff --git a/src/systems/SolveFixedJointSystem.cpp b/src/systems/SolveFixedJointSystem.cpp index 054d78e5..ced19853 100644 --- a/src/systems/SolveFixedJointSystem.cpp +++ b/src/systems/SolveFixedJointSystem.cpp @@ -47,6 +47,8 @@ SolveFixedJointSystem::SolveFixedJointSystem(PhysicsWorld& world, RigidBodyCompo // Initialize before solving the constraint void SolveFixedJointSystem::initBeforeSolve() { + const decimal biasFactor = BETA / mTimeStep; + // For each joint const uint32 nbJoints = mFixedJointComponents.getNbEnabledComponents(); for (uint32 i=0; i < nbJoints; i++) { @@ -64,17 +66,6 @@ void SolveFixedJointSystem::initBeforeSolve() { // Get the inertia tensor of bodies mFixedJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity); mFixedJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity); - } - - // For each joint - 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.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -82,17 +73,6 @@ void SolveFixedJointSystem::initBeforeSolve() { // Compute the vector from body center to the anchor point in world-space mFixedJointComponents.mR1World[i] = orientationBody1 * mFixedJointComponents.mLocalAnchorPointBody1[i]; mFixedJointComponents.mR2World[i] = orientationBody2 * mFixedJointComponents.mLocalAnchorPointBody2[i]; - } - - // For each joint - 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.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Compute the corresponding skew-symmetric matrices Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR1World[i]); @@ -117,19 +97,6 @@ void SolveFixedJointSystem::initBeforeSolve() { mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(); } - } - - const decimal biasFactor = BETA / mTimeStep; - - // For each joint - 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.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Get the bodies positions and orientations const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity); @@ -143,17 +110,6 @@ void SolveFixedJointSystem::initBeforeSolve() { if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mFixedJointComponents.mBiasTranslation[i] = biasFactor * (x2 + r2World - x1 - r1World); } - } - - // For each joint - 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.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]; @@ -161,35 +117,17 @@ void SolveFixedJointSystem::initBeforeSolve() { mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse(); } - } - - // For each joint - 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.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Compute the bias "b" for the 3 rotation constraints mFixedJointComponents.mBiasRotation[i].setToZero(); - const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); - const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); - 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(); } - } - // If warm-starting is not enabled - if (!mIsWarmStartingActive) { - - // For each joint - for (uint32 i=0; i < nbJoints; i++) { + // If warm-starting is not enabled + if (!mIsWarmStartingActive) { // Reset the accumulated impulses mFixedJointComponents.mImpulseTranslation[i].setToZero(); @@ -367,47 +305,17 @@ void SolveFixedJointSystem::solvePositionConstraint() { const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix(); RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity), mFixedJointComponents.mI2[i]); - } - // For each joint - 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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; + const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); + const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); // Get the bodies positions and orientations - const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity); - const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity); + Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; + Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; // Compute the vector from body center to the anchor point in world-space mFixedJointComponents.mR1World[i] = q1 * mFixedJointComponents.mLocalAnchorPointBody1[i]; mFixedJointComponents.mR2World[i] = q2 * mFixedJointComponents.mLocalAnchorPointBody2[i]; - } - - // For each joint - 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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); // Get the inverse mass and inverse inertia tensors of the bodies decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; @@ -434,33 +342,9 @@ void SolveFixedJointSystem::solvePositionConstraint() { mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(); } - } - - // For each joint - 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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - const Vector3& r1World = mFixedJointComponents.mR1World[i]; - const Vector3& r2World = mFixedJointComponents.mR2World[i]; - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; - Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; - Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; - // Compute position error for the 3 translation constraints const Vector3 errorTranslation = x2 + r2World - x1 - r1World; @@ -471,8 +355,6 @@ void SolveFixedJointSystem::solvePositionConstraint() { Vector3 linearImpulseBody1 = -lambdaTranslation; Vector3 angularImpulseBody1 = lambdaTranslation.cross(r1World); - const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; - // Compute the pseudo velocity of body 1 const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; Vector3 w1 = mFixedJointComponents.mI2[i] * angularImpulseBody1; @@ -485,8 +367,6 @@ void SolveFixedJointSystem::solvePositionConstraint() { // Compute the impulse of body 2 Vector3 angularImpulseBody2 = -lambdaTranslation.cross(r2World); - const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; - // Compute the pseudo velocity of body 2 const Vector3 v2 = inverseMassBody2 * lambdaTranslation; Vector3 w2 = mFixedJointComponents.mI2[i] * angularImpulseBody2; diff --git a/src/systems/SolveHingeJointSystem.cpp b/src/systems/SolveHingeJointSystem.cpp index 45b66aed..8279664e 100644 --- a/src/systems/SolveHingeJointSystem.cpp +++ b/src/systems/SolveHingeJointSystem.cpp @@ -47,6 +47,8 @@ SolveHingeJointSystem::SolveHingeJointSystem(PhysicsWorld& world, RigidBodyCompo // Initialize before solving the constraint void SolveHingeJointSystem::initBeforeSolve() { + const decimal biasFactor = (BETA / mTimeStep); + // For each joint const uint32 nbJoints = mHingeJointComponents.getNbEnabledComponents(); for (uint32 i=0; i < nbJoints; i++) { @@ -58,23 +60,15 @@ void SolveHingeJointSystem::initBeforeSolve() { const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity)); - assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity)); + const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); + const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); + + assert(!mRigidBodyComponents.mIsEntityDisabled[componentIndexBody1]); + assert(!mRigidBodyComponents.mIsEntityDisabled[componentIndexBody2]); // Get the inertia tensor of bodies - mHingeJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity); - mHingeJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity); - } - - // For each joint - 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.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; + mHingeJointComponents.mI1[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody1]; + mHingeJointComponents.mI2[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody2]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -82,22 +76,6 @@ void SolveHingeJointSystem::initBeforeSolve() { // Compute the vector from body center to the anchor point in world-space mHingeJointComponents.mR1World[i] = orientationBody1 * mHingeJointComponents.mLocalAnchorPointBody1[i]; mHingeJointComponents.mR2World[i] = orientationBody2 * mHingeJointComponents.mLocalAnchorPointBody2[i]; - } - - const decimal biasFactor = (BETA / mTimeStep); - - // For each joint - 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.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); - const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); // Compute vectors needed in the Jacobian Vector3& a1 = mHingeJointComponents.mA1[i]; @@ -116,25 +94,11 @@ void SolveHingeJointSystem::initBeforeSolve() { 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 < nbJoints; i++) { - - const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; - const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); - - // Get the bodies entities - 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]); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR2World[i]); - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; @@ -150,38 +114,16 @@ void SolveHingeJointSystem::initBeforeSolve() { mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(); } - } - - // For each joint - 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.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; // Get the bodies positions and orientations - const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity); - const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(body2Entity); + const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody1]; + const Vector3& x2 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody2]; // Compute the bias "b" of the translation constraints mHingeJointComponents.mBiasTranslation[i].setToZero(); 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 < nbJoints; i++) { - - const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; - const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); - - // Get the bodies entities - const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Matrix3x3& i1 = mHingeJointComponents.mI1[i]; const Matrix3x3& i2 = mHingeJointComponents.mI2[i]; @@ -199,17 +141,13 @@ void SolveHingeJointSystem::initBeforeSolve() { const decimal el22 = c2CrossA1.dot(i1C2CrossA1) + c2CrossA1.dot(i2C2CrossA1); const Matrix2x2 matrixKRotation(el11, el12, el21, el22); mHingeJointComponents.mInverseMassMatrixRotation[i].setToZero(); - if (mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || - mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse(); } - } - // If warm-starting is not enabled - if (!mIsWarmStartingActive) { - - // For each joint - for (uint32 i=0; i < nbJoints; i++) { + // If warm-starting is not enabled + if (!mIsWarmStartingActive) { // Reset all the accumulated impulses mHingeJointComponents.mImpulseTranslation[i].setToZero(); @@ -218,20 +156,6 @@ void SolveHingeJointSystem::initBeforeSolve() { mHingeJointComponents.mImpulseUpperLimit[i] = decimal(0.0); mHingeJointComponents.mImpulseMotor[i] = decimal(0.0); } - } - - // For each joint - 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.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); - const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); // Compute the current angle around the hinge axis decimal hingeAngle = computeCurrentHingeAngle(jointEntity, orientationBody1, orientationBody2); @@ -553,49 +477,24 @@ void SolveHingeJointSystem::solvePositionConstraint() { Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; + const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); + const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); + // Recompute the world inverse inertia tensors const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix(); - RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity), + RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody1], mHingeJointComponents.mI1[i]); const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix(); - RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity), + RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody2], mHingeJointComponents.mI2[i]); - } - // For each joint component - 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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity); - const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity); + Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; + Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; // Compute the vector from body center to the anchor point in world-space mHingeJointComponents.mR1World[i] = q1 * mHingeJointComponents.mLocalAnchorPointBody1[i]; mHingeJointComponents.mR2World[i] = q2 * mHingeJointComponents.mLocalAnchorPointBody2[i]; - } - - // For each joint component - 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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - // Get the bodies entities - 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]); @@ -603,9 +502,6 @@ void SolveHingeJointSystem::solvePositionConstraint() { // --------------- Translation Constraints --------------- // - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints const decimal body1InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; const decimal body2InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; @@ -620,26 +516,6 @@ void SolveHingeJointSystem::solvePositionConstraint() { mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(); } - } - - // For each joint component - 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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - - Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; - Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i]; Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i]; @@ -744,23 +620,6 @@ void SolveHingeJointSystem::solvePositionConstraint() { // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2.normalize(); - } - - // For each joint component - 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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity); - Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity); // Compute the current angle around the hinge axis const decimal hingeAngle = computeCurrentHingeAngle(jointEntity, q1, q2); diff --git a/src/systems/SolveSliderJointSystem.cpp b/src/systems/SolveSliderJointSystem.cpp index 914ffcf8..ee47357f 100644 --- a/src/systems/SolveSliderJointSystem.cpp +++ b/src/systems/SolveSliderJointSystem.cpp @@ -47,6 +47,8 @@ SolveSliderJointSystem::SolveSliderJointSystem(PhysicsWorld& world, RigidBodyCom // Initialize before solving the constraint void SolveSliderJointSystem::initBeforeSolve() { + const decimal biasFactor = (BETA / mTimeStep); + // For each joint const uint32 nbJoints = mSliderJointComponents.getNbEnabledComponents(); for (uint32 i=0; i < nbJoints; i++) { @@ -58,23 +60,15 @@ void SolveSliderJointSystem::initBeforeSolve() { const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity)); - assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity)); + const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); + const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); + + assert(!mRigidBodyComponents.mIsEntityDisabled[componentIndexBody1]); + assert(!mRigidBodyComponents.mIsEntityDisabled[componentIndexBody2]); // Get the inertia tensor of bodies - mSliderJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity); - mSliderJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity); - } - - // For each joint - 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.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; + mSliderJointComponents.mI1[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody1]; + mSliderJointComponents.mI2[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody2]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -82,43 +76,13 @@ void SolveSliderJointSystem::initBeforeSolve() { // Vector from body center to the anchor point mSliderJointComponents.mR1[i] = orientationBody1 * mSliderJointComponents.mLocalAnchorPointBody1[i]; mSliderJointComponents.mR2[i] = orientationBody2 * mSliderJointComponents.mLocalAnchorPointBody2[i]; - } - - // For each joint - for (uint32 i=0; i < nbJoints; i++) { - - const Entity jointEntity = mSliderJointComponents.mJointEntities[i]; - const uint32 jointIndex = mJointComponents.getEntityIndex(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 mSliderJointComponents.mSliderAxisWorld[i] = orientationBody1 * mSliderJointComponents.mSliderAxisBody1[i]; mSliderJointComponents.mSliderAxisWorld[i].normalize(); - } - - // For each joint - for (uint32 i=0; i < nbJoints; i++) { mSliderJointComponents.mN1[i] = mSliderJointComponents.mSliderAxisWorld[i].getOneUnitOrthogonalVector(); mSliderJointComponents.mN2[i] = mSliderJointComponents.mSliderAxisWorld[i].cross(mSliderJointComponents.mN1[i]); - } - - const decimal biasFactor = (BETA / mTimeStep); - - // For each joint - 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.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody1]; const Vector3& x2 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody2]; @@ -188,26 +152,11 @@ void SolveSliderJointSystem::initBeforeSolve() { mSliderJointComponents.mBUpperLimit[i] = biasFactor * upperLimitError; } } - } - - // For each joint - 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]); mSliderJointComponents.mR2CrossN2[i] = mSliderJointComponents.mR2[i].cross(mSliderJointComponents.mN2[i]); mSliderJointComponents.mR2CrossSliderAxis[i] = mSliderJointComponents.mR2[i].cross(mSliderJointComponents.mSliderAxisWorld[i]); - } - - // For each joint - 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.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; const Vector3& r2CrossN1 = mSliderJointComponents.mR2CrossN1[i]; const Vector3& r2CrossN2 = mSliderJointComponents.mR2CrossN2[i]; @@ -217,9 +166,6 @@ void SolveSliderJointSystem::initBeforeSolve() { const Matrix3x3& i1 = mSliderJointComponents.mI1[i]; const Matrix3x3& i2 = mSliderJointComponents.mI2[i]; - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; @@ -253,20 +199,6 @@ void SolveSliderJointSystem::initBeforeSolve() { mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mInverseMassMatrixRotation[i].getInverse(); } - } - - // For each joint - 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.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(); @@ -278,8 +210,8 @@ void SolveSliderJointSystem::initBeforeSolve() { // If the motor is enabled if (mSliderJointComponents.mIsMotorEnabled[i]) { - const decimal body1MassInverse = mRigidBodyComponents.getMassInverse(body1Entity); - const decimal body2MassInverse = mRigidBodyComponents.getMassInverse(body2Entity); + const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; + const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; const decimal sumInverseMass = body1MassInverse + body2MassInverse; // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) @@ -287,13 +219,9 @@ void SolveSliderJointSystem::initBeforeSolve() { mSliderJointComponents.mInverseMassMatrixMotor[i] = (mSliderJointComponents.mInverseMassMatrixMotor[i] > decimal(0.0)) ? decimal(1.0) / mSliderJointComponents.mInverseMassMatrixMotor[i] : decimal(0.0); } - } - // If warm-starting is not enabled - if (!mIsWarmStartingActive) { - - // For each joint - for (uint32 i=0; i < nbJoints; i++) { + // If warm-starting is not enabled + if (!mIsWarmStartingActive) { // Reset all the accumulated impulses mSliderJointComponents.mImpulseTranslation[i].setToZero(); @@ -452,26 +380,9 @@ void SolveSliderJointSystem::solveVelocityConstraint() { // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; w2 += i2 * angularImpulseBody2; - } - - // For each joint component - 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.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); // --------------- Rotation Constraints --------------- // - Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1]; - Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2]; - // Compute J*v for the 3 rotation constraints const Vector3 JvRotation = w2 - w1; @@ -481,36 +392,16 @@ void SolveSliderJointSystem::solveVelocityConstraint() { mSliderJointComponents.mImpulseRotation[i] += deltaLambda2; // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - Vector3 angularImpulseBody1 = -deltaLambda2; + angularImpulseBody1 = -deltaLambda2; - // Apply the impulse to the body to body 1 + // Apply the impulse to the body 1 w1 += mSliderJointComponents.mI1[i] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 - Vector3 angularImpulseBody2 = deltaLambda2; + angularImpulseBody2 = deltaLambda2; // Apply the impulse to the body 2 w2 += mSliderJointComponents.mI2[i] * angularImpulseBody2; - } - - // For each joint component - 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.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - - Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody1]; - Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody2]; - - decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; - decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; const Vector3& r2CrossSliderAxis = mSliderJointComponents.mR2CrossSliderAxis[i]; const Vector3& r1PlusUCrossSliderAxis = mSliderJointComponents.mR1PlusUCrossSliderAxis[i]; @@ -634,54 +525,24 @@ void SolveSliderJointSystem::solvePositionConstraint() { const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; + const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); + const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); + // Recompute the world inverse inertia tensors const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix(); - RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity), + RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody1], mSliderJointComponents.mI1[i]); const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix(); - RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity), + RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody2], mSliderJointComponents.mI2[i]); - } - // For each joint component - 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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity); - const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity); + Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; + Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; // Vector from body center to the anchor point mSliderJointComponents.mR1[i] = q1 * mSliderJointComponents.mLocalAnchorPointBody1[i]; mSliderJointComponents.mR2[i] = q2 * mSliderJointComponents.mLocalAnchorPointBody2[i]; - } - - // For each joint component - 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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); // Get the inverse mass and inverse inertia tensors of the bodies const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; @@ -699,9 +560,6 @@ void SolveSliderJointSystem::solvePositionConstraint() { // Compute the vector u (difference between anchor points) const Vector3 u = x2 + r2 - x1 - r1; - Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; - Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; - // Compute the two orthogonal vectors to the slider axis in world-space mSliderJointComponents.mSliderAxisWorld[i] = q1 * mSliderJointComponents.mSliderAxisBody1[i]; mSliderJointComponents.mSliderAxisWorld[i].normalize(); @@ -790,31 +648,6 @@ void SolveSliderJointSystem::solvePositionConstraint() { x2 += v2; q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2.normalize(); - } - - // For each joint component - 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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; - - // Get the bodies entities - const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; - const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; - - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - - Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; - Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; - - // Get the velocities - Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1]; - Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2]; // --------------- Rotation Constraints --------------- // @@ -858,7 +691,7 @@ void SolveSliderJointSystem::solvePositionConstraint() { Vector3 lambdaRotation = mSliderJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - Vector3 angularImpulseBody1 = -lambdaRotation; + angularImpulseBody1 = -lambdaRotation; // Apply the impulse to the body 1 w1 = mSliderJointComponents.mI1[i] * angularImpulseBody1; @@ -868,7 +701,7 @@ void SolveSliderJointSystem::solvePositionConstraint() { q1.normalize(); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 - Vector3 angularImpulseBody2 = lambdaRotation; + angularImpulseBody2 = lambdaRotation; // Apply the impulse to the body 2 w2 = mSliderJointComponents.mI2[i] * angularImpulseBody2;