diff options
Diffstat (limited to 'compat')
-rw-r--r-- | compat/euler.cpp | 68 |
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 + }; } } |