四元数，欧拉角，矩阵的相互转换

网上太多的将转换的了，翻来覆去转载没有意义。。奉上源码，TC下直接编译即可～～在附上编译好了的exe可以直接下载运行～～

～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～
～～～～～～～～不华丽的分割～～以下是源码～～～～～～～～～～～～～～～～～～～～～～
/*  输入欧拉角，能看到四元数，以及再转换回去成欧拉角
    Yaw范围（-180~180）
    Pitch范围（-90~90）
    Roll范围（-180~180）
*/

#include "stdio.h"
#include "math.h"
#include "conio.h"

main()
{
float theta_z , theta_y ,theta_x ;

float cos_z_2;
float cos_y_2;
float cos_x_2;

float sin_z_2;
float sin_y_2;
float sin_x_2;

float Pitch;
float Roll;
float Yaw;
float Q[4];
float   T[3][3];
do{
printf("/nYaw = ");
scanf("%f",&theta_z);
printf("/nPitch = ");
scanf("%f",&theta_y);
printf("/nRoll = ");
scanf("%f",&theta_x);

theta_z = theta_z*3.1416/180;
theta_y = theta_y*3.1416/180;
theta_x = theta_x*3.1416/180;

 cos_z_2 = cos(0.5*theta_z);
 cos_y_2 = cos(0.5*theta_y);
 cos_x_2 = cos(0.5*theta_x);

 sin_z_2 = sin(0.5*theta_z);
 sin_y_2 = sin(0.5*theta_y);
 sin_x_2 = sin(0.5*theta_x);

Q[0] = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
Q[1] = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
Q[2] = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
Q[3] = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;

printf("/nQ=[ %f %f %f %f]/n/n",Q[0],Q[1],Q[2],Q[3]) ;
printf("alpha = %f/n/n",acos(Q[0])*2*180/3.1416) ;

    T[0][0] =   Q[0]*Q[0]+Q[1]*Q[1]-Q[2]*Q[2]-Q[3]*Q[3] ;
    T[0][1] =                    2*(Q[1]*Q[2]-Q[0]*Q[3]);
    T[0][2] =                    2*(Q[1]*Q[3]+Q[0]*Q[2]);

    T[1][0] =                    2*(Q[1]*Q[2]+Q[0]*Q[3]);
    T[1][1] =   Q[0]*Q[0]-Q[1]*Q[1]+Q[2]*Q[2]-Q[3]*Q[3] ;
    T[1][2] =                    2*(Q[2]*Q[3]-Q[0]*Q[1]);

    T[2][0] =                    2*(Q[1]*Q[3]-Q[0]*Q[2]);
    T[2][1] =                    2*(Q[2]*Q[3]+Q[0]*Q[1]);
    T[2][2] =   Q[0]*Q[0]-Q[1]*Q[1]-Q[2]*Q[2]+Q[3]*Q[3] ;

    printf("T[0][0] = %9f,T[0][1] = %9f,T[0][2] = %9f/n",T[0][0],T[0][1],T[0][2]);
    printf("T[1][0] = %9f,T[1][1] = %9f,T[1][2] = %9f/n",T[1][0],T[1][1],T[1][2]);
    printf("T[2][0] = %9f,T[2][1] = %9f,T[2][2] = %9f/n/n",T[2][0],T[2][1],T[2][2]);

    Pitch = asin(-T[2][0]);
    Roll  = atan( T[2][1]/T[2][2]);
    Yaw   = atan( T[1][0]/T[0][0]);

    if(T[2][2]<0)
    {
        if(Roll < 0)
        {
           Roll = Roll+3.1416;
        }
        else
        {
           Roll = Roll-3.1416;
        }
    }

    if(T[0][0]<0)
    {
        if(T[1][0]>0)
        {
            Yaw = Yaw + 3.1416;
        }
        else
        {
            Yaw = Yaw - 3.1416;
        }
    }

    printf("Yaw   = %f/nPitch = %f/nRoll  = %f/n",Yaw*180/3.1416,Pitch*180/3.1416,Roll*180/3.1416) ;
}while(1);
    printf("Hello, world/n");
    getch();
}