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
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
|
#include "euler.hpp"
#include <cmath>
namespace euler {
euler_t OTR_COMPAT_EXPORT rmat_to_euler(const rmat& R)
{
using std::atan2;
using std::sqrt;
const double cy = sqrt(R(2,2)*R(2, 2) + R(2, 1)*R(2, 1));
const bool large_enough = cy > 1e-10;
if (large_enough)
return euler_t(atan2(-R(1, 0), R(0, 0)),
atan2(R(2, 0), cy),
atan2(-R(2, 1), R(2, 2)));
else
return euler_t(atan2(R(0, 1), R(1, 1)),
atan2(R(2, 0), cy),
0);
}
// tait-bryan angles, not euler
rmat OTR_COMPAT_EXPORT euler_to_rmat(const euler_t& input)
{
const double H = -input(0);
const double P = -input(1);
const double B = -input(2);
using std::cos;
using std::sin;
const auto c1 = cos(H);
const auto s1 = sin(H);
const auto c2 = cos(P);
const auto s2 = sin(P);
const auto c3 = cos(B);
const auto s3 = sin(B);
return rmat(
// z
c1 * c2,
c1 * s2 * s3 - c3 * s1,
s1 * s3 + c1 * c3 * s2,
// y
c2 * s1,
c1 * c3 + s1 * s2 * s3,
c3 * s1 * s2 - c1 * s3,
// x
-s2,
c2 * s3,
c2 * c3
);
}
// https://en.wikipedia.org/wiki/Davenport_chained_rotations#Tait.E2.80.93Bryan_chained_rotations
void OTR_COMPAT_EXPORT tait_bryan_to_matrices(const euler_t& input,
rmat& r_roll,
rmat& r_pitch,
rmat& r_yaw)
{
using std::cos;
using std::sin;
{
const double phi = -input(2);
const double sin_phi = sin(phi);
const double cos_phi = cos(phi);
r_roll = rmat(1, 0, 0,
0, cos_phi, -sin_phi,
0, sin_phi, cos_phi);
}
{
const double theta = input(1);
const double sin_theta = sin(theta);
const double cos_theta = cos(theta);
r_pitch = rmat(cos_theta, 0, -sin_theta,
0, 1, 0,
sin_theta, 0, cos_theta);
}
{
const double psi = -input(0);
const double sin_psi = sin(psi);
const double cos_psi = cos(psi);
r_yaw = rmat(cos_psi, -sin_psi, 0,
sin_psi, cos_psi, 0,
0, 0, 1);
}
}
// from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
Quat matrix_to_quat(const rmat& M)
{
Quat q(1, 0, 0, 0);
using std::sqrt;
double trace = M(0, 0) + M(1, 1) + M(2, 2); // I removed + 1.0; see discussion with Ethan
if( trace > 0 ) {// I changed M_EPSILON to 0
double s = .5 / std::sqrt(trace + 1);
q.w() = .25 / s;
q.x() = ( M(2, 1) - M(1, 2) ) * s;
q.y() = ( M(0, 2) - M(2, 0) ) * s;
q.z() = ( M(1, 0) - M(0, 1) ) * s;
} else {
if ( M(0, 0) > M(1, 1) && M(0, 0) > M(2, 2) ) {
double s = 2.0 * sqrt( 1.0 + M(0, 0) - M(1, 1) - M(2, 2));
q.w() = (M(2, 1) - M(1, 2) ) / s;
q.x() = .25 * s;
q.y() = (M(0, 1) + M(1, 0) ) / s;
q.z() = (M(0, 2) + M(2, 0) ) / s;
} else if (M(1, 1) > M(2, 2)) {
double s = 2.0 * sqrt( 1.0 + M(1, 1) - M(0, 0) - M(2, 2));
q.w() = (M(0, 2) - M(2, 0) ) / s;
q.x() = (M(0, 1) + M(1, 0) ) / s;
q.y() = .25 * s;
q.z() = (M(1, 2) + M(2, 1) ) / s;
} else {
double s = 2.0 * sqrt( 1.0 + M(2, 2) - M(0, 0) - M(1, 1) );
q.w() = (M(1, 0) - M(0, 1) ) / s;
q.x() = (M(0, 2) + M(2, 0) ) / s;
q.y() = (M(1, 2) + M(2, 1) ) / s;
q.z() = .25 * s;
}
}
return q;
}
} // end ns euler
|
