From 84b3da227cda0b66a2c8ebaa99aeddedf8858b02 Mon Sep 17 00:00:00 2001
From: kritz <kritz@ethz.ch>
Date: Thu, 22 Aug 2019 15:05:14 +0200
Subject: [PATCH] Canonical Quaternion with tests (#81)

---
 matrix/Quaternion.hpp | 24 ++++++++++++++++++++++++
 test/attitude.cpp     | 11 +++++++++++
 2 files changed, 35 insertions(+)

diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
index 29e711dc46..f543a1b929 100644
--- a/matrix/Quaternion.hpp
+++ b/matrix/Quaternion.hpp
@@ -352,6 +352,30 @@ public:
                    -q(3)/normSq);
     }
 
+    /**
+     * Bring quaternion to canonical form
+     */
+    void canonicalize()
+    {
+        *this = this->canonical();
+    }
+
+
+    /**
+     * Return canonical form of the quaternion
+     *
+     * @return quaternion in canonical from
+     */
+    Quaternion canonical() const
+    {
+        const Quaternion &q = *this;
+        if(q(0)<Type(0)) {
+            return Quaternion(-q(0),-q(1),-q(2),-q(3));
+        } else {
+            return Quaternion(q(0),q(1),q(2),q(3));
+        }
+    }
+
     /**
      * Rotate quaternion from rotation vector
      *
diff --git a/test/attitude.cpp b/test/attitude.cpp
index 3f1c132277..525bbd06be 100644
--- a/test/attitude.cpp
+++ b/test/attitude.cpp
@@ -258,6 +258,17 @@ int main()
     TEST(fabs(q_check(2) + q(2)) < eps);
     TEST(fabs(q_check(3) + q(3)) < eps);
 
+    // quaternion canonical
+    Quatf q_non_canonical(-0.7f,0.4f, 0.3f, -0.3f);
+    Quatf q_canonical(0.7f,-0.4f, -0.3f, 0.3f);
+    Quatf q_canonical_ref(0.7f,-0.4f, -0.3f, 0.3f);
+    TEST(isEqual(q_non_canonical.canonical(),q_canonical_ref));
+    TEST(isEqual(q_canonical.canonical(),q_canonical_ref));
+    q_non_canonical.canonicalize();
+    q_canonical.canonicalize();
+    TEST(isEqual(q_non_canonical,q_canonical_ref));
+    TEST(isEqual(q_canonical,q_canonical_ref));
+
     // non-unit quaternion invese
     Quatf qI(1.0f, 0.0f, 0.0f, 0.0f);
     Quatf q_nonunit(0.1f, 0.2f, 0.3f, 0.4f);