PDA

View Full Version : Euler to Quaternion conversion issues



phlake
03-02-2002, 03:48 PM
i'm trying to get some euler angle to quaternion conversion code working and having some difficulty.

the goal is to take a set of euler angles (rotX, rotY, rotZ) and convert to a quaternion (q). then to take (q) and convert to (angle, vector) form for rotation in opengl.

in real code, of course, the conversion from euler to quaternion will happen a single time, so only the conversion from quaternion to angle, vector form needs to be quick.

quickness, however, is not currently the issue. the problem is that my code (see big chunk below) does not produce the quaternion equivelant of euler angles. it produces some other seemingly arbitrary rotation. now: if only one euler angle is used (rotation about one and only one axis), then it seems to be correct.

am i missing something totally obvious? is there something horribly wrong with my code? have i been staring at it too long?

any help much appreciated!




#define DEGREE_TO_RADIAN ((3.1415926535897932384626433832795 * 0.5) / 180.0)
#define RADIAN_TO_DEGREE (180.0 / (3.1415926535897932384626433832795 * 0.5))

// define a simplistic quaternion type
typedef struct t_Quaternion {
float x, y, z, w;
} Quaternion;

// declare five quaternion: one for each Euler angle, one temporary, and the final result.
Quaternion qx, qy, qz, qt, q;

// use these angle for this test
float rotX = 90.0f;
float rotY = 90.0f;
float rotZ = 90.0f;


// calculate each Euler-angle-equivelant quaternion:

// X
qx.w = cos(rotX * 0.5 * DEGREE_TO_RADIAN);
qx.x = sin(rotX * 0.5 * DEGREE_TO_RADIAN);
qx.y = 0;
qx.z = 0;

qy.w = cos(rotY * 0.5 * DEGREE_TO_RADIAN);
qy.x = 0;
qy.y = sin(rotY * 0.5 * DEGREE_TO_RADIAN);
qy.z = 0;

qz.w = cos(rotZ * 0.5 * DEGREE_TO_RADIAN);
qz.x = 0;
qz.y = 0;
qz.z = sin(rotZ * 0.5 * DEGREE_TO_RADIAN);


// find the product of qx * qy * qz

// temporary quaternion is the product of qx and qy.
qt.w = qx.w * qy.w - qx.x * qy.x - qx.y * qy.y - qx.z * qy.z;
qt.x = qx.w * qy.x + qx.x * qy.w + qx.y * qy.z - qx.z * qy.y;
qt.y = qx.w * qy.y + qx.y * qy.w + qx.z * qy.x - qx.x * qy.z;
qt.z = qx.w * qy.z + qx.z * qy.w + qx.x * qy.y - qx.y * qy.x;

// final quaternion is the product of qt and qz
q.w = qt.w * qz.w - qt.x * qz.x - qt.y * qz.y - qt.z * qz.z;
q.x = qt.w * qz.x + qt.x * qz.w + qt.y * qz.z - qt.z * qz.y;
q.y = qt.w * qz.y + qt.y * qz.w + qt.z * qz.x - qt.x * qz.z;
q.z = qt.w * qz.z + qt.z * qz.w + qt.x * qz.y - qt.y * qz.x;


// now, we have the final quaternion: convert it into
// angle and vector form
float cos_angle = q.w;
float angle = acos( cos_angle ) * 2 * RADIAN_TO_DEGREE;
float sin_angle = sqrt( 1.0 - cos_angle * cos_angle );

float sa = sin_angle;

if ( fabs( sin_angle ) < 0.0005 )
sa = 1;

float vx = q.x / sa;
float vy = q.y / sa;
float vz = q.z / sa;

// use the angle-vector form to perform rotation.
glRotatef(angle, vx, vy, vz);

/**
* Mysteriously, the above does not seem to be equivelant to:
*
* glRotatef(rotX, 1.0, 0.0, 0.0);
* glRotatef(rotY, 0.0, 1.0, 0.0);
* glRotatef(rotZ, 0.0, 0.0, 1.0);
*/

heiman
03-03-2002, 03:46 AM
I thought that qx * qy * qz was equivalent to

glRotatef( rotz, 0, 0, 1 );
glRotatef( roty, 0, 1, 0 );
glRotatef( rotx, 1, 0, 0 );

Also, your angular conversions are incorrect. RADIANS_TO_DEGREES = 180 / pi, not 180 / (pi / 2 )

Regards,
Scott

[This message has been edited by heiman (edited 03-03-2002).]

phlake
03-03-2002, 02:24 PM
hrm... yes. converting degrees to radians properly does wonders.

incidentally,

glRotatef(rotZ, 0.0, 0.0, 1.0);
glRotatef(rotY, 0.0, 1.0, 0.0);
glRotatef(rotX, 1.0, 0.0, 0.0);

is what i want in the end, but qx * qy * qz appears to match

glRotatef(rotX, 1.0, 0.0, 0.0);
glRotatef(rotY, 0.0, 1.0, 0.0);
glRotatef(rotZ, 0.0, 0.0, 1.0);

exactly.

muliplying the quaternions in reverse order (qz * qy *qx) changes this.


[This message has been edited by phlake (edited 03-03-2002).]

thelamberto
03-05-2002, 03:17 AM
If your doing a simulation (such as a flight sim) of any kind and wish to update the position of your object from frame to frame then you can use a standard method. If you use some form of dynamics model to calculate the angular rates of motion of your object each frame then the following method will return your new new attitude.

I use ex for the quarternions and Axx for the elements of the direction cosine matrix (DCM). Pdot, Qdot and Rdot refer to the rates of change of the angular velocities, P, Q and R.

Calculate your angular rates, Pdot, Rdot and Qdot

Calculate the rate of change of thew quarternions and integrate over the time step:

Lambda_e := 25.0 * (1.0 - (e0*e0 + e1*e1 + e2*e2 + e3*e3));
e0Dot := -0.5 * (e1*P + e2*Q + e3*R) + Lambda_e*e0;
e1Dot := 0.5 * (e0*P + e2*R - e3*Q) + Lambda_e*e1;
e2Dot := 0.5 * (e0*Q + e3*P - e1*R) + Lambda_e*e2;
e3Dot := 0.5 * (e0*R + e1*Q - e2*P) + Lambda_e*e3;

Integrate(e0, e0Dot);
Integrate(e1, e1Dot);
Integrate(e2, e2Dot);
Integrate(e3, e3Dot);

For the integeration a simple forward euler is usually suficient.

Now set the DCM:

e00 := e0*e0;
e11 := e1*e1;
e22 := e2*e2;
e33 := e3*e3;

A11 := e00 + e11 - e22 - e33;
A12 := 2.0*(e1*e2 - e0*e3);
A13 := 2.0*(e0*e2 + e1*e3);

A21 := 2.0*(e1*e2 + e0*e3);
A22 := e00 - e11 + e22 - e33;
A23 := 2.0*(e2*e3 - e0*e1);

A31 := 2.0*(e1*e3 - e0*e2);
A32 := 2.0*(e2*e3 + e0*e1);
A33 := e00 - e11 - e22 + e33;

Then your attitude is calculated as:

t := -A31;
Limit(t, -1.0, 1.0);
Pitch := arcsin(t);
Roll := arctanXY(A33, A32);
Yaw := arctanXY(A11, A21);

Just a note, P, Q and R are the rates of rotation not the angles themselves.

Hapy simming.