Quaternion: Implemented the largest pivot element method for conversion from Dcm to Quaternion to avoid numerical problems

matrix/Quaternion.hpp

@@ -90,24 +90,39 @@ public:
      *
      * @param dcm dcm to set quaternion to
      */
-    Quaternion(const Dcm<Type> &dcm) :
+    Quaternion(const Dcm<Type> &R) :
         Vector<Type, 4>()
     {
         Quaternion &q = *this;
-        q(0) = 1;
-        q(1) = 0;
-        q(2) = 0;
-        q(3) = 0;
-        Type q0_tmp = Type(0.5) * Type(sqrt(Type(1) + dcm(0, 0) +
-                                            dcm(1, 1) + dcm(2, 2)));
-        if (fabsf(q0_tmp) > 0) {
-            q(0) = q0_tmp;
-            q(1) = Type((dcm(2, 1) - dcm(1, 2)) /
-                        (Type(4) * q(0)));
-            q(2) = Type((dcm(0, 2) - dcm(2, 0)) /
-                        (Type(4) * q(0)));
-            q(3) = Type((dcm(1, 0) - dcm(0, 1)) /
-                        (Type(4) * q(0)));
+        Type t = R.trace();
+        if (t > Type(0)) {
+            t = sqrtf(Type(1) + t);
+            q(0) = Type(0.5) * t;
+            t = Type(0.5) / t;
+            q(1) = (R(2,1) - R(1,2)) * t;
+            q(2) = (R(0,2) - R(2,0)) * t;
+            q(3) = (R(1,0) - R(0,1)) * t;
+        } else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) {
+            t = sqrtf(Type(1) + R(0,0) - R(1,1) - R(2,2));
+            q(1) = Type(0.5) * t;
+            t = Type(0.5) / t;
+            q(0) = (R(2,1) - R(1,2)) * t;
+            q(2) = (R(1,0) + R(0,1)) * t;
+            q(3) = (R(0,2) + R(2,0)) * t;
+        } else if (R(1,1) > R(2,2)) {
+            t = sqrtf(Type(1) - R(0,0) + R(1,1) - R(2,2));
+            q(2) = Type(0.5) * t;
+            t = Type(0.5) / t;
+            q(0) = (R(0,2) - R(2,0)) * t;
+            q(1) = (R(1,0) + R(0,1)) * t;
+            q(3) = (R(2,1) + R(1,2)) * t;
+        } else {
+            t = sqrtf(Type(1) - R(0,0) - R(1,1) + R(2,2));
+            q(3) = Type(0.5) * t;
+            t = Type(0.5) / t;
+            q(0) = (R(1,0) - R(0,1)) * t;
+            q(1) = (R(0,2) + R(2,0)) * t;
+            q(2) = (R(2,1) + R(1,2)) * t;
         }
     }