diff --git a/.travis.yml b/.travis.yml index b069cab1..d3445273 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,20 +1,42 @@ language: cpp -os: - - linux - - osx -compiler: - - gcc - - clang -install: -- if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi -addons: - apt: - sources: - - ubuntu-toolchain-r-test - packages: - - gcc-4.8 - - g++-4.8 - - clang +matrix: + + # Linux / GCC + include: + - os: linux + addons: + apt: + sources: + - ubuntu-toolchain-r-test + packages: + - g++-4.9 + env: + - MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9" + + # OS X / GCC + - os: osx + osx_image: xcode8 + env: + - MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9" + + # Linux / Clang + - os: linux + addons: + apt: + sources: + - ubuntu-toolchain-r-test + - llvm-toolchain-precise-3.6 + packages: + - clang-3.6 + env: + - MATRIX_EVAL="CC=clang-3.6 && CXX=clang++-3.6" + + # OS X / Clang + - os: osx + osx_image: xcode8 + +before_install: + - eval "${MATRIX_EVAL}" branches: only: - master diff --git a/documentation/UserManual/ReactPhysics3D-UserManual.tex b/documentation/UserManual/ReactPhysics3D-UserManual.tex index c7e66ccb..e1c20e5d 100644 --- a/documentation/UserManual/ReactPhysics3D-UserManual.tex +++ b/documentation/UserManual/ReactPhysics3D-UserManual.tex @@ -373,6 +373,7 @@ world.enableSleeping(false); \end{sloppypar} \subsection{Updating the Dynamics World} + \label{sec:updating_dynamics_world} The \texttt{DynamicsWorld} is used to simulate physics through time. It has to be updated each time you want to simulate a step forward in time. Most of the time, you want to update the world right before rendering a new frame in a real-time application. \\ @@ -611,19 +612,36 @@ rigidBody->applyTorque(torque); \subsection{Updating a Rigid Body} When you call the \texttt{DynamicsWorld::update()} method, the collisions between the bodies are computed and the joints are evaluated. Then, the bodies position - and orientation - are updated accordingly. After calling this method, you can get the updated position and orientation of each body to render it. To do that, you simply need to use the - \texttt{RigidBody::getInterpolatedTransform()} method to get the interpolated transform. This transform represents the current local-to-world-space transformation - of the body. \\ + and orientation are updated accordingly. \\ - Here is how to get the interpolated transform of a rigid body: \\ + Remember that in section \ref{sec:updating_dynamics_world} we were using a time accumulator in order to always have fixed physics time steps. + Now imagine a situation where the rendering frame rate is higher than the the physics frame rate. It means that at the end of most rendering + frames there will be some time left in the accumulator for the physics time that has not been simulated yet by the physics engine. + It means that we are rendering the state of the physics simulation at a time different from the rendering time which can cause a visual stuttering effect. \\ + + To solve this, the idea is to interpolate between the previous and current physics state of the simulation based on how much time is left in the + accumulator. First we compute the interpolation factor as follows: \\ \begin{lstlisting} -// Here, body is a RigidBody* pointer previously created -// Get the interpolated transform of the rigid body -rp3d::Transform transform = body->getInterpolatedTransform(); - \end{lstlisting} + // Compute the interpolation factor ("accumulator" is the time left in the accumulator and + // "dt" is the physics time step) + const float interpolationFactor = accumulator / dt; + \end{lstlisting} + + \vspace{0.6cm} + + Then we get the current transform of the rigid body and use it with the previous transform (transform at the previous frame) to + compute the interpolated transform as in the following code: \\ + + \begin{lstlisting} + + // Get the current transform of the rigid body + rp3d::Transform currentTransform = body->getTransform(); + + // Interpolate the transform between the previous one and the new one + rp3d::Transform interpolatedTransform = rp3d::Transform::interpolateTransforms(previousTransform, currentTransform, interpolationFactor); + \end{lstlisting} \vspace{0.6cm} @@ -631,9 +649,9 @@ rp3d::Transform transform = body->getInterpolatedTransform(); following code: \\ \begin{lstlisting} -// Get the OpenGL matrix array of the transform -float matrix[16]; -transform.getOpenGLMatrix(matrix); + // Get the OpenGL matrix array of the transform + float matrix[16]; + transform.getOpenGLMatrix(matrix); \end{lstlisting} \subsection{Destroying a Rigid Body} diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 36c362dc..5e5a67c7 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -42,7 +42,7 @@ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyinde : CollisionBody(transform, world, id), mArrayIndex(0), mInitMass(decimal(1.0)), mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), - mJointsList(nullptr) { + mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) { // Compute the inverse mass mMassInverse = decimal(1.0) / mInitMass; @@ -91,19 +91,20 @@ 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(); - // Update the world inverse inertia tensor - updateInertiaTensorInverseWorld(); + if (mIsInertiaTensorSetByUser) { + mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse; + } } + // Update the world inverse inertia tensor + updateInertiaTensorInverseWorld(); + // Awake the body setIsSleeping(false); @@ -120,24 +121,50 @@ void RigidBody::setType(BodyType type) { } // Set the local inertia tensor of the body (in local-space coordinates) +/// If the inertia tensor is set with this method, it will not be computed +/// using the collision shapes of the body. /** * @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; - 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 local-space coordinates) +/// If the inverse inertia tensor is set with this method, it will not be computed +/// using the collision shapes of the body. +/** + * @param inverseInertiaTensorLocal The 3x3 inverse inertia tensor matrix of the body in local-space + * coordinates + */ +void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTensorLocal) { + + mUserInertiaTensorLocalInverse = inverseInertiaTensorLocal; + mIsInertiaTensorSetByUser = true; + + if (mType != BodyType::DYNAMIC) return; // Compute the inverse local inertia tensor - mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); + mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse; // Update the world inverse inertia tensor updateInertiaTensorInverseWorld(); } // Set the local center of mass of the body (in local-space coordinates) +/// If you set the center of mass with the method, it will not be computed +/// automatically using collision shapes. /** * @param centerOfMassLocal The center of mass of the body in local-space * coordinates @@ -146,6 +173,8 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) { if (mType != BodyType::DYNAMIC) return; + mIsCenterOfMassSetByUser = true; + const Vector3 oldCenterOfMass = mCenterOfMassWorld; mCenterOfMassLocal = centerOfMassLocal; @@ -346,12 +375,13 @@ void RigidBody::recomputeMassInformation() { mInitMass = decimal(0.0); mMassInverse = decimal(0.0); - mInertiaTensorLocal.setToZero(); - mInertiaTensorLocalInverse.setToZero(); - mInertiaTensorInverseWorld.setToZero(); - mCenterOfMassLocal.setToZero(); + if (!mIsInertiaTensorSetByUser) mInertiaTensorLocalInverse.setToZero(); + if (!mIsInertiaTensorSetByUser) mInertiaTensorInverseWorld.setToZero(); + if (!mIsCenterOfMassSetByUser) 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; @@ -362,7 +392,10 @@ void RigidBody::recomputeMassInformation() { // Compute the total mass of the body for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) { mInitMass += shape->getMass(); - mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass(); + + if (!mIsCenterOfMassSetByUser) { + mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass(); + } } if (mInitMass > decimal(0.0)) { @@ -375,39 +408,46 @@ void RigidBody::recomputeMassInformation() { // Compute the center of mass const Vector3 oldCenterOfMass = mCenterOfMassWorld; - mCenterOfMassLocal *= mMassInverse; - mCenterOfMassWorld = mTransform * mCenterOfMassLocal; - // Compute the total mass and 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(); - - mInertiaTensorLocal += inertiaTensor + offsetMatrix; + if (!mIsCenterOfMassSetByUser) { + mCenterOfMassLocal *= mMassInverse; } - // Compute the local inverse inertia tensor - mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); + 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(); diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 23603bda..22c784ef 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; @@ -109,6 +109,12 @@ class RigidBody : public CollisionBody { /// First element of the linked list of joints involving this body JointListElement* mJointsList; + /// True if the center of mass is set by the user + bool mIsCenterOfMassSetByUser; + + /// True if the inertia tensor is set by the user + bool mIsInertiaTensorSetByUser; + // -------------------- Methods -------------------- // /// Remove a joint from the joints list @@ -163,24 +169,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 +279,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/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index 01fe3957..17f9e455 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -42,11 +42,18 @@ FixedJoint::FixedJoint(const FixedJointInfo& jointInfo) mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; - // Compute the inverse of the initial orientation difference between the two bodies - mInitOrientationDifferenceInv = transform2.getOrientation() * - transform1.getOrientation().getInverse(); - mInitOrientationDifferenceInv.normalize(); - mInitOrientationDifferenceInv.inverse(); + // Store inverse of initial rotation from body 1 to body 2 in body 1 space: + // + // q20 = q10 r0 + // <=> r0 = q10^-1 q20 + // <=> r0^-1 = q20^-1 q10 + // + // where: + // + // q20 = initial orientation of body 2 + // q10 = initial orientation of body 1 + // r0 = initial rotation rotation from body 1 to body 2 + mInitOrientationDifferenceInv = transform2.getOrientation().getInverse() * transform1.getOrientation(); } // Initialize before solving the constraint @@ -104,10 +111,9 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the bias "b" for the 3 rotation constraints mBiasRotation.setToZero(); + if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { - Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); - currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; + const Quaternion qError = orientationBody2 * mInitOrientationDifferenceInv * orientationBody1.getInverse(); mBiasRotation = biasFactor * decimal(2.0) * qError.getVectorV(); } @@ -295,10 +301,32 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); } - // Compute the position error for the 3 rotation constraints - Quaternion currentOrientationDifference = q2 * q1.getInverse(); - currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; + // 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 * mInitOrientationDifferenceInv * 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 diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 7697013d..715bdc25 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -51,11 +51,18 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; - // Compute the inverse of the initial orientation difference between the two bodies - mInitOrientationDifferenceInv = transform2.getOrientation() * - transform1.getOrientation().getInverse(); - mInitOrientationDifferenceInv.normalize(); - mInitOrientationDifferenceInv.inverse(); + // Store inverse of initial rotation from body 1 to body 2 in body 1 space: + // + // q20 = q10 r0 + // <=> r0 = q10^-1 q20 + // <=> r0^-1 = q20^-1 q10 + // + // where: + // + // q20 = initial orientation of body 2 + // q10 = initial orientation of body 1 + // r0 = initial rotation rotation from body 1 to body 2 + mInitOrientationDifferenceInv = transform2.getOrientation().getInverse() * transform1.getOrientation(); // Compute the slider axis in local-space of body 1 mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() * @@ -157,9 +164,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the bias "b" of the rotation constraint mBRotation.setToZero(); if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { - Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); - currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; + const Quaternion qError = orientationBody2 * mInitOrientationDifferenceInv * orientationBody1.getInverse(); mBRotation = biasFactor * decimal(2.0) * qError.getVectorV(); } @@ -539,10 +544,32 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); } - // Compute the position error for the 3 rotation constraints - Quaternion currentOrientationDifference = q2 * q1.getInverse(); - currentOrientationDifference.normalize(); - const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv; + // 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 * mInitOrientationDifferenceInv * 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 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();