/**
* @param inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space
* coordinates
*/
void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
mUserInertiaTensorLocalInverse = inertiaTensorLocal.getInverse();
mIsInertiaTensorSetByUser = true;
if (mType != BodyType::DYNAMIC) return;
// Compute the inverse local inertia tensor
mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string());
}
// Recompute the center of mass, total mass and inertia tensor of the body using all
// the collision shapes attached to the body.
void RigidBody::recomputeMassInformation() {
mInitMass = decimal(0.0);
mMassInverse = decimal(0.0);
if (!mIsInertiaTensorSetByUser) mInertiaTensorLocalInverse.setToZero();
if (!mIsInertiaTensorSetByUser) mInertiaTensorInverseWorld.setToZero();
if (!mIsCenterOfMassSetByUser) mCenterOfMassLocal.setToZero();
Matrix3x3 inertiaTensorLocal;
inertiaTensorLocal.setToZero();
// If it is a STATIC or a KINEMATIC body
if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) {
mCenterOfMassWorld = mTransform.getPosition();
return;
}
assert(mType == BodyType::DYNAMIC);
// Compute the total mass of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) {
mInitMass += shape->getMass();
if (!mIsCenterOfMassSetByUser) {
mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass();
}
}
if (mInitMass > decimal(0.0)) {
mMassInverse = decimal(1.0) / mInitMass;
}
else {
mCenterOfMassWorld = mTransform.getPosition();
return;
}
// Compute the center of mass
const Vector3 oldCenterOfMass = mCenterOfMassWorld;
if (!mIsCenterOfMassSetByUser) {
mCenterOfMassLocal *= mMassInverse;
}
mCenterOfMassWorld = mTransform * mCenterOfMassLocal;
if (!mIsInertiaTensorSetByUser) {
// Compute the inertia tensor using all the collision shapes
for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) {
// Get the inertia tensor of the collision shape in its local-space
Matrix3x3 inertiaTensor;
shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass());
// Convert the collision shape inertia tensor into the local-space of the body
const Transform& shapeTransform = shape->getLocalToBodyTransform();
Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix();
inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose();
// Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape
// center into a inertia tensor w.r.t to the body origin.
Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal;
decimal offsetSquare = offset.lengthSquare();
Matrix3x3 offsetMatrix;
offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0));
offsetMatrix[1].setAllValues(decimal(0.0), offsetSquare, decimal(0.0));
offsetMatrix[2].setAllValues(decimal(0.0), decimal(0.0), offsetSquare);
offsetMatrix[0] += offset * (-offset.x);
offsetMatrix[1] += offset * (-offset.y);
offsetMatrix[2] += offset * (-offset.z);
offsetMatrix *= shape->getMass();
inertiaTensorLocal += inertiaTensor + offsetMatrix;
}
// Compute the local inverse inertia tensor
mInertiaTensorLocalInverse = inertiaTensorLocal.getInverse();
}
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
// Update the linear velocity of the center of mass
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
}
// Initialize before solving the constraint
void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld;
const Vector3& x2 = mBody2->mCenterOfMassWorld;
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1;
mR2World = orientationBody2 * mLocalAnchorPointBody2;
// Compute the current angle around the hinge axis
decimal hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2);
// Check if the limit constraints are violated or not
decimal lowerLimitError = hingeAngle - mLowerLimit;
decimal upperLimitError = mUpperLimit - hingeAngle;
bool oldIsLowerLimitViolated = mIsLowerLimitViolated;
mIsLowerLimitViolated = lowerLimitError <= 0;
if (mIsLowerLimitViolated != oldIsLowerLimitViolated) {
mImpulseLowerLimit = 0.0;
}
bool oldIsUpperLimitViolated = mIsUpperLimitViolated;
mIsUpperLimitViolated = upperLimitError <= 0;
if (mIsUpperLimitViolated != oldIsUpperLimitViolated) {
mImpulseUpperLimit = 0.0;
}
// Compute vectors needed in the Jacobian
mA1 = orientationBody1 * mHingeLocalAxisBody1;
Vector3 a2 = orientationBody2 * mHingeLocalAxisBody2;
mA1.normalize();
a2.normalize();
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
const Vector3 c2 = a2.cross(b2);
mB2CrossA1 = b2.cross(mA1);
mC2CrossA1 = c2.cross(mA1);
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrixTranslation.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
// Compute the bias "b" of the translation constraints
mBTranslation.setToZero();
decimal biasFactor = (BETA / constraintSolverData.timeStep);
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBTranslation = biasFactor * (x2 + mR2World - x1 - mR1World);
}
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
Vector3 I1B2CrossA1 = mI1 * mB2CrossA1;
Vector3 I1C2CrossA1 = mI1 * mC2CrossA1;
Vector3 I2B2CrossA1 = mI2 * mB2CrossA1;
Vector3 I2C2CrossA1 = mI2 * mC2CrossA1;
const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1);
const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) +
mB2CrossA1.dot(I2C2CrossA1);
const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) +
mC2CrossA1.dot(I2B2CrossA1);
const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) +
mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotation = matrixKRotation.getInverse();
}
// Compute the bias "b" of the rotation constraints
mBRotation.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBRotation = biasFactor * Vector2(mA1.dot(b2), mA1.dot(c2));
}
// If warm-starting is not enabled
if (!constraintSolverData.isWarmStartingActive) {
// Reset all the accumulated impulses
mImpulseTranslation.setToZero();
mImpulseRotation.setToZero();
mImpulseLowerLimit = 0.0;
mImpulseUpperLimit = 0.0;
mImpulseMotor = 0.0;
}
// If the motor or limits are enabled
if (mIsMotorEnabled || (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated))) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1);
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0);
if (mIsLimitEnabled) {
// Compute the bias "b" of the lower limit constraint
mBLowerLimit = 0.0;
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBLowerLimit = biasFactor * lowerLimitError;
}
// Compute the bias "b" of the upper limit constraint
mBUpperLimit = 0.0;
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBUpperLimit = biasFactor * upperLimitError;
}
}
}
}
// Solve the position constraint (for position error correction)
void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1;
mR2World = q2 * mLocalAnchorPointBody2;
// Compute the current angle around the hinge axis
decimal hingeAngle = computeCurrentHingeAngle(q1, q2);
// Check if the limit constraints are violated or not
decimal lowerLimitError = hingeAngle - mLowerLimit;
decimal upperLimitError = mUpperLimit - hingeAngle;
mIsLowerLimitViolated = lowerLimitError <= 0;
mIsUpperLimitViolated = upperLimitError <= 0;
// Compute vectors needed in the Jacobian
mA1 = q1 * mHingeLocalAxisBody1;
Vector3 a2 = q2 * mHingeLocalAxisBody2;
mA1.normalize();
a2.normalize();
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
const Vector3 c2 = a2.cross(b2);
mB2CrossA1 = b2.cross(mA1);
mC2CrossA1 = c2.cross(mA1);
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrixTranslation.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
// Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World;
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation);
// Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World);
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = mI1 * 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(mR2World);
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
Vector3 w2 = mI2 * 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 = mI1 * mB2CrossA1;
Vector3 I1C2CrossA1 = mI1 * mC2CrossA1;
Vector3 I2B2CrossA1 = mI2 * mB2CrossA1;
Vector3 I2C2CrossA1 = mI2 * mC2CrossA1;
const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1);
const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) +
mB2CrossA1.dot(I2C2CrossA1);
const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) +
mC2CrossA1.dot(I2B2CrossA1);
const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) +
mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotation = matrixKRotation.getInverse();
}
// Compute the position error for the 3 rotation constraints
const Vector2 errorRotation = Vector2(mA1.dot(b2), mA1.dot(c2));
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector2 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x - mC2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity of body 1
w1 = mI1 * 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
angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x + mC2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity of body 2
w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
// --------------- Limits Constraints --------------- //
if (mIsLimitEnabled) {
if (mIsLowerLimitViolated || mIsUpperLimitViolated) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1);
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0);
}
// If the lower limit is violated
if (mIsLowerLimitViolated) {
// Compute the Lagrange multiplier lambda for the lower limit constraint
decimal lambdaLowerLimit = mInverseMassMatrixLimitMotor * (-lowerLimitError );
// Compute the impulse P=J^T * lambda of body 1
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1;
// Compute the pseudo velocity of body 1
const Vector3 w1 = mI1 * 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 = lambdaLowerLimit * mA1;
// Compute the pseudo velocity of body 2
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// If the upper limit is violated
if (mIsUpperLimitViolated) {
// Compute the Lagrange multiplier lambda for the upper limit constraint
decimal lambdaUpperLimit = mInverseMassMatrixLimitMotor * (-upperLimitError);
// Compute the impulse P=J^T * lambda of body 1
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1;
// Compute the pseudo velocity of body 1
const Vector3 w1 = mI1 * 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 = -lambdaUpperLimit * mA1;
// Compute the pseudo velocity of body 2
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}
}
