Merge branch 'optimization' into sat

.travis.yml

@@ -1,20 +1,42 @@
 language: cpp
-os:
+matrix:
-  - linux
+
-  - osx
+  # Linux / GCC
-compiler:
+  include:
-  - gcc
+    - os: linux
-  - clang
+      addons:
-install:
+        apt:
-- if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi
+          sources:
-addons:
+            - ubuntu-toolchain-r-test
-  apt:
+          packages:
-    sources:
+            - g++-4.9
-    - ubuntu-toolchain-r-test
+      env:
-    packages:
+        - MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9"
-    - gcc-4.8
+
-    - g++-4.8
+    # OS X / GCC
-    - clang
+    - 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

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
+    and orientation are updated accordingly. \\
-    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. \\
 
-    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
+     // Compute the interpolation factor ("accumulator" is the time left in the accumulator and
-rp3d::Transform transform = body->getInterpolatedTransform();
+     // "dt" is the physics time step)
-  \end{lstlisting}
+     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
+      // Get the OpenGL matrix array of the transform
-float matrix[16];
+      float matrix[16];
-transform.getOpenGLMatrix(matrix);
+      transform.getOpenGLMatrix(matrix);
   \end{lstlisting}
 
     \subsection{Destroying a Rigid Body}

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
+        if (mIsInertiaTensorSetByUser) {
-        updateInertiaTensorInverseWorld();
+            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();
+    if (!mIsInertiaTensorSetByUser) mInertiaTensorLocalInverse.setToZero();
-    mInertiaTensorLocalInverse.setToZero();
+    if (!mIsInertiaTensorSetByUser) mInertiaTensorInverseWorld.setToZero();
-    mInertiaTensorInverseWorld.setToZero();
+    if (!mIsCenterOfMassSetByUser) mCenterOfMassLocal.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;
@@ -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
+    if (!mIsCenterOfMassSetByUser) {
-    for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) {
+        mCenterOfMassLocal *= mMassInverse;
-
-        // 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;
     }
 
-    // Compute the local inverse inertia tensor
+    mCenterOfMassWorld = mTransform * mCenterOfMassLocal;
-    mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
+
+    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();

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
+        /// Inverse Local inertia tensor of the body (in local-space) set
-        /// center of mass of the body
+        /// by the user with respect to the center of mass of the body
-        Matrix3x3 mInertiaTensorLocal;
+        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)
+// Get the inverse local inertia tensor of the body (in body coordinates)
-/**
+inline const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
- * @return The 3x3 inertia tensor matrix of the body (in local-space coordinates)
+    return mInertiaTensorLocalInverse;
- */
-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();
 }
 
 // Return the inverse of the inertia tensor in world coordinates.

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
+	// Store inverse of initial rotation from body 1 to body 2 in body 1 space:
-    mInitOrientationDifferenceInv = transform2.getOrientation() *
+	//
-                                    transform1.getOrientation().getInverse();
+	// q20 = q10 r0 
-    mInitOrientationDifferenceInv.normalize();
+	// <=> r0 = q10^-1 q20 
-    mInitOrientationDifferenceInv.inverse();
+	// <=> 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();
+        const Quaternion qError = orientationBody2 * mInitOrientationDifferenceInv * orientationBody1.getInverse();
-        currentOrientationDifference.normalize();
-        const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
         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
+	// Calculate difference in rotation
-    Quaternion currentOrientationDifference = q2 * q1.getInverse();
+	//
-    currentOrientationDifference.normalize();
+	// The rotation should be:
-    const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
+	//
+	// 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

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
+	// Store inverse of initial rotation from body 1 to body 2 in body 1 space:
-    mInitOrientationDifferenceInv = transform2.getOrientation() *
+	//
-                                 transform1.getOrientation().getInverse();
+	// q20 = q10 r0 
-    mInitOrientationDifferenceInv.normalize();
+	// <=> r0 = q10^-1 q20 
-    mInitOrientationDifferenceInv.inverse();
+	// <=> 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();
+        const Quaternion qError = orientationBody2 * mInitOrientationDifferenceInv * orientationBody1.getInverse();
-        currentOrientationDifference.normalize();
-        const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
         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
+	// Calculate difference in rotation
-    Quaternion currentOrientationDifference = q2 * q1.getInverse();
+	//
-    currentOrientationDifference.normalize();
+	// The rotation should be:
-    const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
+	//
+	// 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

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();