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

Refactor the getter/setter for inertia tensor of a RigidBody

Browse Source
This commit is contained in:
Daniel Chappuis 2018-01-15 07:18:00 +01:00
parent 27a451adcf
commit c7f7a169f8
3 changed files with 47 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/body/RigidBody.cpp
View File

@ -91,14 +91,13 @@ void RigidBody::setType(BodyType type) {
// Reset the inverse mass and inverse inertia tensor to zero
mMassInverse = decimal(0.0);
mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero();
mInertiaTensorInverseWorld.setToZero();
}
else { // If it is a dynamic body
mMassInverse = decimal(1.0) / mInitMass;
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
@ -126,12 +125,26 @@ void RigidBody::setType(BodyType type) {
*/
void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
mUserInertiaTensorLocalInverse = inertiaTensorLocal.getInverse();
if (mType != BodyType::DYNAMIC) return;
mInertiaTensorLocal = inertiaTensorLocal;
// Compute the inverse local inertia tensor
mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
}
// Set the inverse local inertia tensor of the body (in body coordinates)
void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTensorLocal) {
mUserInertiaTensorLocalInverse = inverseInertiaTensorLocal;
if (mType != BodyType::DYNAMIC) return;
// Compute the inverse local inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
@ -346,12 +359,13 @@ void RigidBody::recomputeMassInformation() {
mInitMass = decimal(0.0);
mMassInverse = decimal(0.0);
mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero();
mInertiaTensorInverseWorld.setToZero();
mCenterOfMassLocal.setToZero();
Matrix3x3 inertiaTensorLocal;
inertiaTensorLocal.setToZero();
// If it is STATIC or KINEMATIC body
// If it is a STATIC or a KINEMATIC body
if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) {
mCenterOfMassWorld = mTransform.getPosition();
return;
@ -403,11 +417,11 @@ void RigidBody::recomputeMassInformation() {
offsetMatrix[2] += offset * (-offset.z);
offsetMatrix *= shape->getMass();
mInertiaTensorLocal += inertiaTensor + offsetMatrix;
inertiaTensorLocal += inertiaTensor + offsetMatrix;
}
// Compute the local inverse inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
mInertiaTensorLocalInverse = inertiaTensorLocal.getInverse();
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();

49
src/body/RigidBody.h
View File

@ -81,9 +81,9 @@ class RigidBody : public CollisionBody {
/// Current external torque on the body
Vector3 mExternalTorque;
/// Local inertia tensor of the body (in local-space) with respect to the
/// center of mass of the body
Matrix3x3 mInertiaTensorLocal;
/// Inverse Local inertia tensor of the body (in local-space) set
/// by the user with respect to the center of mass of the body
Matrix3x3 mUserInertiaTensorLocalInverse;
/// Inverse of the inertia tensor of the body
Matrix3x3 mInertiaTensorLocalInverse;
@ -163,24 +163,24 @@ class RigidBody : public CollisionBody {
/// Set the variable to know whether or not the body is sleeping
virtual void setIsSleeping(bool isSleeping) override;
/// Return the local inertia tensor of the body (in body coordinates)
const Matrix3x3& getInertiaTensorLocal() const;
/// Set the local inertia tensor of the body (in body coordinates)
void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);
/// Set the inverse local inertia tensor of the body (in body coordinates)
void setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTensorLocal);
/// Get the inverse local inertia tensor of the body (in body coordinates)
const Matrix3x3& getInverseInertiaTensorLocal() const;
/// Return the inverse of the inertia tensor in world coordinates.
Matrix3x3 getInertiaTensorInverseWorld() const;
/// Set the local center of mass of the body (in local-space coordinates)
void setCenterOfMassLocal(const Vector3& centerOfMassLocal);
/// Set the mass of the rigid body
void setMass(decimal mass);
/// Return the inertia tensor in world coordinates.
Matrix3x3 getInertiaTensorWorld() const;
/// Return the inverse of the inertia tensor in world coordinates.
Matrix3x3 getInertiaTensorInverseWorld() const;
/// Return true if the gravity needs to be applied to this rigid body
bool isGravityEnabled() const;
@ -273,28 +273,9 @@ inline Vector3 RigidBody::getAngularVelocity() const {
return mAngularVelocity;
}
// Return the local inertia tensor of the body (in local-space coordinates)
/**
* @return The 3x3 inertia tensor matrix of the body (in local-space coordinates)
*/
inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const {
return mInertiaTensorLocal;
}
// Return the inertia tensor in world coordinates.
/// The inertia tensor I_w in world coordinates is computed
/// with the local inertia tensor I_b in body coordinates
/// by I_w = R * I_b * R^T
/// where R is the rotation matrix (and R^T its transpose) of
/// the current orientation quaternion of the body
/**
* @return The 3x3 inertia tensor matrix of the body in world-space coordinates
*/
inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
// Compute and return the inertia tensor in world coordinates
return mTransform.getOrientation().getMatrix() * mInertiaTensorLocal *
mTransform.getOrientation().getMatrix().getTranspose();
// Get the inverse local inertia tensor of the body (in body coordinates)
inline const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
return mInertiaTensorLocalInverse;
}
// Return the inverse of the inertia tensor in world coordinates.

11
src/engine/ContactSolver.cpp
View File

@ -259,8 +259,17 @@ void ContactSolver::initializeForIsland(Island* island) {
bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero();
if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) {
mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2;
mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse();
decimal det = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getDeterminant();
// If the matrix is not inversible
if (approxEqual(det, decimal(0.0))) {
mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero();
}
else {
mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse();
}
}
mContactConstraints[mNbContactManifolds].normal.normalize();