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

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

网上太多的将转换的了，翻来覆去转载没有意义。。奉上源码，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]{if(Roll {Roll =

Roll+3.1416;}else{Roll =

Roll-3.1416;}}if(T[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.141 6) ;}while(1);printf("Hello, world/n");getch();}