getting euler angles from a 4x4 matrix ( OpenGL matrix )

[OSasuke]

Hi every one.

I need to extract euler angles from a 4x4 matrix ( OpenGL matrix ).
I have some problems, and i your help.

This is my matrix :

Code: Select all

     |M0     M4     M8     M12|
M =  |M1     M5     M9     M13|
     |M2     M6     M10    M14|
     |M3      M7      M11     M15|

After rotation i have a matrix like this :

Code: Select all

R1 = cos(y) * cos(z)
R2 = -cos(y) * sin(z)
R3 = sin(y)
R4 = sin(x) * sin(y) * cos(z) + cos(x) * sin(z)
R5 = -sin(x) * sin(y) * sin(z) + cos(x) * cos(z)
R6 = -sin(x) * cos(y)
R7 = -cos(x) * sin(y) * cos(z) + sin(x) * sin(z)
R8 = cos(x) * sin(y) * sin(z) + sin(x) * cos(z)
R9 = cos(x) * cos(y)

This is my code :

Code: Select all

#include <cstdlib>
#include <iostream>
#include <SDL/SDL.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <cmath>

#define size 480

using namespace std;

int main(int argc, char *argv[])
{

    SDL_Init(SDL_INIT_VIDEO);

    SDL_WM_SetCaption("OpenGL Matrix !",0);

    SDL_SetVideoMode(size, size, 32, SDL_OPENGL);

    FILE *stream ;
    stream = freopen("CON", "w", stdout);

    glMatrixMode( GL_MODELVIEW );
    glLoadIdentity( );

    glTranslated(0,0,0);
    glRotated(95,1,0,0); // Problem with this axe
    glRotated(130,0,1,0);
    glRotated(-99,0,0,1);

    float m[16] = {0};

    glGetFloatv(GL_MODELVIEW_MATRIX , m);

    for(int i=0; i<4; i++)
    {
        for(int j=0; j<4; j++)
        {
            cout << m[i+j*4] << "\t";
        }
        cout << endl;
    }

    float fAngX,fAngY,fAngZ;

    fAngZ = atan2f(m[1], m[5]) * 180 / M_PI;
    fAngY = atan2f(m[8], m[10]) * 180 / M_PI;
    fAngX = -asinf(m[9]) * 180 / M_PI;

    cout << "X >> " << fAngX << endl;
    cout << "Y >> " << fAngY << endl;
    cout << "Z >> " << fAngZ << endl;

    SDL_Quit();

    return 0;
}

Thank you.

[bone]

From Eberly's 3D Game Engine Design book, I believe that is factored as RxRyRz, his pseudocode to get back the Euler angles is (but using your r1-r9 notation):

Code: Select all

thetaY = asin( r3 )
if( thetaY < PI/2 )
{
  if( thetaY > -PI/2 )
  {
    thetaX = atan2( -r6, r9 );
    thetaZ = atan2( -r2, r1 );
  }
  else
  {
    // not a unique solution
    thetaX = -atan2( r4, r5 );
    thetaZ = 0;
  }
}
else
{
  // not a unique solution
  thetaX = atan2( r4, r5 );
  thetaZ = 0;
}

[OSasuke]

No there are problems when i make a rotation about the Y axis.

Code: Select all

#include <cstdlib>
#include <iostream>
#include <SDL/SDL.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <cmath>

#define size 480

#define DEG 180/M_PI

using namespace std;

int main(int argc, char *argv[])
{
    SDL_Init(SDL_INIT_VIDEO);
    SDL_WM_SetCaption("La 2D avec OpenGL !",0);
    SDL_SetVideoMode(size, size, 32, SDL_OPENGL);

    FILE *stream ;
    stream = freopen("CON", "w", stdout);

    glMatrixMode( GL_PROJECTION );
    glLoadIdentity();
    gluOrtho2D(-size/2,size/2,-size/2,size/2);

    bool stop = false;

    SDL_Event event;

    glMatrixMode( GL_MODELVIEW );
    glLoadIdentity( );

    float m[16] = {0};

    glTranslated(0,0,0);

    glPushMatrix();

    glRotated(50,1,0,0);
    glRotated(190,0,1,0);
    glRotated(70,0,0,1);

    glGetFloatv(GL_MODELVIEW_MATRIX , m);

    for(int i=0; i<4; i++)
    {
        for(int j=0; j<4; j++)
        {
            cout << m[i+j*4] << "\t";
        }
        cout << endl;
    }

    float fAngX=0,fAngY=0,fAngZ=0;

    fAngY = asin( m[8] )*DEG;
    if( fAngY < M_PI/2 )
    {
        if( fAngY > -M_PI/2 )
        {
            fAngX = atan2( -m[9], m[10] )*DEG;
            fAngZ = atan2( -m[4], m[0] )*DEG;
        }
        else
        {
            // not a unique solution
            fAngX = -atan2( m[1], m[5] )*DEG;
            fAngZ = 0;
        }
    }
    else
    {
        // not a unique solution
        fAngX = atan2( m[1], m[5] )*DEG;
        fAngZ = 0;
    }

    cout << "X >> " << fAngX << endl;
    cout << "Y >> " << fAngY << endl;
    cout << "Z >> " << fAngZ << endl;
    glPopMatrix();

    SDL_Quit();

    return 0;
}

