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.unifreiburg.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 semiimplicit 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.unifreiburg.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)
[SOLVED]Quaterion to Matrix and back conversion issues
[SOLVED]Quaterion to Matrix and back conversion issues
 Attachments

 quats
 quats.jpg (117.34 KiB) Viewed 4593 times

 angke
 Untitled.jpg (97.6 KiB) Viewed 4593 times

 Quats.zip
 excel
 (1.75 MiB) Downloaded 117 times
Re: [SOLVED]Quaterion to Matrix and back conversion issues
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...
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
 Untitled.jpg (180.82 KiB) Viewed 4585 times
 Erwin Coumans
 Site Admin
 Posts: 4189
 Joined: Sun Jun 26, 2005 6:43 pm
 Location: California, USA
 Contact:
Re: [SOLVED]Quaterion to Matrix and back conversion issues
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.unifreiburg.de/pe ... /corde.htm
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.unifreiburg.de/pe ... /corde.htm
Re: [SOLVED]Quaterion to Matrix and back conversion issues
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:
The link is working fine, however the links on authors website don't work for some reason. Anyway I've attached all three papers.
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();
 Attachments

 spillmann_eg08_adaptiverods.rar
 Contact
 (564.59 KiB) Downloaded 133 times

 COSSERAT.rar
 Cosserat
 (1.95 MiB) Downloaded 134 times
Re: [SOLVED]Quaterion to Matrix and back conversion issues
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).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...
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;
}
}
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;
}
}
PS. This source code is derived from the Ogre library, so it inherits its MIT license.
PS2. The topic looks very interesting.
Re: [SOLVED]Quaterion to Matrix and back conversion issues
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
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