summaryrefslogtreecommitdiffhomepage
path: root/compat
diff options
context:
space:
mode:
Diffstat (limited to 'compat')
-rw-r--r--compat/euler.cpp68
1 files changed, 39 insertions, 29 deletions
diff --git a/compat/euler.cpp b/compat/euler.cpp
index ab119d3e..f02365c9 100644
--- a/compat/euler.cpp
+++ b/compat/euler.cpp
@@ -11,13 +11,17 @@ euler_t OTR_COMPAT_EXPORT rmat_to_euler(const rmat& R)
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)));
+ return {
+ 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);
+ return {
+ atan2(R(0, 1), R(1, 1)),
+ atan2(R(2, 0), cy),
+ 0
+ };
}
// tait-bryan angles, not euler
@@ -37,20 +41,20 @@ rmat OTR_COMPAT_EXPORT euler_to_rmat(const euler_t& input)
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
- );
+ return {
+ // 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
@@ -67,9 +71,11 @@ void OTR_COMPAT_EXPORT tait_bryan_to_matrices(const euler_t& input,
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);
+ r_roll = {
+ 1, 0, 0,
+ 0, cos_phi, -sin_phi,
+ 0, sin_phi, cos_phi
+ };
}
{
@@ -77,9 +83,11 @@ void OTR_COMPAT_EXPORT tait_bryan_to_matrices(const euler_t& input,
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);
+ r_pitch = {
+ cos_theta, 0, -sin_theta,
+ 0, 1, 0,
+ sin_theta, 0, cos_theta
+ };
}
{
@@ -87,9 +95,11 @@ void OTR_COMPAT_EXPORT tait_bryan_to_matrices(const euler_t& input,
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);
+ r_yaw = {
+ cos_psi, -sin_psi, 0,
+ sin_psi, cos_psi, 0,
+ 0, 0, 1
+ };
}
}