Solve most important constraints at the end for better simulation results
This commit is contained in:
Daniel Chappuis
parent
31c071fcca
commit
f39cad7482
|
|
@ -240,28 +240,6 @@ void SolveBallAndSocketJointSystem::solveVelocityConstraint() {
|
|||
const Matrix3x3& i1 = mBallAndSocketJointComponents.mI1[i];
|
||||
const Matrix3x3& i2 = mBallAndSocketJointComponents.mI2[i];
|
||||
|
||||
// Compute J*v
|
||||
const Vector3 Jv = v2 + w2.cross(mBallAndSocketJointComponents.mR2World[i]) - v1 - w1.cross(mBallAndSocketJointComponents.mR1World[i]);
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
const Vector3 deltaLambda = mBallAndSocketJointComponents.mInverseMassMatrix[i] * (-Jv - mBallAndSocketJointComponents.mBiasVector[i]);
|
||||
mBallAndSocketJointComponents.mImpulse[i] += deltaLambda;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the body 1
|
||||
const Vector3 linearImpulseBody1 = -deltaLambda;
|
||||
const Vector3 angularImpulseBody1 = deltaLambda.cross(mBallAndSocketJointComponents.mR1World[i]);
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += mRigidBodyComponents.mInverseMasses[componentIndexBody1] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
|
||||
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 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) {
|
||||
|
|
@ -292,6 +270,30 @@ void SolveBallAndSocketJointSystem::solveVelocityConstraint() {
|
|||
|
||||
}
|
||||
}
|
||||
|
||||
// --------------- Joint Constraints --------------- //
|
||||
|
||||
// Compute J*v
|
||||
const Vector3 Jv = v2 + w2.cross(mBallAndSocketJointComponents.mR2World[i]) - v1 - w1.cross(mBallAndSocketJointComponents.mR1World[i]);
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
const Vector3 deltaLambda = mBallAndSocketJointComponents.mInverseMassMatrix[i] * (-Jv - mBallAndSocketJointComponents.mBiasVector[i]);
|
||||
mBallAndSocketJointComponents.mImpulse[i] += deltaLambda;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the body 1
|
||||
const Vector3 linearImpulseBody1 = -deltaLambda;
|
||||
const Vector3 angularImpulseBody1 = deltaLambda.cross(mBallAndSocketJointComponents.mR1World[i]);
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += mRigidBodyComponents.mInverseMasses[componentIndexBody1] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
|
||||
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 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -342,6 +344,54 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
|
|||
const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) {
|
||||
|
||||
// Check if the cone limit constraints is violated or not
|
||||
const Vector3 coneAxisBody1World = q1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[i];
|
||||
const Vector3 coneAxisBody2World = q2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[i];
|
||||
mBallAndSocketJointComponents.mConeLimitACrossB[i] = coneAxisBody1World.cross(coneAxisBody2World);
|
||||
decimal coneAngle = computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World);
|
||||
decimal coneLimitError = mBallAndSocketJointComponents.mConeLimitHalfAngle[i] - coneAngle;
|
||||
mBallAndSocketJointComponents.mIsConeLimitViolated[i] = coneLimitError < 0;
|
||||
|
||||
// If the cone limit is violated
|
||||
if (mBallAndSocketJointComponents.mIsConeLimitViolated[i]) {
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the cone limit (1x1 matrix)
|
||||
decimal inverseMassMatrixConeLimit = mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI1[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]) +
|
||||
mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI2[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]);
|
||||
mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] = (inverseMassMatrixConeLimit > decimal(0.0)) ?
|
||||
decimal(1.0) / inverseMassMatrixConeLimit : decimal(0.0);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the cone limit constraint
|
||||
decimal lambdaConeLimit = mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] * (-coneLimitError );
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 1
|
||||
const Vector3 angularImpulseBody1 = lambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i];
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
const Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1);
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 2
|
||||
const Vector3 angularImpulseBody2 = -lambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i];
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
const Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mBallAndSocketJointComponents.mI2[i] * angularImpulseBody2);
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
}
|
||||
|
||||
// --------------- Joint Constraints --------------- //
|
||||
|
||||
// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints
|
||||
decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2;
|
||||
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
|
||||
|
|
@ -394,51 +444,5 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
|
|||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) {
|
||||
|
||||
// Check if the cone limit constraints is violated or not
|
||||
const Vector3 coneAxisBody1World = q1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[i];
|
||||
const Vector3 coneAxisBody2World = q2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[i];
|
||||
mBallAndSocketJointComponents.mConeLimitACrossB[i] = coneAxisBody1World.cross(coneAxisBody2World);
|
||||
decimal coneAngle = computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World);
|
||||
decimal coneLimitError = mBallAndSocketJointComponents.mConeLimitHalfAngle[i] - coneAngle;
|
||||
mBallAndSocketJointComponents.mIsConeLimitViolated[i] = coneLimitError < 0;
|
||||
|
||||
// If the cone limit is violated
|
||||
if (mBallAndSocketJointComponents.mIsConeLimitViolated[i]) {
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the cone limit (1x1 matrix)
|
||||
decimal inverseMassMatrixConeLimit = mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI1[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]) +
|
||||
mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI2[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]);
|
||||
mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] = (inverseMassMatrixConeLimit > decimal(0.0)) ?
|
||||
decimal(1.0) / inverseMassMatrixConeLimit : decimal(0.0);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the cone limit constraint
|
||||
decimal lambdaConeLimit = mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] * (-coneLimitError );
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 1
|
||||
const Vector3 angularImpulseBody1 = lambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i];
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
const Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1);
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 2
|
||||
const Vector3 angularImpulseBody2 = -lambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i];
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
const Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mBallAndSocketJointComponents.mI2[i] * angularImpulseBody2);
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -330,57 +330,6 @@ void SolveHingeJointSystem::solveVelocityConstraint() {
|
|||
|
||||
const decimal inverseMassMatrixLimitMotor = mHingeJointComponents.mInverseMassMatrixLimitMotor[i];
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
// Compute J*v
|
||||
const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World);
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
const Vector3 deltaLambdaTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i] *
|
||||
(-JvTranslation - mHingeJointComponents.mBiasTranslation[i]);
|
||||
mHingeJointComponents.mImpulseTranslation[i] += deltaLambdaTranslation;
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 1
|
||||
const Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
|
||||
Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(r1World);
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
|
||||
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 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
const Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i];
|
||||
const Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i];
|
||||
|
||||
// Compute J*v for the 2 rotation constraints
|
||||
const Vector2 JvRotation(-b2CrossA1.dot(w1) + b2CrossA1.dot(w2),
|
||||
-c2CrossA1.dot(w1) + c2CrossA1.dot(w2));
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 2 rotation constraints
|
||||
Vector2 deltaLambdaRotation = mHingeJointComponents.mInverseMassMatrixRotation[i] *
|
||||
(-JvRotation - mHingeJointComponents.mBiasRotation[i]);
|
||||
mHingeJointComponents.mImpulseRotation[i] += deltaLambdaRotation;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
|
||||
angularImpulseBody1 = -b2CrossA1 * deltaLambdaRotation.x - c2CrossA1 * deltaLambdaRotation.y;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
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 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mHingeJointComponents.mIsLimitEnabled[i]) {
|
||||
|
|
@ -463,6 +412,57 @@ void SolveHingeJointSystem::solveVelocityConstraint() {
|
|||
// Apply the impulse to the body 2
|
||||
w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
|
||||
}
|
||||
|
||||
// --------------- Joint Rotation Constraints --------------- //
|
||||
|
||||
const Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i];
|
||||
const Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i];
|
||||
|
||||
// Compute J*v for the 2 rotation constraints
|
||||
const Vector2 JvRotation(-b2CrossA1.dot(w1) + b2CrossA1.dot(w2),
|
||||
-c2CrossA1.dot(w1) + c2CrossA1.dot(w2));
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 2 rotation constraints
|
||||
Vector2 deltaLambdaRotation = mHingeJointComponents.mInverseMassMatrixRotation[i] *
|
||||
(-JvRotation - mHingeJointComponents.mBiasRotation[i]);
|
||||
mHingeJointComponents.mImpulseRotation[i] += deltaLambdaRotation;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
|
||||
Vector3 angularImpulseBody1 = -b2CrossA1 * deltaLambdaRotation.x - c2CrossA1 * deltaLambdaRotation.y;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2
|
||||
Vector3 angularImpulseBody2 = b2CrossA1 * deltaLambdaRotation.x + c2CrossA1 * deltaLambdaRotation.y;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
|
||||
|
||||
// --------------- Joint Translation Constraints --------------- //
|
||||
|
||||
// Compute J*v
|
||||
const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World);
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
const Vector3 deltaLambdaTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i] *
|
||||
(-JvTranslation - mHingeJointComponents.mBiasTranslation[i]);
|
||||
mHingeJointComponents.mImpulseTranslation[i] += deltaLambdaTranslation;
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 1
|
||||
const Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
|
||||
angularImpulseBody1 = deltaLambdaTranslation.cross(r1World);
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
|
||||
w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1);
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 2
|
||||
angularImpulseBody2 = -deltaLambdaTranslation.cross(r2World);
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * deltaLambdaTranslation;
|
||||
w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -504,7 +504,6 @@ void SolveHingeJointSystem::solvePositionConstraint() {
|
|||
Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR1World[i]);
|
||||
Matrix3x3 skewSymmetricMatrixU2 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR2World[i]);
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i];
|
||||
Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i];
|
||||
|
|
@ -524,116 +523,6 @@ void SolveHingeJointSystem::solvePositionConstraint() {
|
|||
c2CrossA1 = c2.cross(a1);
|
||||
mHingeJointComponents.mC2CrossA1[i] = c2CrossA1;
|
||||
|
||||
// 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];
|
||||
decimal inverseMassBodies = body1InverseMass + body2InverseMass;
|
||||
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
|
||||
0, inverseMassBodies, 0,
|
||||
0, 0, inverseMassBodies) +
|
||||
skewSymmetricMatrixU1 * mHingeJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() +
|
||||
skewSymmetricMatrixU2 * mHingeJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
|
||||
mHingeJointComponents.mInverseMassMatrixTranslation[i].setToZero();
|
||||
decimal matrixDeterminant = massMatrix.getDeterminant();
|
||||
if (std::abs(matrixDeterminant) > MACHINE_EPSILON) {
|
||||
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
|
||||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(matrixDeterminant);
|
||||
}
|
||||
|
||||
|
||||
Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
|
||||
Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
|
||||
|
||||
// Compute position error for the 3 translation constraints
|
||||
const Vector3 errorTranslation = x2 + mHingeJointComponents.mR2World[i] - x1 - mHingeJointComponents.mR1World[i];
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
const Vector3 lambdaTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i] * (-errorTranslation);
|
||||
|
||||
// Compute the impulse of body 1
|
||||
Vector3 linearImpulseBody1 = -lambdaTranslation;
|
||||
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mHingeJointComponents.mR1World[i]);
|
||||
|
||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||
decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
|
||||
Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1);
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
x1 += v1;
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse of body 2
|
||||
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mHingeJointComponents.mR2World[i]);
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambdaTranslation;
|
||||
Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2);
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
x2 += v2;
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
|
||||
Vector3 I1B2CrossA1 = mHingeJointComponents.mI1[i] * b2CrossA1;
|
||||
Vector3 I1C2CrossA1 = mHingeJointComponents.mI1[i] * c2CrossA1;
|
||||
Vector3 I2B2CrossA1 = mHingeJointComponents.mI2[i] * b2CrossA1;
|
||||
Vector3 I2C2CrossA1 = mHingeJointComponents.mI2[i] * c2CrossA1;
|
||||
const decimal el11 = b2CrossA1.dot(I1B2CrossA1) +
|
||||
b2CrossA1.dot(I2B2CrossA1);
|
||||
const decimal el12 = b2CrossA1.dot(I1C2CrossA1) +
|
||||
b2CrossA1.dot(I2C2CrossA1);
|
||||
const decimal el21 = c2CrossA1.dot(I1B2CrossA1) +
|
||||
c2CrossA1.dot(I2B2CrossA1);
|
||||
const decimal el22 = c2CrossA1.dot(I1C2CrossA1) +
|
||||
c2CrossA1.dot(I2C2CrossA1);
|
||||
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
|
||||
mHingeJointComponents.mInverseMassMatrixRotation[i].setToZero();
|
||||
matrixDeterminant = matrixKRotation.getDeterminant();
|
||||
if (std::abs(matrixDeterminant) > MACHINE_EPSILON) {
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
|
||||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse(matrixDeterminant);
|
||||
}
|
||||
|
||||
// Compute the position error for the 3 rotation constraints
|
||||
const Vector2 errorRotation = Vector2(a1.dot(b2), a1.dot(c2));
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
|
||||
Vector2 lambdaRotation = mHingeJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
Vector3 angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y;
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
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);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse of body 2
|
||||
Vector3 angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y;
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
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);
|
||||
q2.normalize();
|
||||
|
||||
}
|
||||
|
||||
// Compute the current angle around the hinge axis
|
||||
const decimal hingeAngle = computeCurrentHingeAngle(jointEntity, q1, q2);
|
||||
|
||||
|
|
@ -713,6 +602,117 @@ void SolveHingeJointSystem::solvePositionConstraint() {
|
|||
q2.normalize();
|
||||
}
|
||||
}
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
|
||||
Vector3 I1B2CrossA1 = mHingeJointComponents.mI1[i] * b2CrossA1;
|
||||
Vector3 I1C2CrossA1 = mHingeJointComponents.mI1[i] * c2CrossA1;
|
||||
Vector3 I2B2CrossA1 = mHingeJointComponents.mI2[i] * b2CrossA1;
|
||||
Vector3 I2C2CrossA1 = mHingeJointComponents.mI2[i] * c2CrossA1;
|
||||
const decimal el11 = b2CrossA1.dot(I1B2CrossA1) +
|
||||
b2CrossA1.dot(I2B2CrossA1);
|
||||
const decimal el12 = b2CrossA1.dot(I1C2CrossA1) +
|
||||
b2CrossA1.dot(I2C2CrossA1);
|
||||
const decimal el21 = c2CrossA1.dot(I1B2CrossA1) +
|
||||
c2CrossA1.dot(I2B2CrossA1);
|
||||
const decimal el22 = c2CrossA1.dot(I1C2CrossA1) +
|
||||
c2CrossA1.dot(I2C2CrossA1);
|
||||
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
|
||||
mHingeJointComponents.mInverseMassMatrixRotation[i].setToZero();
|
||||
decimal matrixDeterminant = matrixKRotation.getDeterminant();
|
||||
if (std::abs(matrixDeterminant) > MACHINE_EPSILON) {
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
|
||||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse(matrixDeterminant);
|
||||
}
|
||||
|
||||
// Compute the position error for the 3 rotation constraints
|
||||
const Vector2 errorRotation = Vector2(a1.dot(b2), a1.dot(c2));
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
|
||||
Vector2 lambdaRotation = mHingeJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
Vector3 angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y;
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
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);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse of body 2
|
||||
Vector3 angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y;
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
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);
|
||||
q2.normalize();
|
||||
}
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
// 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];
|
||||
decimal inverseMassBodies = body1InverseMass + body2InverseMass;
|
||||
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
|
||||
0, inverseMassBodies, 0,
|
||||
0, 0, inverseMassBodies) +
|
||||
skewSymmetricMatrixU1 * mHingeJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() +
|
||||
skewSymmetricMatrixU2 * mHingeJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
|
||||
mHingeJointComponents.mInverseMassMatrixTranslation[i].setToZero();
|
||||
matrixDeterminant = massMatrix.getDeterminant();
|
||||
if (std::abs(matrixDeterminant) > MACHINE_EPSILON) {
|
||||
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
|
||||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(matrixDeterminant);
|
||||
}
|
||||
|
||||
|
||||
Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
|
||||
Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
|
||||
|
||||
// Compute position error for the 3 translation constraints
|
||||
const Vector3 errorTranslation = x2 + mHingeJointComponents.mR2World[i] - x1 - mHingeJointComponents.mR1World[i];
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
const Vector3 lambdaTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i] * (-errorTranslation);
|
||||
|
||||
// Compute the impulse of body 1
|
||||
Vector3 linearImpulseBody1 = -lambdaTranslation;
|
||||
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mHingeJointComponents.mR1World[i]);
|
||||
|
||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||
decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
|
||||
Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1);
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
x1 += v1;
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse of body 2
|
||||
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mHingeJointComponents.mR2World[i]);
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambdaTranslation;
|
||||
Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2);
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
x2 += v2;
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -357,58 +357,6 @@ void SolveSliderJointSystem::solveVelocityConstraint() {
|
|||
decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
// Compute J*v for the 2 translation constraints
|
||||
const decimal el1 = -n1.dot(v1) - w1.dot(r1PlusUCrossN1) +
|
||||
n1.dot(v2) + w2.dot(r2CrossN1);
|
||||
const decimal el2 = -n2.dot(v1) - w1.dot(r1PlusUCrossN2) +
|
||||
n2.dot(v2) + w2.dot(r2CrossN2);
|
||||
const Vector2 JvTranslation(el1, el2);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 2 translation constraints
|
||||
const Vector2 deltaLambda = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-JvTranslation - mSliderJointComponents.mBiasTranslation[i]);
|
||||
mSliderJointComponents.mImpulseTranslation[i] += deltaLambda;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
|
||||
const Vector3 linearImpulseBody1 = -n1 * deltaLambda.x - n2 * deltaLambda.y;
|
||||
Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * deltaLambda.x -
|
||||
r1PlusUCrossN2 * deltaLambda.y;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
|
||||
w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
|
||||
const Vector3 linearImpulseBody2 = -linearImpulseBody1;
|
||||
Vector3 angularImpulseBody2 = r2CrossN1 * deltaLambda.x + r2CrossN2 * deltaLambda.y;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2;
|
||||
w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
// Compute J*v for the 3 rotation constraints
|
||||
const Vector3 JvRotation = w2 - w1;
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
|
||||
Vector3 deltaLambda2 = mSliderJointComponents.mInverseMassMatrixRotation[i] *
|
||||
(-JvRotation - mSliderJointComponents.getBiasRotation(jointEntity));
|
||||
mSliderJointComponents.mImpulseRotation[i] += deltaLambda2;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
angularImpulseBody1 = -deltaLambda2;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
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 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2);
|
||||
|
||||
const Vector3& r2CrossSliderAxis = mSliderJointComponents.mR2CrossSliderAxis[i];
|
||||
const Vector3& r1PlusUCrossSliderAxis = mSliderJointComponents.mR1PlusUCrossSliderAxis[i];
|
||||
|
||||
|
|
@ -510,6 +458,58 @@ void SolveSliderJointSystem::solveVelocityConstraint() {
|
|||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2;
|
||||
}
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
// Compute J*v for the 3 rotation constraints
|
||||
const Vector3 JvRotation = w2 - w1;
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
|
||||
Vector3 deltaLambda2 = mSliderJointComponents.mInverseMassMatrixRotation[i] *
|
||||
(-JvRotation - mSliderJointComponents.getBiasRotation(jointEntity));
|
||||
mSliderJointComponents.mImpulseRotation[i] += deltaLambda2;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
Vector3 angularImpulseBody1 = -deltaLambda2;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
|
||||
Vector3 angularImpulseBody2 = deltaLambda2;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2);
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
// Compute J*v for the 2 translation constraints
|
||||
const decimal el1 = -n1.dot(v1) - w1.dot(r1PlusUCrossN1) +
|
||||
n1.dot(v2) + w2.dot(r2CrossN1);
|
||||
const decimal el2 = -n2.dot(v1) - w1.dot(r1PlusUCrossN2) +
|
||||
n2.dot(v2) + w2.dot(r2CrossN2);
|
||||
const Vector2 JvTranslation(el1, el2);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 2 translation constraints
|
||||
const Vector2 deltaLambda = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-JvTranslation - mSliderJointComponents.mBiasTranslation[i]);
|
||||
mSliderJointComponents.mImpulseTranslation[i] += deltaLambda;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
|
||||
const Vector3 linearImpulseBody1 = -n1 * deltaLambda.x - n2 * deltaLambda.y;
|
||||
angularImpulseBody1 = -r1PlusUCrossN1 * deltaLambda.x -
|
||||
r1PlusUCrossN2 * deltaLambda.y;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
|
||||
w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
|
||||
const Vector3 linearImpulseBody2 = -linearImpulseBody1;
|
||||
angularImpulseBody2 = r2CrossN1 * deltaLambda.x + r2CrossN2 * deltaLambda.y;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2;
|
||||
w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -591,137 +591,6 @@ void SolveSliderJointSystem::solvePositionConstraint() {
|
|||
const Vector3& r1PlusUCrossN1 = mSliderJointComponents.mR1PlusUCrossN1[i];
|
||||
const Vector3& r1PlusUCrossN2 = mSliderJointComponents.mR1PlusUCrossN2[i];
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
const Matrix3x3& i1 = mSliderJointComponents.getI1(jointEntity);
|
||||
const Matrix3x3& i2 = mSliderJointComponents.getI2(jointEntity);
|
||||
|
||||
// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
|
||||
// constraints (2x2 matrix)
|
||||
const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
decimal sumInverseMass = body1MassInverse + body2MassInverse;
|
||||
Vector3 I1R1PlusUCrossN1 = i1 * r1PlusUCrossN1;
|
||||
Vector3 I1R1PlusUCrossN2 = i1 * r1PlusUCrossN2;
|
||||
Vector3 I2R2CrossN1 = i2 * r2CrossN1;
|
||||
Vector3 I2R2CrossN2 = i2 * r2CrossN2;
|
||||
const decimal el11 = sumInverseMass + r1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
|
||||
r2CrossN1.dot(I2R2CrossN1);
|
||||
const decimal el12 = r1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
|
||||
r2CrossN1.dot(I2R2CrossN2);
|
||||
const decimal el21 = r1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
|
||||
r2CrossN2.dot(I2R2CrossN1);
|
||||
const decimal el22 = sumInverseMass + r1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
|
||||
r2CrossN2.dot(I2R2CrossN2);
|
||||
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
|
||||
mSliderJointComponents.mInverseMassMatrixTranslation[i].setToZero();
|
||||
decimal matrixKTranslationDeterminant = matrixKTranslation.getDeterminant();
|
||||
if (std::abs(matrixKTranslationDeterminant) > MACHINE_EPSILON) {
|
||||
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
|
||||
mSliderJointComponents.mInverseMassMatrixTranslation[i] = matrixKTranslation.getInverse(matrixKTranslationDeterminant);
|
||||
}
|
||||
|
||||
// Compute the position error for the 2 translation constraints
|
||||
const Vector2 translationError(u.dot(n1), u.dot(n2));
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 2 translation constraints
|
||||
Vector2 lambdaTranslation = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-translationError);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
|
||||
const Vector3 linearImpulseBody1 = -n1 * lambdaTranslation.x - n2 * lambdaTranslation.y;
|
||||
Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * lambdaTranslation.x -
|
||||
r1PlusUCrossN2 * lambdaTranslation.y;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
|
||||
Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1);
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
x1 += v1;
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
|
||||
const Vector3 linearImpulseBody2 = n1 * lambdaTranslation.x + n2 * lambdaTranslation.y;
|
||||
Vector3 angularImpulseBody2 = r2CrossN1 * lambdaTranslation.x + r2CrossN2 * lambdaTranslation.y;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2;
|
||||
Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
x2 += v2;
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
|
||||
// contraints (3x3 matrix)
|
||||
mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mI1[i] + mSliderJointComponents.mI2[i];
|
||||
decimal massMatrixRotationDeterminant = mSliderJointComponents.mInverseMassMatrixRotation[i].getDeterminant();
|
||||
if (std::abs(massMatrixRotationDeterminant) > MACHINE_EPSILON) {
|
||||
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
|
||||
mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mInverseMassMatrixRotation[i].getInverse(massMatrixRotationDeterminant);
|
||||
}
|
||||
|
||||
// Calculate difference in rotation
|
||||
//
|
||||
// The rotation should be:
|
||||
//
|
||||
// q2 = q1 r0
|
||||
//
|
||||
// But because of drift the actual rotation is:
|
||||
//
|
||||
// q2 = qError q1 r0
|
||||
// <=> qError = q2 r0^-1 q1^-1
|
||||
//
|
||||
// Where:
|
||||
// q1 = current rotation of body 1
|
||||
// q2 = current rotation of body 2
|
||||
// qError = error that needs to be reduced to zero
|
||||
Quaternion qError = q2 * mSliderJointComponents.mInitOrientationDifferenceInv[i] * q1.getInverse();
|
||||
|
||||
// A quaternion can be seen as:
|
||||
//
|
||||
// q = [sin(theta / 2) * v, cos(theta/2)]
|
||||
//
|
||||
// Where:
|
||||
// v = rotation vector
|
||||
// theta = rotation angle
|
||||
//
|
||||
// If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
|
||||
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
|
||||
Vector3 lambdaRotation = mSliderJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
Vector3 angularImpulseBody1 = -lambdaRotation;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1);
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
|
||||
Vector3 angularImpulseBody2 = lambdaRotation;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2);
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mSliderJointComponents.mIsLimitEnabled[i]) {
|
||||
|
|
@ -825,5 +694,136 @@ void SolveSliderJointSystem::solvePositionConstraint() {
|
|||
q2.normalize();
|
||||
}
|
||||
}
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
|
||||
// contraints (3x3 matrix)
|
||||
mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mI1[i] + mSliderJointComponents.mI2[i];
|
||||
decimal massMatrixRotationDeterminant = mSliderJointComponents.mInverseMassMatrixRotation[i].getDeterminant();
|
||||
if (std::abs(massMatrixRotationDeterminant) > MACHINE_EPSILON) {
|
||||
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
|
||||
mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mInverseMassMatrixRotation[i].getInverse(massMatrixRotationDeterminant);
|
||||
}
|
||||
|
||||
// Calculate difference in rotation
|
||||
//
|
||||
// The rotation should be:
|
||||
//
|
||||
// q2 = q1 r0
|
||||
//
|
||||
// But because of drift the actual rotation is:
|
||||
//
|
||||
// q2 = qError q1 r0
|
||||
// <=> qError = q2 r0^-1 q1^-1
|
||||
//
|
||||
// Where:
|
||||
// q1 = current rotation of body 1
|
||||
// q2 = current rotation of body 2
|
||||
// qError = error that needs to be reduced to zero
|
||||
Quaternion qError = q2 * mSliderJointComponents.mInitOrientationDifferenceInv[i] * q1.getInverse();
|
||||
|
||||
// A quaternion can be seen as:
|
||||
//
|
||||
// q = [sin(theta / 2) * v, cos(theta/2)]
|
||||
//
|
||||
// Where:
|
||||
// v = rotation vector
|
||||
// theta = rotation angle
|
||||
//
|
||||
// If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
|
||||
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
|
||||
Vector3 lambdaRotation = mSliderJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
Vector3 angularImpulseBody1 = -lambdaRotation;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1);
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
|
||||
Vector3 angularImpulseBody2 = lambdaRotation;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2);
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
const Matrix3x3& i1 = mSliderJointComponents.getI1(jointEntity);
|
||||
const Matrix3x3& i2 = mSliderJointComponents.getI2(jointEntity);
|
||||
|
||||
// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
|
||||
// constraints (2x2 matrix)
|
||||
const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
decimal sumInverseMass = body1MassInverse + body2MassInverse;
|
||||
Vector3 I1R1PlusUCrossN1 = i1 * r1PlusUCrossN1;
|
||||
Vector3 I1R1PlusUCrossN2 = i1 * r1PlusUCrossN2;
|
||||
Vector3 I2R2CrossN1 = i2 * r2CrossN1;
|
||||
Vector3 I2R2CrossN2 = i2 * r2CrossN2;
|
||||
const decimal el11 = sumInverseMass + r1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
|
||||
r2CrossN1.dot(I2R2CrossN1);
|
||||
const decimal el12 = r1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
|
||||
r2CrossN1.dot(I2R2CrossN2);
|
||||
const decimal el21 = r1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
|
||||
r2CrossN2.dot(I2R2CrossN1);
|
||||
const decimal el22 = sumInverseMass + r1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
|
||||
r2CrossN2.dot(I2R2CrossN2);
|
||||
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
|
||||
mSliderJointComponents.mInverseMassMatrixTranslation[i].setToZero();
|
||||
decimal matrixKTranslationDeterminant = matrixKTranslation.getDeterminant();
|
||||
if (std::abs(matrixKTranslationDeterminant) > MACHINE_EPSILON) {
|
||||
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
|
||||
mSliderJointComponents.mInverseMassMatrixTranslation[i] = matrixKTranslation.getInverse(matrixKTranslationDeterminant);
|
||||
}
|
||||
|
||||
// Compute the position error for the 2 translation constraints
|
||||
const Vector2 translationError(u.dot(n1), u.dot(n2));
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 2 translation constraints
|
||||
Vector2 lambdaTranslation = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-translationError);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
|
||||
const Vector3 linearImpulseBody1 = -n1 * lambdaTranslation.x - n2 * lambdaTranslation.y;
|
||||
Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * lambdaTranslation.x -
|
||||
r1PlusUCrossN2 * lambdaTranslation.y;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
|
||||
Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1);
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
x1 += v1;
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
|
||||
const Vector3 linearImpulseBody2 = n1 * lambdaTranslation.x + n2 * lambdaTranslation.y;
|
||||
Vector3 angularImpulseBody2 = r2CrossN1 * lambdaTranslation.x + r2CrossN2 * lambdaTranslation.y;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2;
|
||||
Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
x2 += v2;
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user