四元数,欧拉角,矩阵的相互转换
2010-05-12 23:40
309 查看
网上太多的将转换的了,翻来覆去转载没有意义。。奉上源码,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();
}
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
~~~~~~~~不华丽的分割~~以下是源码~~~~~~~~~~~~~~~~~~~~~~
/* 输入欧拉角,能看到四元数,以及再转换回去成欧拉角
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();
}
相关文章推荐
- 三维空间 旋转矩阵 四元数 欧拉角 相互转换 组合旋转
- 四元数、欧拉角及方向余弦矩阵的相互转换公式
- 四元数、欧拉角及方向余弦矩阵的相互转换公式
- 四元数、欧拉角及方向余弦矩阵的相互转换公式
- 3D数学基础公式------>矩阵,欧拉角,四元数,之间转换<2>
- Unity 四元数与欧拉角的相互转换及推导
- Eigen库:旋转矩阵,旋转向量和四元数的初始化和相互转换
- 四元数与欧拉角(RPY角)的相互转换
- Eigen库使用教程之旋转矩阵,旋转向量和四元数的初始化和相互转换的实现
- 旋转矩阵、欧拉角、四元数、轴/角之间的转换
- Eigen库中( 旋转矩阵,变换矩阵,轴向量,欧拉角,四元数) 表示及转换
- 旋转矩阵、欧拉角、四元数、轴/角之间的转换
- Eigen中欧拉角,旋转向量,旋转矩阵,四元数的转换
- math: 四元数与欧拉角(RPY角)的相互转换
- CSharpGL(32)矩阵与四元数与角度旋转轴的相互转换
- Eigen库使用教程之旋转矩阵,旋转向量和四元数的初始化和相互转换的实现
- 四元数、欧拉角、旋转矩阵之间互相转换C++源码
- 坐标系转换之三:欧拉角、四元数、旋转矩阵、方向余弦矩阵、旋转向量、轴角表示
- 四元数、欧拉角、旋转矩阵之间互相转换C++源码
- 旋转矩阵、四元数和欧拉角之间的转换——Matlab