Added better roundtrip euler test.

matrix/Dcm.hpp

@@ -64,12 +64,12 @@ public:
 
     Dcm(const Euler<Type> & euler) {
         Dcm &dcm = *this;
-        Type cosPhi = cosf(euler.phi());
+        Type cosPhi = Type(cos(euler.phi()));
-        Type sinPhi = sinf(euler.phi());
+        Type sinPhi = Type(sin(euler.phi()));
-        Type cosThe = cosf(euler.theta());
+        Type cosThe = Type(cos(euler.theta()));
-        Type sinThe = sinf(euler.theta());
+        Type sinThe = Type(sin(euler.theta()));
-        Type cosPsi = cosf(euler.psi());
+        Type cosPsi = Type(cos(euler.psi()));
-        Type sinPsi = sinf(euler.psi());
+        Type sinPsi = Type(sin(euler.psi()));
 
         dcm(0, 0) = cosThe * cosPsi;
         dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;

matrix/Matrix.hpp

@@ -131,7 +131,7 @@ public:
     bool operator==(const Matrix<Type, M, N> &other) const
     {
         const Matrix<Type, M, N> &self = *this;
-        static const Type eps = Type(1e-6);
+        static const Type eps = Type(1e-4);
 
         for (size_t i = 0; i < M; i++) {
             for (size_t j = 0; j < N; j++) {

test/attitude.cpp

@@ -86,20 +86,36 @@ int main()
     Quatf q2(dcm_check);
     TEST(isEqual(q2, q_check));
 
+    // constants
+    double deg2rad = M_PI/180.0;
+    double rad2deg = 180.0/M_PI;
 
-    // euler gimbal lock check
+    // euler dcm round trip check
-    // note if theta = pi/2, then roll is set to zero
+    for (int roll=-90; roll<90; roll+=1) {
-    float pi_2 = float(M_PI_2);
+        for (int pitch=-90; pitch<90; pitch+=1) {
-    Eulerf euler_gimbal_lock(0.0f, pi_2, 0.2f);
+            for (int yaw=0; yaw<360; yaw+=1) {
-    Dcmf dcm_lock(euler_gimbal_lock);
+                // note if theta = pi/2, then roll is set to zero
-    Eulerf euler_gimbal_lock_out(dcm_lock);
+                if (pitch == 90 || pitch == -90) {
-    TEST(isEqual(euler_gimbal_lock, euler_gimbal_lock_out));
+                    roll = 0;
+                }
+                printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
+                Euler<double> euler(deg2rad*double(roll),
+                        deg2rad*double(pitch),
+                        deg2rad*double(yaw));
+                Dcm<double> dcm_from_euler(euler);
+                Euler<double> euler_out(dcm_from_euler);
+                TEST(isEqual(rad2deg*euler, rad2deg*euler_out));
 
-    // note if theta = pi/2, then roll is set to zero
+                Eulerf eulerf(float(deg2rad)*float(roll),
-    Eulerf euler_gimbal_lock2(0.0f, -pi_2, 0.2f);
+                        float(deg2rad)*float(pitch),
-    Dcmf dcm_lock2(euler_gimbal_lock2);
+                        float(deg2rad)*float(yaw));
-    Eulerf euler_gimbal_lock_out2(dcm_lock2);
+                Dcm<float> dcm_from_eulerf(eulerf);
-    TEST(isEqual(euler_gimbal_lock2, euler_gimbal_lock_out2));
+                Euler<float> euler_outf(dcm_from_eulerf);
+                TEST(isEqual(float(rad2deg)*eulerf,
+                            float(rad2deg)*euler_outf));
+            }
+        }
+    }
 
     // quaterion copy ctors
     float data_v4[] = {1, 2, 3, 4};