1 |
tim |
1254 |
#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 |
tim |
1452 |
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 |
tim |
1254 |
} |
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 |
|
|
|