Euler: simplify DCM constructor

Co-authored-by: Matthias Grob <maetugr@gmail.com>

matrix/Euler.hpp

@@ -91,23 +91,20 @@ public:
     */
     Euler(const Dcm<Type> &dcm)
     {
-        Type phi_val = Type(atan2(dcm(2, 1), dcm(2, 2)));
+        theta() = asin(-dcm(2, 0));
-        Type theta_val = Type(asin(-dcm(2, 0)));
+
-        Type psi_val = Type(atan2(dcm(1, 0), dcm(0, 0)));
+        if ((fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) {
-        Type pi = Type(M_PI);
+            phi() = 0;
-
+            psi() = atan2(dcm(1, 2), dcm(0, 2));
-        if (Type(fabs(theta_val - pi / Type(2))) < Type(1.0e-3)) {
-            phi_val = Type(0);
-            psi_val = Type(atan2(dcm(1, 2), dcm(0, 2)));
-
-        } else if (Type(fabs(theta_val + pi / Type(2))) < Type(1.0e-3)) {
-            phi_val = Type(0);
-            psi_val = Type(atan2(-dcm(1, 2), -dcm(0, 2)));
-        }
 
-        phi() = phi_val;
+        } else if ((fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) {
-        theta() = theta_val;
+            phi() = 0;
-        psi() = psi_val;
+            psi() = atan2(-dcm(1, 2), -dcm(0, 2));
+
+        } else {
+            phi() = atan2(dcm(2, 1), dcm(2, 2));
+            psi() = atan2(dcm(1, 0), dcm(0, 0));
+        }
     }
 
     /**