Canonical Quaternion with tests (#81)

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
      *

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);