Merge branch 'fix_joint_bug'

Fix bug in FixedJoint and SliderJoint

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();
 }
 
 // Destructor
@@ -110,9 +117,7 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
     // Compute the bias "b" for the 3 rotation constraints
     mBiasRotation.setToZero();
     if (mPositionCorrectionTechnique == 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();
     }
 
@@ -300,10 +305,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() *
@@ -162,9 +169,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
     // Compute the bias "b" of the rotation constraint
     mBRotation.setToZero();
     if (mPositionCorrectionTechnique == 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();
     }
 
@@ -544,10 +549,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