|
網(wǎng)上太多的將轉(zhuǎn)換的了，翻來覆去轉(zhuǎn)載沒有意義。。奉上源碼，TC下直接編譯即可～～在附上編譯好了的exe可以直接下載運行～～
 ～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～ ～～～～～～～～不華麗的分割～～以下是源碼～～～～～～～～～～～～～～～～～～～～～～ /* 輸入歐拉角，能看到四元數(shù)，以及再轉(zhuǎn)換回去成歐拉角 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(); } |
|
|
