mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix issue with angular lock axis factor

Browse Source
This commit is contained in:
Daniel Chappuis 2021-01-22 21:54:44 +01:00
parent 5af0cbfae4
commit c615555daa
6 changed files with 70 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/systems/ContactSolverSystem.cpp
View File

@ -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;

4
src/systems/DynamicsSystem.cpp
View File

@ -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

12
src/systems/SolveBallAndSocketJointSystem.cpp
View File

@ -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;

20
src/systems/SolveFixedJointSystem.cpp
View File

@ -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);

40
src/systems/SolveHingeJointSystem.cpp
View File

@ -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);

36
src/systems/SolveSliderJointSystem.cpp
View File

@ -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;