[SOLVED]Quaterion to Matrix and back conversion issues

Post Reply
korzen303
Posts: 30
Joined: Thu Dec 19, 2013 12:13 pm

[SOLVED]Quaterion to Matrix and back conversion issues

Post by korzen303 »

Dear Bullet community,

I am trying to integrate a model for simulating 1D flexible bodies (ropes, wires etc.) into Bullet. The model is based on a Cosserat Theory, specifically on this paper (http://cg.informatik.uni-freiburg.de/pu ... _ropes.pdf). The implementation uses quaternions internally and employs symbolic differentiation software (Maple) to differentiate energy into restitution forces and torques which are later integrated using semi-implicit Euler scheme.

Unfortunately, the model running in Bullet easily explodes in a 100% reproducible manner. After a few days of debugging and eliminating out all the possible differences between Bullet and our reference implementation it turned out that the reason is the conversion from quaternions to matrices and vice versa during position integration (Transform.set/getRotation()). Our reference implementation stores orientations as quaternions and does all the calculations using then so such a conversion is not needed. Bullet stores orientations as matrices which are converted to quaternions for angular velocity integration and then the obtained orientation is converted back to matrix and stored.

I made a quick test using our reference implementation where for a few seconds a rod is spinning in different directions in a free space. During the integration the quaternion is converted to a matrix and immediately back using methods extracted from Bullet (Transform.set/getRotation()). The difference in x,y,z,w componenets is compared against a reference i.e. not converted quaternion which is used in the simulation. I attached the excel file and chart with the results. My model obviously explodes at the first peak. It is clearly visible that some values are compensated by other values but for some oriantations this is flipped. I also used other methods I found on the internet to do the conversion but the results are similar.

I would really appreciate your help guys. I want to extend my model with collisions and constraints but would like to avoid reinventing a wheel. The model itself would also be a nice contribution to Bullet. It is based on a solid theoretical footing and reproduces not only stretch and bend of the rod but also material torsion resulting in looping phenomenom, see here http://cg.informatik.uni-freiburg.de/pe ... /corde.htm.


Thanks in advance

Kay

EDIT: I have added the difference against the angle in degrees by spinning the rod just in one plane (only X and W components change and X angle)
Attachments
quats
quats
quats.jpg (117.34 KiB) Viewed 7944 times
angke
angke
Untitled.jpg (97.6 KiB) Viewed 7944 times
Quats.zip
excel
(1.75 MiB) Downloaded 324 times
korzen303
Posts: 30
Joined: Thu Dec 19, 2013 12:13 pm

Re: [SOLVED]Quaterion to Matrix and back conversion issues

Post by korzen303 »

OK, I solved it. For the reference I put a short solution here. Basically, when we convert from a matrix (or, I guess, always when we create a quaternion) we can obtain two quaternions representing the same rotation q and -q. However, if for example, we want to do a SLERP using quats it may happen that the interpolation will go along the long path i.e. other way around. To fix this we need to take the dot product of the two quaternions and if it is negative we need to inverse one of the quaternions (negate ALL of its components). Now the interpolation should go via the shortest path. I fixed mine stuff in a similar way. I simply store the quat from the previous iteration and take the dot product with the new one coming from Bullet after conversions to and from a matrix. If the dot product is negative I invert it and do the energy calculations.

I guess some algorithms may by invulnerable for this, mine unfortunately is not and needs quat's components to change in a continuous manner.

May sounds trivial, but it took me a week to trace it and fix with 3 lines of code...
Attachments
a
a
Untitled.jpg (180.82 KiB) Viewed 7936 times
User avatar
Erwin Coumans
Site Admin
Posts: 4221
Joined: Sun Jun 26, 2005 6:43 pm
Location: California, USA
Contact:

Re: [SOLVED]Quaterion to Matrix and back conversion issues

Post by Erwin Coumans »

hi Kay,

Thanks for sharing your throughts, your projects look interesting. If you have some contribution, please let us know.

Did you need to make some change in the Bullet source code, related to the quaterion/matrix conversion issue?
THanks,
Erwin

by the way, the link to the pdf at the top of your page is broken.
http://cg.informatik.uni-freiburg.de/pe ... /corde.htm
korzen303
Posts: 30
Joined: Thu Dec 19, 2013 12:13 pm

Re: [SOLVED]Quaterion to Matrix and back conversion issues

Post by korzen303 »

Thanks Erwin,

yes at some point we aim to release the sources working with Bullet.

I didn't have to change anything in Bullet. A small fix before force calculations solved the issue:

Code: Select all

if(q.dot(newQ) < 0)
{
  newQ.negate();
  q.set(newQ);
}
calculateRodInternalForces();
The link is working fine, however the links on authors website don't work for some reason. Anyway I've attached all three papers.
Attachments
spillmann_eg08_adaptiverods.rar
Contact
(564.59 KiB) Downloaded 332 times
COSSERAT.rar
Cosserat
(1.95 MiB) Downloaded 333 times
Flix
Posts: 456
Joined: Tue Dec 25, 2007 1:06 pm

Re: [SOLVED]Quaterion to Matrix and back conversion issues

Post by Flix »

korzen303 wrote:OK, I solved it. For the reference I put a short solution here. Basically, when we convert from a matrix (or, I guess, always when we create a quaternion) we can obtain two quaternions representing the same rotation q and -q. However, if for example, we want to do a SLERP using quats it may happen that the interpolation will go along the long path i.e. other way around. To fix this we need to take the dot product of the two quaternions and if it is negative we need to inverse one of the quaternions (negate ALL of its components). Now the interpolation should go via the shortest path. I fixed mine stuff in a similar way. I simply store the quat from the previous iteration and take the dot product with the new one coming from Bullet after conversions to and from a matrix. If the dot product is negative I invert it and do the energy calculations.

I guess some algorithms may by invulnerable for this, mine unfortunately is not and needs quat's components to change in a continuous manner.

May sounds trivial, but it took me a week to trace it and fix with 3 lines of code...
This is a issue that is common to many math libraries (including the famous 'glm'): the slerp function does not seem to work. Luckily for me, I usually keep an eye on OgreMath as a reference (it's a very solid math library IMO).
Here is something I've just found inside one of my projects:

Code: Select all

// Based on the Ogre source code
SIMD_FORCE_INLINE btScalar btAbs(btScalar v) {return (v<0?-v:v);}
btQuaternion Slerp(btScalar fT,const btQuaternion& rkP,const btQuaternion& rkQ,bool shortestPath=true)	{
	btScalar fCos = rkP.dot(rkQ);	
    btQuaternion rkT;
	
	// Do we need to invert rotation?
    if (fCos < 0.0f && shortestPath)	{
    	fCos = -fCos;
        rkT = rkQ.operator-(); //-rkQ;
    }
    else rkT = rkQ;

    if (btAbs(fCos) < 1.f - SIMD_EPSILON)	{
    		// Standard case (slerp)
            btScalar fSin = btSqrt(1.f - fCos*fCos);
            btScalar fAngle = btAtan2(fSin, fCos);
            btScalar fInvSin = 1.0f / fSin;
            btScalar fCoeff0 = btSin((1.f - fT) * fAngle) * fInvSin;
            btScalar fCoeff1 = btSin(fT * fAngle) * fInvSin;
            return  rkP * fCoeff0 + rkT * fCoeff1;
	}
    else	{
            // There are two situations:
            // 1. "rkP" and "rkQ" are very close (fCos ~= +1), so we can do a linear
            //    interpolation safely.
            // 2. "rkP" and "rkQ" are almost inverse of each other (fCos ~= -1), there
            //    are an infinite number of possibilities interpolation. but we haven't
            //    have method to fix this case, so just use linear interpolation here.
            btQuaternion t = rkP * (1.0f - fT) + rkT * fT;
            // taking the complement requires renormalisation
            t.normalize();
            return t;
    }
}
Hope it's not broken.. (and I've set the default value 'shortestPath=true')
However I'm not sure this is the fastest way of doing it. This is another implementation I've made that can be used to make some performance tests. Actually it's for the glm library, but it shouldn't be hard to convert it:

Code: Select all

namespace glm {// Removed the "bool shortestPath" argument (hard coded to true)
template <typename Real> GLM_INLINE void quatSlerp(
		detail::tquat<Real>& qOut,
		Real const & factor,
		detail::tquat<Real> const & qStart, 
		detail::tquat<Real> const & qEnd, 
		bool normalizeQOutAfterLerp = true,		// When using Lerp instead of Slerp qOut should be normalized. However some users prefer setting eps small enough so that they can leave the Lerp as it is.
		Real eps=static_cast < Real > (0.0001),	// In [0 = 100% Slerp,1 = 100% Lerp] Faster but less precise with bigger epsilon (Lerp is used instead of Slerp more often). Users should tune it to achieve a performance boost.
		bool useAcosAndSinInsteadOfAtan2AndSqrt = false		//Another possible minimal Speed vs Precision tweak (I suggest just changing it here and not in the caller code)
		)	
{
	// Real fCos  = qStart.dot(qEnd);	//Avoided for maximum portability and conversion of the code
	Real fCos  = qStart.x * qEnd.x + qStart.y * qEnd.y + qStart.z * qEnd.z + qStart.w * qEnd.w;
	detail::tquat<Real> end;	

	// Do we need to invert rotation?
	if(fCos < static_cast < Real >(0.0))	//Originally it was if(fCos < static_cast < Real >(0.0) && shortestPath)
	{
		fCos = -fCos;
		//end = -qEnd;	//Avoided for maximum portability and conversion of the code
		end.x = -qEnd.x;   
		end.y = -qEnd.y;
		end.z = -qEnd.z;
		end.w = -qEnd.w;
	} 
	else end = qEnd;

	if( fCos < static_cast < Real >(1.0) - eps)	// Originally if was "Ogre::Math::Abs(fCos)" instead of "fCos", but we know fCos>0, because we have hard coded shortestPath=true
	{
		// Standard case (slerp)
        Real fSin,fAngle;
		if (!useAcosAndSinInsteadOfAtan2AndSqrt)	{
			// Ogre::Quaternion uses this branch by default
        	fSin = sqrt(static_cast < Real >(1.0) - fCos*fCos);
        	fAngle = atan2(fSin, fCos);
        }
        else	{
        	// Possible replacement of the two lines above 
        	// (it's hard to tell it they're faster, but my instinct tells me I should trust atan2 better than acos (geometry geeks needed here...)):
        	// But probably sin(...) is faster than (sqrt + 1 subtraction and mult)
			fAngle = acos(fCos); 
			fSin = sin(fAngle);        
        }
        
        const Real fInvSin = static_cast < Real >(1.0) / fSin;
        const Real fCoeff0 = sin((static_cast < Real >(1.0) - factor) * fAngle) * fInvSin;
        const Real fCoeff1 = sin(factor * fAngle) * fInvSin;                  		

		//qOut =  fCoeff0 * qStart + fCoeff1 * end; //Avoided for maximum portability and conversion of the code
		qOut.x = fCoeff0 * qStart.x + fCoeff1 * end.x;
		qOut.y = fCoeff0 * qStart.y + fCoeff1 * end.y;
		qOut.z = fCoeff0 * qStart.z + fCoeff1 * end.z;
		qOut.w = fCoeff0 * qStart.w + fCoeff1 * end.w;      
	} else
	{
		// There are two situations:
        // 1. "qStart" and "qEnd" are very close (fCos ~= +1), so we can do a linear
        //    interpolation safely.
        // 2. "qStart" and "qEnd" are almost inverse of each other (fCos ~= -1), there
        //    are an infinite number of possibilities interpolation. but we haven't
        //    have method to fix this case, so just use linear interpolation here.
        // IMPORTANT: CASE 2 can't happen anymore because we have hardcoded "shortestPath = true" and now fCos > 0
                   
		const Real fCoeff0 = static_cast < Real >(1.0) - factor;
		const Real fCoeff1 = factor;
		
		//qOut =  fCoeff0 * qStart + fCoeff1 * end; //Avoided for maximum portability and conversion of the code
		qOut.x = fCoeff0 * qStart.x + fCoeff1 * end.x;
		qOut.y = fCoeff0 * qStart.y + fCoeff1 * end.y;
		qOut.z = fCoeff0 * qStart.z + fCoeff1 * end.z;
		qOut.w = fCoeff0 * qStart.w + fCoeff1 * end.w;    
		if (normalizeQOutAfterLerp)  normalizeInPlace(qOut);
	}


}

template <typename Real> GLM_INLINE detail::tquat<Real> quatSlerp(
		Real const & factor,
		detail::tquat<Real> const & qStart, 
		detail::tquat<Real> const & qEnd,
		bool normalizeQOutAfterLerp = true,		// When using Lerp instead of Slerp qOut should be normalized. However some users prefer setting eps small enough so that they can leave the Lerp as it is.
		Real eps=static_cast < Real > (0.0001),	// In [0 = 100% Slerp,1 = 100% Lerp] Faster but less precise with bigger epsilon (Lerp is used instead of Slerp more often). Users should tune it to achieve a performance boost.
		bool useAcosAndSinInsteadOfAtan2AndSqrt = false		//Another possible minimal Speed vs Precision tweak (I suggest just changing it here and not in the caller code)		
		)	{
	detail::tquat<Real>& qOut;
	quatSlerp(qOut,qStart,qEnd,factor,normalizeQOutAfterLerp,eps,useAcosAndSinInsteadOfAtan2AndSqrt);
	return qOut;		
}

template <typename Real> GLM_INLINE void quatLerp(
		detail::tquat<Real>& qOut,
		Real const & factor,
		detail::tquat<Real> const & qStart, 
		detail::tquat<Real> const & qEnd,
		bool normalizeQOutAfterLerp = true
		)	{
		
		const Real fCoeff0 = static_cast < Real >(1.0) - factor;
		const Real fCoeff1 = factor;
		
		//qOut =  fCoeff0 * qStart + fCoeff1 * end; //Avoided for maximum portability and conversion of the code
		qOut.x = fCoeff0 * qStart.x + fCoeff1 * qEnd.x;
		qOut.y = fCoeff0 * qStart.y + fCoeff1 * qEnd.y;
		qOut.z = fCoeff0 * qStart.z + fCoeff1 * qEnd.z;
		qOut.w = fCoeff0 * qStart.w + fCoeff1 * qEnd.w;    
		if (normalizeQOutAfterLerp)  normalizeInPlace(qOut);
}

template <typename Real> GLM_INLINE detail::tquat<Real> quatLerp(
		Real const & factor,
		detail::tquat<Real> const & qStart, 
		detail::tquat<Real> const & qEnd,
		bool normalizeQOutAfterLerp = true)	{
	detail::tquat<Real>& qOut;
	quatLerp(qOut,qStart,qEnd,factor,normalizeQOutAfterLerp);
	return qOut;			
}
}
Hope this can help other people from losing a week :wink:
PS. This source code is derived from the Ogre library, so it inherits its MIT license.
PS2. The topic looks very interesting.
brtietz
Posts: 5
Joined: Tue Jul 17, 2012 12:39 am

Re: [SOLVED]Quaterion to Matrix and back conversion issues

Post by brtietz »

Kay,

I am very interested in implementing this elastic rod model for the open source tensegrity robotics simulator I'm working on (NTRT Project Page, Project on Github). Is your Bullet or reference implementation available at this point? Even if I can pick up where you left off it would save some effort.

Thanks,
Brian
Post Reply