四元数和欧拉角的相互转换
四元数转欧拉角&欧拉角转四元数程序：四元数转欧拉角&欧拉角转四元数程序。
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
|
#include "stdafx.h"
#include "math.h"
#include<iostream>#include<string.h>using namespace std;void EulerAnglesToQuaternion(double roll, double yaw, double pitch)
{ double cosRoll,sinRoll,cosPitch,sinPitch,cosyaw,sinyaw,qw,qx,qy,qz;
cosRoll = cosf(roll * 0.5f); //z
sinRoll = sinf(roll * 0.5f);//z
cosyaw= cosf(yaw * 0.5f);//y
sinyaw = sinf(yaw * 0.5f);//y
cosPitch = cosf(pitch * 0.5f);//x
sinPitch = sinf(pitch * 0.5f);//x
qw = cosRoll * cosyaw* cosPitch + sinRoll * sinyaw * sinPitch;
qx = cosRoll * sinyaw * cosPitch + sinRoll * cosyaw * sinPitch;
qy = cosRoll * cosyaw * sinPitch - sinRoll * sinyaw * cosPitch;
qz = sinRoll * cosyaw * cosPitch - cosRoll * sinyaw * sinPitch;
cout << "qw = " << qw << endl;
cout << "qx = " << qx << endl;
cout << "qy = " << qy << endl;
cout << "qz = " << qz << endl;
}void QuaternionToEulerAngles(double qw, double qx, double qy, double qz)
{ double roll, yaw, pitch;
roll = atan2f(2.f * (qw*qz + qx*qy), 1-2*(qz*qz+qx*qx)); //Z
yaw = asinf(2.f * (qw*qx - qy*qz)); //Y
pitch =atan2f(2.f * (qw*qy + qz*qx), 1-2*(qy*qy+qx*qx));//X
cout << "roll = " << roll << endl;
cout << "yaw = " << yaw << endl;
cout << "pitch = " << pitch << endl;
}int main()
{ QuaternionToEulerAngles(0, 0.707106, 0.707106, 0);
EulerAnglesToQuaternion(1.5707,0,3.14159);
return 0;
}</string.h></iostream> |
二、Ctrl+F5执行
三、结果