Can you give me the code directely from the Eberly's 3D Game Engine Design book, please.

And, i think that we can't get the rotation of the Y axis by using asin because asin = [-90;90]

and

Code: Select all

sin(150) = 0.5;
asin(0.5) = 30;

// We found the wrong value.

Thank you.

[bone]

First off, sin() and asin() work with radians in C/C++, not degrees, so you are incorrect. I have successfully used the above code with a slightly different factorization for about 10 years now. It works perfectly fine. [edit: I see the bug in your code, you can't multiply fAngY by DEG and then use it the same way as my sample code ... can you try debugging first next time?]

You should probably just buy Eberly's book, you can try some of the other factorizations if that's not the correct one (but I think it is). The code is exactly the same as above except he uses the matrix notation:

| r00 r01 r02 |
| r10 r11 r12 |
| r20 r21 r22 |

... so r3 == r10, etc.

Note that I'm not sure how OpenGL works and if they use column-major or row-major matrix storage, so you might also try the transpose.

[OSasuke]

First off, sin() and asin() work with radians in C/C++, not degrees, so you are incorrect.

I know, it was just an example. Ok let's convert :

Code: Select all

float RAD = M_PI/180;

float sinAngle;
sinAngle = sin(150*RAD);
std::cout << asin(sinAngle)/RAD << std::endl;
// Division by RAD is converting to DEG

This is the result : 30
The result that should be is 150.

| r00 r01 r02 |
| r10 r11 r12 |
| r20 r21 r22 |

Mmm. In OpenGL the matrix are in this order :

Code: Select all

|  r00  r10  r20  |
|  r01  r11  r21  |
|  r02  r12  r22  |

Thanks again.

[OSasuke]

And how to get eulers from a quaternion.

[bone]

So you want asin to return 0 to +pi rather than -pi/2 to +pi/2? Can't help you, that's what it does. There's two possible return values for most input values, so it can only cover half of a circle, and somebody along the way picked the range -pi/2 to +pi/2. This was a good choice, though, because sin(30)=sin(150), and most people would expect a return value of 30.

Eulers from a quaternion? It can be done, and I don't even remember where I got my code from (a google search may help). My matrix factorization assumes a different order of Euler angles, so the following may or may not help you. You might consider changing the signs in the final four equations, though, you might find the correct answer through trial-and-error.

Code: Select all

void EulerToQuat( const TripleS &ori, QuatS &q )
{
  const float x = (RealS) 0.5 * ori.x;
  const float cx = cos( x );
  const float sx = sin( x );
  
  const float y = (RealS) 0.5 * ori.y;
  const float cy = cos( y );
  const float sy = sin( y );

  const float z = (RealS) 0.5 * ori.z;
  const float cz = cos( z );
  const float sz = sin( z );

  const float cy_x_cz = cy * cz;
  const float sy_x_sz = sy * sz;
  const float cy_x_sz = cy * sz;
  const float sy_x_cz = sy * cz;

  q.w = ( cx * cy_x_cz ) + ( sx * sy_x_sz );
  q.x = ( sx * cy_x_cz ) + ( cx * sy_x_sz );
  q.y = ( cx * sy_x_cz ) - ( sx * cy_x_sz );
  q.z = ( cx * cy_x_sz ) - ( sx * sy_x_cz );
}

[OSasuke]

I found a good way to get the eulers. But I still have problems.

This is the code :

Code: Select all

#include <cstdlib>
#include <iostream>
#include <SDL/SDL.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <cmath>

#define size 480

using namespace std;

int main(int argc, char *argv[])
{
    SDL_Init(SDL_INIT_VIDEO);
    SDL_WM_SetCaption("OpenGL",0);
    SDL_SetVideoMode(size, size, 32, SDL_OPENGL);

    FILE *stream ;
    stream = freopen("CON", "w", stdout);

    glMatrixMode( GL_PROJECTION );
    glLoadIdentity();
    gluOrtho2D(-size/2,size/2,-size/2,size/2);

    glMatrixMode( GL_MODELVIEW );
    glLoadIdentity( );

    float m[16] = {0};

    glTranslated(0,0,0);
    
    for(int i=0; i<=360; i+=10)
    {
    glPushMatrix();

    glRotated(0,1,0,0);
    glRotated(i,0,1,0);
    glRotated(0,0,0,1);

    glGetFloatv(GL_MODELVIEW_MATRIX , m);

    float fAngX=0,fAngY=0,fAngZ=0;

    float RADIANS = 180/M_PI;

    fAngY = asin(m[8]);
    float cy = cos(fAngY);

    float cz = m[0]/cy;
    float sz = -m[4]/cy;
    fAngZ = atan2(sz,cz) * RADIANS;

    float cx = m[10]/cy;
    float sx = -m[9]/cy;
    fAngX = atan2(sx,cx) * RADIANS;

    fAngY *= RADIANS;

    if(fAngY<0) fAngY+=360;
    if(fAngX<0) fAngX+=360;
    if(fAngZ<0) fAngZ+=360;

    cout << "X: " << fAngX << "\t";
    cout << "Y: " << fAngY << "\t";
    cout << "Z: " << fAngZ << "\t";
    cout << endl;

    glPopMatrix();
    }

    SDL_Quit();

    return 0;
}

And this is the result :

Code: Select all

X: 0       Y: 0        Z: 0
X: 0       Y: 10      Z: 0
X: 0       Y: 20      Z: 0
X: 0       Y: 30      Z: 0
X: 0       Y: 40      Z: 0
X: 0       Y: 50      Z: 0
X: 0       Y: 60      Z: 0
X: 0       Y: 70      Z: 0
X: 0       Y: 80      Z: 0
X: 180    Y: 90      Z: 180
X: 180    Y: 80      Z: 180
X: 180    Y: 70      Z: 180
X: 180    Y: 60      Z: 180
X: 180    Y: 50      Z: 180
X: 180    Y: 40      Z: 180
X: 180    Y: 30      Z: 180
X: 180    Y: 20      Z: 180
X: 180    Y: 10      Z: 180
X: 180    Y: 0        Z: 180
X: 180    Y: 350     Z: 180
X: 180    Y: 340     Z: 180
X: 180    Y: 330     Z: 180
X: 180    Y: 320     Z: 180
X: 180    Y: 310     Z: 180
X: 180    Y: 300     Z: 180
X: 180    Y: 290     Z: 180
X: 180    Y: 280     Z: 180
X: 0       Y: 270     Z: 0
X: 0       Y: 280     Z: 0
X: 0       Y: 290     Z: 0
X: 0       Y: 300     Z: 0
X: 0       Y: 310     Z: 0
X: 0       Y: 320     Z: 0
X: 0       Y: 330     Z: 0
X: 0       Y: 340     Z: 0
X: 0       Y: 350     Z: 0
X: 0       Y: 360     Z: 0

As you can see, there is problems when Y = [90 ; 270[ ( Y is between 90 and 270 ).

There are no problem with the other axis. The broblem is the Y axis.

For exemple :

Code: Select all

glRotated(i,1,0,0);
glRotated(0,0,1,0);
glRotated(i,0,0,1);

Gives me the right values. ( X = [0 ; 360] and Y = {0} and Z = [0 ; 360] ).

Any help, please.

[bone]

I could be wrong, but I don't think there are unique answers to those cases. Notice the comments I copied from Eberly's book.

Perhaps you're not aware, but representing rotations with Euler angles is a horrible mess, with the interdependencies between the values causing gimbal lock. If you are able to, you should move to matrices or quaternions. Both of those have their own advantages and disadvantages, but they're both much better than Euler angles (which I only use to support legacy code).

[OSasuke]

But I need Eulers to build Artificiel intelligence.

I have a last question : How to find the value of cos(y) in the matrix. Because atan2(sin(y), cos(y)) will return a number between pi and -pi .

I found a solution but not complete :

Code: Select all

m[9] = -sin(x)*cos(y);
m[10] = cos(x)*cos(y);
cos(y) = sqrt((-m[9]*-m[9])+(m[10]*m[10]));
// cos(y) = sqrt(sin(x)²*cos(y)² + cos(x)²*cos(y)²)
//          = sqrt( cos(y)²*(sin(x)²+cos(x)²) )
//          = sqrt(cos(y)² * 1)

This solution don't gives me cos(y) but it gives ||cos(y)|| or fabs(cos(y))
If the real cos(y) is -0.5 the result of this solution is 0.5

[dneckels]

If you get totally stuck, Eberly's library has implementations:

http://www.geometrictools.com/LibFounda ... 4Matrix3.h

I was able to integrate his matrix/vector libraries into my code pretty easily (they don't have a ton of dependencies).

cheers
-dn

[OSasuke]

Thanks.

But how to download this library

The Wm4Matrix3.cpp file don't have all functions code.

[dneckels]

Looks like the implentation is here:

http://www.geometrictools.com/LibFounda ... atrix3.inl

Templates, .

I don't know how to download everything, but the matrix and vector libs are pretty stand alone, as I recall. I got the code from the book CD.

Also, maybe try wget:
http://en.wikipedia.org/wiki/Wget

[OSasuke]

Thanks again.