Merge joint bug fix into develop

.travis.yml

@@ -1,20 +1,42 @@
 language: cpp
-os:
+matrix:
-  - linux
+
-  - osx
+  # Linux / GCC
-compiler:
+  include:
-  - gcc
+    - os: linux
-  - clang
+      addons:
-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.9
-    - g++-4.8
+      env:
-    - clang
+        - 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

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