From 2f4427a9231823f198d8857d98e3a3f6c316a623 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Wed, 14 Dec 2016 17:55:05 +0100
Subject: [PATCH] Quaternion: Implemented the largest pivot element method for
 conversion from Dcm to Quaternion to avoid numerical problems

---
 matrix/Quaternion.hpp | 45 ++++++++++++++++++++++++++++---------------
 1 file changed, 30 insertions(+), 15 deletions(-)

diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
index 880e63142d..08d18d55bc 100644
--- a/matrix/Quaternion.hpp
+++ b/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;
         }
     }