1 files changed, 27 insertions, 107 deletions
diff --git a/compat/euler.cpp b/compat/euler.cpp
index e48d977b..5d6fb25b 100644
--- a/compat/euler.cpp
+++ b/compat/euler.cpp
@@ -1,35 +1,34 @@
 #include "euler.hpp"
+#include "math-imports.hpp"
 #include <cmath>
 
 namespace euler {
 
-euler_t OTR_COMPAT_EXPORT rmat_to_euler(const rmat& R)
+Pose_ 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)));
+       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
-rmat OTR_COMPAT_EXPORT euler_to_rmat(const euler_t& input)
+rmat euler_to_rmat(const Pose_& 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);
@@ -37,99 +36,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
-                );
-}
-
-// 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;
+    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
+    };
 }
 
 } // end ns euler