diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 36c362dc..bad11916 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -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(); diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 23603bda..eaadb59c 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -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. diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index f4762c49..a7b86b35 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -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();