C Matrix to Euler Rotation Conversion

2011/03/01
(この記事の文字数: 972)

先日、回転行列からオイラー角の変換をOpenMayaを使ってやりましたが、下記サイトにJavaのサンプルコードが載っていたので参考にしてCで書いてみました。

http://www.euclideanspace.com/maths/geometry/rotations/conversions/
matrixToEuler/index.htm

Conversion between Rotation Matrix and Euler in C is like following.


#include <stdio.h>

#define _USE_MATH_DEFINES

#include <math.h>



void MatrixToEuler(double o_euler[3],const double *in_mat)

{

        double heading, attitude, bank;

        if(in_mat[3] > 0.998)

        {        //** singularity at north pole

                heading = atan2(in_mat[2], in_mat[8]);

                attitude = M_PI/2.0;

                bank = 0;

        }

        else if(in_mat[3] < -0.998)

        {        //** singularity at south pole

                heading = atan2(in_mat[2], in_mat[8]);

                attitude = -M_PI/2.0;

                bank = 0;

        }

        else

        {

                heading = atan2(-in_mat[6], in_mat[0]);

                bank = atan2(-in_mat[5], in_mat[4]);

                attitude = asin(in_mat[3]);

        }

        o_euler[0]=heading;

        o_euler[1]=attitude;

        o_euler[2]=bank;

}



int main()

{

        double euler[3];

        double mat[]={0.8047, -0.5059, -0.3106,

                      0.3106,  0.8047, -0.5059,

                      0.5059,  0.3106,  0.8047};



        MatrixToEuler(euler, mat);



        printf("%f, %f, %f\n", euler[0]*180/M_PI, 

               euler[1]*180/M_PI,euler[2]*180/M_PI);

        return 0;

}