Re: [Algorithms] Quaternions in IK

[Jeff L. <je...@di...>]

The process is really Quat->Matrix->Euler.  Since Quat->Matrix is a pretty easy operation, you can use it as basically Quat->Euler.

I am sure others here have more robust or flexible routines like this but this is what I use.
Since the Matrix->Euler solution has a dependency on order of rotations, a general solution would handle any order.  I typically only use either XYZ or YXZ (HPB) so I have just two versions.  This returns the XYZ decomposition of a Quaternion (x,y,z,w)

You can run this into a EulerToQuat routine to check the results and as long as the EulerToQuat is XYZ order, it should be equal.  The HPB version can be calculated from this easily but if you need it, you can email me.

void QuatToEuler(const tQuaternion *quat, tVector *euler)
{
/// Local Variables ///////////////////////////////////////////////////////////
         float matrix[3][3];
         float cx,sx,x;
         float cy,sy,y,yr;
         float cz,sz,z;
///////////////////////////////////////////////////////////////////////////////
         // CONVERT QUATERNION TO MATRIX - I DON'T REALLY NEED ALL OF IT
         matrix[0][0] = 1.0f - (2.0f * quat->y * quat->y) - (2.0f * quat->z * quat->z);
//      matrix[0][1] = (2.0f * quat->x * quat->y) - (2.0f * quat->w * quat->z);
//      matrix[0][2] = (2.0f * quat->x * quat->z) + (2.0f * quat->w * quat->y);
         matrix[1][0] = (2.0f * quat->x * quat->y) + (2.0f * quat->w * quat->z);
//      matrix[1][1] = 1.0f - (2.0f * quat->x * quat->x) - (2.0f * quat->z * quat->z);
//      matrix[1][2] = (2.0f * quat->y * quat->z) - (2.0f * quat->w * quat->x);
         matrix[2][0] = (2.0f * quat->x * quat->z) - (2.0f * quat->w * quat->y);
         matrix[2][1] = (2.0f * quat->y * quat->z) + (2.0f * quat->w * quat->x);
         matrix[2][2] = 1.0f - (2.0f * quat->x * quat->x) - (2.0f * quat->y * quat->y);

         sy = -matrix[2][0];
         cy = sqrt(1 - (sy * sy));
         yr = (float)atan2(sy,cy);
         euler->y = (yr * 180.0f) / (float)M_PI;

         // AVOID DIVIDE BY ZERO ERROR ONLY WHERE Y= +-90 or +-270 
         // NOT CHECKING cy BECAUSE OF PRECISION ERRORS
         if (sy != 1.0f && sy != -1.0f)  
         {
                 cx = matrix[2][2] / cy;
                 sx = matrix[2][1] / cy;
                 euler->x = ((float)atan2(sx,cx) * 180.0f) / (float)M_PI;        // RAD TO DEG

                 cz = matrix[0][0] / cy;
                 sz = matrix[1][0] / cy;
                 euler->z = ((float)atan2(sz,cz) * 180.0f) / (float)M_PI;        // RAD TO DEG
         }
         else
         {
                 // SINCE Cos(Y) IS 0, I AM SCREWED.  ADOPT THE STANDARD Z = 0
                 // NEED SOME MORE OF THE MATRIX TERMS NOW
                 matrix[1][1] = 1.0f - (2.0f * quat->x * quat->x) - (2.0f * quat->z * quat->z);
                 matrix[1][2] = (2.0f * quat->y * quat->z) - (2.0f * quat->w * quat->x);
                 cx = matrix[1][1];
                 sx = -matrix[1][2];
                 euler->x = ((float)atan2(sx,cx) * 180.0f) / (float)M_PI;        // RAD TO DEG

                 cz = 1.0f;
                 sz = 0.0f;
                 euler->z = ((float)atan2(sz,cz) * 180.0f) / (float)M_PI;        // RAD TO DEG
         }
}
// QuatToEuler  ///////////////////////////////////////////////////////////////


At 10:45 PM 9/5/2000 -0400, you wrote:
>Can you convert between Quaternion and Euler without any loss of
>information?  I have a routine to convert Euler->Quat but I've never seen
>one that goes from Quat->Euler.  This would be most helpful for my current
>project.
>
>- Jason Zisk
>- nFusion Interactive LLC