// 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);
}
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
tensor.setToZero();
}
