四元数转欧拉角&欧拉角转四元数程序：四元数转欧拉角&欧拉角转四元数程序。
|
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执行
三、结果
