1 |
#include <cmath> |
2 |
#include "Euler3.hpp" |
3 |
#include "Vector3d.hpp" |
4 |
#include "Mat3x3d.hpp" |
5 |
#include "Quaternion.hpp" |
6 |
|
7 |
Quaternion::Quaternion(Mat3x3d& m){ |
8 |
*this = m.toQuaternion(); |
9 |
} |
10 |
|
11 |
Quaternion::Quaternion(const Euler3& e){ |
12 |
x = cos(e.theta/2) * cos((e.phi + e.psi)/2); |
13 |
y = sin(e.theta/2) * cos((e.phi - e.psi)/2); |
14 |
z = sin(e.theta/2) * sin((e.phi - e.psi)/2); |
15 |
w = cos(e.theta/2) * sin((e.phi + e.psi)/2); |
16 |
} |
17 |
|
18 |
Mat3x3d Quaternion::toRotationMatrix(){ |
19 |
|
20 |
return Mat3x3d(*this); |
21 |
} |
22 |
|
23 |
Euler3 Quaternion::toEuler(){ |
24 |
Mat3x3d m; |
25 |
m = toRotationMatrix(); |
26 |
return m.toEuler(); |
27 |
} |
28 |
|