From c615555daa8e07344fac7e81c445b7920ec01874 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 22 Jan 2021 21:54:44 +0100 Subject: [PATCH] Fix issue with angular lock axis factor --- src/systems/ContactSolverSystem.cpp | 28 ++++++------- src/systems/DynamicsSystem.cpp | 4 +- src/systems/SolveBallAndSocketJointSystem.cpp | 12 +++--- src/systems/SolveFixedJointSystem.cpp | 20 +++++----- src/systems/SolveHingeJointSystem.cpp | 40 +++++++++---------- src/systems/SolveSliderJointSystem.cpp | 36 ++++++++--------- 6 files changed, 70 insertions(+), 70 deletions(-) diff --git a/src/systems/ContactSolverSystem.cpp b/src/systems/ContactSolverSystem.cpp index d029d9e9..e94f218d 100644 --- a/src/systems/ContactSolverSystem.cpp +++ b/src/systems/ContactSolverSystem.cpp @@ -412,11 +412,11 @@ void ContactSolverSystem::warmStart() { // Update the velocities of the body 1 by applying the impulse P mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2 * mContactConstraints[c].linearLockAxisFactorBody1; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].inverseInertiaTensorBody1 * mContactConstraints[c].angularLockAxisFactorBody1 * angularImpulseBody1; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].angularLockAxisFactorBody1 * (mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1); // Update the velocities of the body 1 by applying the impulse P mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2 * mContactConstraints[c].linearLockAxisFactorBody2; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * mContactConstraints[c].angularLockAxisFactorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].angularLockAxisFactorBody2 * (mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2); // ------ Second friction constraint at the center of the contact manifold ----- // @@ -436,14 +436,14 @@ void ContactSolverSystem::warmStart() { mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y * mContactConstraints[c].linearLockAxisFactorBody1.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z * mContactConstraints[c].linearLockAxisFactorBody1.z; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].inverseInertiaTensorBody1 * mContactConstraints[c].angularLockAxisFactorBody1 * angularImpulseBody1; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].angularLockAxisFactorBody1 * (mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1); // Update the velocities of the body 2 by applying the impulse P mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x * mContactConstraints[c].linearLockAxisFactorBody2.x; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y * mContactConstraints[c].linearLockAxisFactorBody2.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z * mContactConstraints[c].linearLockAxisFactorBody2.z; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * mContactConstraints[c].angularLockAxisFactorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].angularLockAxisFactorBody2 * (mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2); // ------ Twist friction constraint at the center of the contact manifold ------ // @@ -457,16 +457,16 @@ void ContactSolverSystem::warmStart() { angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].inverseInertiaTensorBody1 * mContactConstraints[c].angularLockAxisFactorBody1 * angularImpulseBody1; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].angularLockAxisFactorBody1 * (mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1); // Update the velocities of the body 2 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * mContactConstraints[c].angularLockAxisFactorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].angularLockAxisFactorBody2 * (mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2); // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] -= mContactConstraints[c].inverseInertiaTensorBody1 * mContactConstraints[c].angularLockAxisFactorBody1 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] -= mContactConstraints[c].angularLockAxisFactorBody1 * (mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2); // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * mContactConstraints[c].angularLockAxisFactorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].angularLockAxisFactorBody2 * (mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2); } else { // If it is a new contact manifold @@ -657,7 +657,7 @@ void ContactSolverSystem::solve() { mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y * mContactConstraints[c].linearLockAxisFactorBody1.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z * mContactConstraints[c].linearLockAxisFactorBody1.z; - Vector3 angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * mContactConstraints[c].angularLockAxisFactorBody1 * angularImpulseBody1; + Vector3 angularVelocity1 = mContactConstraints[c].angularLockAxisFactorBody1 * (mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1); mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x += angularVelocity1.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y += angularVelocity1.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z += angularVelocity1.z; @@ -667,7 +667,7 @@ void ContactSolverSystem::solve() { mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y * mContactConstraints[c].linearLockAxisFactorBody2.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z * mContactConstraints[c].linearLockAxisFactorBody2.z; - Vector3 angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * mContactConstraints[c].angularLockAxisFactorBody2 * angularImpulseBody2; + Vector3 angularVelocity2 = mContactConstraints[c].angularLockAxisFactorBody2 * (mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2); mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z; @@ -712,7 +712,7 @@ void ContactSolverSystem::solve() { mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y * mContactConstraints[c].linearLockAxisFactorBody1.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z * mContactConstraints[c].linearLockAxisFactorBody1.z; - angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * mContactConstraints[c].angularLockAxisFactorBody1 * angularImpulseBody1; + angularVelocity1 = mContactConstraints[c].angularLockAxisFactorBody1 * (mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1); mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x += angularVelocity1.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y += angularVelocity1.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z += angularVelocity1.z; @@ -722,7 +722,7 @@ void ContactSolverSystem::solve() { mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y * mContactConstraints[c].linearLockAxisFactorBody2.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z * mContactConstraints[c].linearLockAxisFactorBody2.z; - angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * mContactConstraints[c].angularLockAxisFactorBody2 * angularImpulseBody2; + angularVelocity2 = mContactConstraints[c].angularLockAxisFactorBody2 * (mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2); mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z; @@ -748,13 +748,13 @@ void ContactSolverSystem::solve() { angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * mContactConstraints[c].angularLockAxisFactorBody1 * angularImpulseBody2; + angularVelocity1 = mContactConstraints[c].angularLockAxisFactorBody1 * (mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2); mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= angularVelocity1.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= angularVelocity1.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= angularVelocity1.z; // Update the velocities of the body 1 by applying the impulse P - angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * mContactConstraints[c].angularLockAxisFactorBody2 * angularImpulseBody2; + angularVelocity2 = mContactConstraints[c].angularLockAxisFactorBody2 * (mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2); mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z; diff --git a/src/systems/DynamicsSystem.cpp b/src/systems/DynamicsSystem.cpp index d0a9aa5b..d650ba8a 100644 --- a/src/systems/DynamicsSystem.cpp +++ b/src/systems/DynamicsSystem.cpp @@ -134,8 +134,8 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) { // Integrate the external force to get the new velocity of the body mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + timeStep * mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mLinearLockAxisFactors[i] * mRigidBodyComponents.mExternalForces[i]; - mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep * mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * - mRigidBodyComponents.mAngularLockAxisFactors[i] * mRigidBodyComponents.mExternalTorques[i]; + mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep * mRigidBodyComponents.mAngularLockAxisFactors[i] * + (mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * mRigidBodyComponents.mExternalTorques[i]); } // Apply gravity force diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp index 5ee605d1..599b8f83 100644 --- a/src/systems/SolveBallAndSocketJointSystem.cpp +++ b/src/systems/SolveBallAndSocketJointSystem.cpp @@ -154,14 +154,14 @@ void SolveBallAndSocketJointSystem::warmstart() { // Apply the impulse to the body 1 v1 += mRigidBodyComponents.mInverseMasses[componentIndexBody1] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); // Compute the impulse P=J^T * lambda for the body 2 const Vector3 angularImpulseBody2 = -mBallAndSocketJointComponents.mImpulse[i].cross(r2World); // Apply the impulse to the body to the body 2 v2 += mRigidBodyComponents.mInverseMasses[componentIndexBody2] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * mBallAndSocketJointComponents.mImpulse[i]; - w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); } } @@ -203,14 +203,14 @@ void SolveBallAndSocketJointSystem::solveVelocityConstraint() { // Apply the impulse to the body 1 v1 += mRigidBodyComponents.mInverseMasses[componentIndexBody1] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); // Compute the impulse P=J^T * lambda for the body 2 const Vector3 angularImpulseBody2 = -deltaLambda.cross(mBallAndSocketJointComponents.mR2World[i]); // Apply the impulse to the body 2 v2 += mRigidBodyComponents.mInverseMasses[componentIndexBody2] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * deltaLambda; - w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); } } @@ -291,7 +291,7 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { // Compute the pseudo velocity of body 1 const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - const Vector3 w1 = mBallAndSocketJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + const Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1); // Update the body center of mass and orientation of body 1 x1 += v1; @@ -303,7 +303,7 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { // Compute the pseudo velocity of body 2 const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambda; - const Vector3 w2 = mBallAndSocketJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + const Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mBallAndSocketJointComponents.mI2[i] * angularImpulseBody2); // Update the body position/orientation of body 2 x2 += v2; diff --git a/src/systems/SolveFixedJointSystem.cpp b/src/systems/SolveFixedJointSystem.cpp index c117c4a2..a36902d2 100644 --- a/src/systems/SolveFixedJointSystem.cpp +++ b/src/systems/SolveFixedJointSystem.cpp @@ -180,7 +180,7 @@ void SolveFixedJointSystem::warmstart() { // Apply the impulse to the body 1 v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2 Vector3 angularImpulseBody2 = -impulseTranslation.cross(r2World); @@ -192,7 +192,7 @@ void SolveFixedJointSystem::warmstart() { // Apply the impulse to the body 2 v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * impulseTranslation; - w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); } } @@ -245,7 +245,7 @@ void SolveFixedJointSystem::solveVelocityConstraint() { // Apply the impulse to the body 1 v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); // Compute the impulse P=J^T * lambda for body 2 const Vector3 angularImpulseBody2 = -deltaLambda.cross(r2World); @@ -254,7 +254,7 @@ void SolveFixedJointSystem::solveVelocityConstraint() { // Apply the impulse to the body 2 v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * deltaLambda; - w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); // --------------- Rotation Constraints --------------- // @@ -272,10 +272,10 @@ void SolveFixedJointSystem::solveVelocityConstraint() { angularImpulseBody1 = -deltaLambda2; // Apply the impulse to the body 1 - w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); // Apply the impulse to the body 2 - w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * deltaLambda2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * deltaLambda2); } } @@ -355,7 +355,7 @@ void SolveFixedJointSystem::solvePositionConstraint() { // Compute the pseudo velocity of body 1 const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - Vector3 w1 = mFixedJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mFixedJointComponents.mI1[i] * angularImpulseBody1); // Update the body position/orientation of body 1 x1 += v1; @@ -367,7 +367,7 @@ void SolveFixedJointSystem::solvePositionConstraint() { // Compute the pseudo velocity of body 2 const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambdaTranslation; - Vector3 w2 = mFixedJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mFixedJointComponents.mI2[i] * angularImpulseBody2); // Update the body position/orientation of body 2 x2 += v2; @@ -419,14 +419,14 @@ void SolveFixedJointSystem::solvePositionConstraint() { angularImpulseBody1 = -lambdaRotation; // Compute the pseudo velocity of body 1 - w1 = mFixedJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mFixedJointComponents.mI1[i] * angularImpulseBody1); // Update the body position/orientation of body 1 q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); // Compute the pseudo velocity of body 2 - w2 = mFixedJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * lambdaRotation; + w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mFixedJointComponents.mI2[i] * lambdaRotation); // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); diff --git a/src/systems/SolveHingeJointSystem.cpp b/src/systems/SolveHingeJointSystem.cpp index 8725b5a3..c2a859cb 100644 --- a/src/systems/SolveHingeJointSystem.cpp +++ b/src/systems/SolveHingeJointSystem.cpp @@ -267,7 +267,7 @@ void SolveHingeJointSystem::warmstart() { // Apply the impulse to the body 1 v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - w1 += mHingeJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1); // Compute the impulse P=J^T * lambda for the 3 translation constraints of body 2 Vector3 angularImpulseBody2 = -impulseTranslation.cross(mHingeJointComponents.mR2World[i]); @@ -283,7 +283,7 @@ void SolveHingeJointSystem::warmstart() { // Apply the impulse to the body 2 v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * impulseTranslation; - w2 += mHingeJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2); } } @@ -340,14 +340,14 @@ void SolveHingeJointSystem::solveVelocityConstraint() { // Apply the impulse to the body 1 v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); // Compute the impulse P=J^T * lambda of body 2 Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(r2World); // Apply the impulse to the body 2 v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * deltaLambdaTranslation; - w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); // --------------- Rotation Constraints --------------- // @@ -367,13 +367,13 @@ void SolveHingeJointSystem::solveVelocityConstraint() { angularImpulseBody1 = -b2CrossA1 * deltaLambdaRotation.x - c2CrossA1 * deltaLambdaRotation.y; // Apply the impulse to the body 1 - w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2 angularImpulseBody2 = b2CrossA1 * deltaLambdaRotation.x + c2CrossA1 * deltaLambdaRotation.y; // Apply the impulse to the body 2 - w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); // --------------- Limits Constraints --------------- // @@ -395,13 +395,13 @@ void SolveHingeJointSystem::solveVelocityConstraint() { const Vector3 angularImpulseBody1 = -deltaLambdaLower * a1; // Apply the impulse to the body 1 - w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 const Vector3 angularImpulseBody2 = deltaLambdaLower * a1; // Apply the impulse to the body 2 - w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); } // If the upper limit is violated @@ -420,13 +420,13 @@ void SolveHingeJointSystem::solveVelocityConstraint() { const Vector3 angularImpulseBody1 = deltaLambdaUpper * a1; // Apply the impulse to the body 1 - w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 const Vector3 angularImpulseBody2 = -deltaLambdaUpper * a1; // Apply the impulse to the body 2 - w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); } } @@ -449,13 +449,13 @@ void SolveHingeJointSystem::solveVelocityConstraint() { const Vector3 angularImpulseBody1 = -deltaLambdaMotor * a1; // Apply the impulse to the body 1 - w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); // Compute the impulse P=J^T * lambda for the motor of body 2 const Vector3 angularImpulseBody2 = deltaLambdaMotor * a1; // Apply the impulse to the body 2 - w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); } } } @@ -552,7 +552,7 @@ void SolveHingeJointSystem::solvePositionConstraint() { // Compute the pseudo velocity of body 1 const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - Vector3 w1 = mHingeJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1); // Update the body position/orientation of body 1 x1 += v1; @@ -564,7 +564,7 @@ void SolveHingeJointSystem::solvePositionConstraint() { // Compute the pseudo velocity of body 2 const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambdaTranslation; - Vector3 w2 = mHingeJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2); // Update the body position/orientation of body 2 x2 += v2; @@ -603,7 +603,7 @@ void SolveHingeJointSystem::solvePositionConstraint() { angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y; // Compute the pseudo velocity of body 1 - w1 = mHingeJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1); // Update the body position/orientation of body 1 q1 += Quaternion(0, w1) * q1 * decimal(0.5); @@ -613,7 +613,7 @@ void SolveHingeJointSystem::solvePositionConstraint() { angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y; // Compute the pseudo velocity of body 2 - w2 = mHingeJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2); // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); @@ -654,7 +654,7 @@ void SolveHingeJointSystem::solvePositionConstraint() { const Vector3 angularImpulseBody1 = -lambdaLowerLimit * a1; // Compute the pseudo velocity of body 1 - const Vector3 w1 = mHingeJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + const Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1); // Update the body position/orientation of body 1 q1 += Quaternion(0, w1) * q1 * decimal(0.5); @@ -664,7 +664,7 @@ void SolveHingeJointSystem::solvePositionConstraint() { const Vector3 angularImpulseBody2 = lambdaLowerLimit * a1; // Compute the pseudo velocity of body 2 - const Vector3 w2 = mHingeJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + const Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2); // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); @@ -681,7 +681,7 @@ void SolveHingeJointSystem::solvePositionConstraint() { const Vector3 angularImpulseBody1 = lambdaUpperLimit * a1; // Compute the pseudo velocity of body 1 - const Vector3 w1 = mHingeJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + const Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1); // Update the body position/orientation of body 1 q1 += Quaternion(0, w1) * q1 * decimal(0.5); @@ -691,7 +691,7 @@ void SolveHingeJointSystem::solvePositionConstraint() { const Vector3 angularImpulseBody2 = -lambdaUpperLimit * a1; // Compute the pseudo velocity of body 2 - const Vector3 w2 = mHingeJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + const Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2); // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); diff --git a/src/systems/SolveSliderJointSystem.cpp b/src/systems/SolveSliderJointSystem.cpp index 5c5e8c8e..9eab7e03 100644 --- a/src/systems/SolveSliderJointSystem.cpp +++ b/src/systems/SolveSliderJointSystem.cpp @@ -290,7 +290,7 @@ void SolveSliderJointSystem::warmstart() { // Apply the impulse to the body 1 v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - w1 += mSliderJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1); // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 Vector3 linearImpulseBody2 = n1 * impulseTranslation.x + n2 * impulseTranslation.y; @@ -309,7 +309,7 @@ void SolveSliderJointSystem::warmstart() { // Apply the impulse to the body 2 v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; - w2 += mSliderJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2); } } @@ -371,7 +371,7 @@ void SolveSliderJointSystem::solveVelocityConstraint() { // Apply the impulse to the body 1 v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 const Vector3 linearImpulseBody2 = n1 * deltaLambda.x + n2 * deltaLambda.y; @@ -379,7 +379,7 @@ void SolveSliderJointSystem::solveVelocityConstraint() { // Apply the impulse to the body 2 v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; - w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); // --------------- Rotation Constraints --------------- // @@ -395,13 +395,13 @@ void SolveSliderJointSystem::solveVelocityConstraint() { angularImpulseBody1 = -deltaLambda2; // Apply the impulse to the body 1 - w1 += mSliderJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 angularImpulseBody2 = deltaLambda2; // Apply the impulse to the body 2 - w2 += mSliderJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2); const Vector3& r2CrossSliderAxis = mSliderJointComponents.mR2CrossSliderAxis[i]; const Vector3& r1PlusUCrossSliderAxis = mSliderJointComponents.mR1PlusUCrossSliderAxis[i]; @@ -436,7 +436,7 @@ void SolveSliderJointSystem::solveVelocityConstraint() { // Apply the impulse to the body 1 v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - w1 += mSliderJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1); // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 const Vector3 linearImpulseBody2 = deltaLambdaLower * sliderAxisWorld; @@ -444,7 +444,7 @@ void SolveSliderJointSystem::solveVelocityConstraint() { // Apply the impulse to the body 2 v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; - w2 += mSliderJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2); } // If the upper limit is violated @@ -466,7 +466,7 @@ void SolveSliderJointSystem::solveVelocityConstraint() { // Apply the impulse to the body 1 v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - w1 += mSliderJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1); // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 const Vector3 linearImpulseBody2 = -deltaLambdaUpper * sliderAxisWorld; @@ -474,7 +474,7 @@ void SolveSliderJointSystem::solveVelocityConstraint() { // Apply the impulse to the body 2 v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; - w2 += mSliderJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2); } } @@ -627,7 +627,7 @@ void SolveSliderJointSystem::solvePositionConstraint() { // Apply the impulse to the body 1 const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - Vector3 w1 = i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); // Update the body position/orientation of body 1 x1 += v1; @@ -640,7 +640,7 @@ void SolveSliderJointSystem::solvePositionConstraint() { // Apply the impulse to the body 2 const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; - Vector3 w2 = i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); // Update the body position/orientation of body 2 x2 += v2; @@ -692,7 +692,7 @@ void SolveSliderJointSystem::solvePositionConstraint() { angularImpulseBody1 = -lambdaRotation; // Apply the impulse to the body 1 - w1 = mSliderJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1); // Update the body position/orientation of body 1 q1 += Quaternion(0, w1) * q1 * decimal(0.5); @@ -702,7 +702,7 @@ void SolveSliderJointSystem::solvePositionConstraint() { angularImpulseBody2 = lambdaRotation; // Apply the impulse to the body 2 - w2 = mSliderJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2); // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); @@ -751,7 +751,7 @@ void SolveSliderJointSystem::solvePositionConstraint() { // Apply the impulse to the body 1 const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - const Vector3 w1 = mSliderJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + const Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1); // Update the body position/orientation of body 1 x1 += v1; @@ -764,7 +764,7 @@ void SolveSliderJointSystem::solvePositionConstraint() { // Apply the impulse to the body 2 const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; - const Vector3 w2 = mSliderJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + const Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2); // Update the body position/orientation of body 2 x2 += v2; @@ -790,7 +790,7 @@ void SolveSliderJointSystem::solvePositionConstraint() { // Apply the impulse to the body 1 const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; - const Vector3 w1 = mSliderJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; + const Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1); // Update the body position/orientation of body 1 x1 += v1; @@ -803,7 +803,7 @@ void SolveSliderJointSystem::solvePositionConstraint() { // Apply the impulse to the body 2 const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; - const Vector3 w2 = mSliderJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; + const Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2); // Update the body position/orientation of body 2 x2 += v2;