1 files changed, 22 insertions, 96 deletions
diff --git a/compat/euler.cpp b/compat/euler.cpp
index 7c753214..39d2b116 100644
--- a/compat/euler.cpp
+++ b/compat/euler.cpp
@@ -2,113 +2,39 @@
 #include "math-imports.hpp"
 #include <cmath>
 
+// rotation order is XYZ
+
 namespace euler {
 
 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 {
-           atan2(-R(1, 0), R(0, 0)),
-           atan2(R(2, 0), cy),
-           atan2(-R(2, 1), R(2, 2))
-       };
-    else
-        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)
-{
-    const double H = -input(0);
-    const double P = -input(1);
-    const double B = -input(2);
+    double alpha, beta, gamma;
 
-    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);
+    beta  = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
+    alpha = atan2( R(1,0), R(0,0));
+    gamma = atan2( R(2,1), R(2,2));
 
-    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
-    };
+    return { alpha, -beta, gamma };
 }
 
-// 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)
+rmat OTR_COMPAT_EXPORT euler_to_rmat(const euler_t& e)
 {
-    {
-        const double phi = -input(2);
-        const double sin_phi = sin(phi);
-        const double cos_phi = cos(phi);
-
-        r_roll = {
-            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);
+    const double X = e(2);
+    const double Y = -e(1);
+    const double Z = e(0);
 
-        r_pitch = {
-            cos_theta, 0, -sin_theta,
-            0, 1, 0,
-            sin_theta, 0, cos_theta
-        };
-    }
+    const double cx = cos(X);
+    const double sx = sin(X);
+    const double cy = cos(Y);
+    const double sy = sin(Y);
+    const double cz = cos(Z);
+    const double sz = sin(Z);
 
-    {
-        const double psi = -input(0);
-        const double sin_psi = sin(psi);
-        const double cos_psi = cos(psi);
-
-        r_yaw = {
-            cos_psi, -sin_psi, 0,
-            sin_psi, cos_psi, 0,
-            0, 0, 1
-        };
-    }
-}
-
-template<int H, int W, typename f = double>
-rmat quaternion_to_mat_(const Mat<f, H, W>& q)
-{
-    const double w = q.w(), x = q.x(), y = q.y(), z = q.z();
-    const double ww = w*w, xx = x*x, yy = y*y, zz = z*z;
-
-    return rmat(
-                ww + xx - yy - zz,  2 * (x*y - w*z),    2 * (x*z + w*y),
-                2 * (x*z - w*y),    2 * (y*z + w*x),    ww - xx - yy + zz,
-                2 * (x*y + w*z),    ww - xx + yy - zz,  2 * (y*z - w*x)
-            );
+    return {
+        cy*cz,              -cy*sz,             sy,
+        cz*sx*sy + cx*sz,   cx*cz - sx*sy*sz,   -cy*sx,
+        -cx*cz*sy + sx*sz,  cz*sx + cx*sy*sz,   cx*cy,
+    };
 }
 
-rmat OTR_COMPAT_EXPORT quaternion_to_mat(const dmat<1, 4>& q) { return quaternion_to_mat_(q); }
-//rmat OTR_COMPAT_EXPORT quaternion_to_mat(const dmat<4, 1>& q) { return quaternion_to_mat_(q); }
-
 } // end ns euler