Catch quaternion canonical corner cases (#116)

matrix/Quaternion.hpp

@@ -367,11 +367,13 @@ public:
     Quaternion canonical() const
     {
         const Quaternion &q = *this;
-        if (q(0) < Type(0)) {
+
-            return Quaternion(-q(0),-q(1),-q(2),-q(3));
+        for (size_t i = 0; i < 4; i++) {
-        } else {
+            if (fabs(q(i)) > FLT_EPSILON) {
-            return Quaternion(q(0),q(1),q(2),q(3));
+                return q * Type(matrix::sign(q(i)));
+            }
         }
+        return q;
     }
 
     /**

matrix/helper_functions.hpp

@@ -78,4 +78,10 @@ Type wrap_2pi(Type x)
     return wrap(x, Type(0), Type(M_TWOPI));
 }
 
+template<typename T>
+int sign(T val)
+{
+    return (T(FLT_EPSILON) < val) - (val < T(FLT_EPSILON));
 }
+
+} // namespace matrix

test/attitude.cpp

@@ -255,15 +255,55 @@ int main()
     TEST(fabs(q_check(3) + q(3)) < eps);
 
     // quaternion canonical
-    Quatf q_non_canonical(-0.7f,0.4f, 0.3f, -0.3f);
+    Quatf q_non_canonical_1(-0.7f,0.4f, 0.3f, -0.3f);
-    Quatf q_canonical(0.7f,-0.4f, -0.3f, 0.3f);
+    Quatf q_canonical_1(0.7f,-0.4f, -0.3f, 0.3f);
-    Quatf q_canonical_ref(0.7f,-0.4f, -0.3f, 0.3f);
+    Quatf q_canonical_ref_1(0.7f,-0.4f, -0.3f, 0.3f);
-    TEST(isEqual(q_non_canonical.canonical(),q_canonical_ref));
+    TEST(isEqual(q_non_canonical_1.canonical(),q_canonical_ref_1));
-    TEST(isEqual(q_canonical.canonical(),q_canonical_ref));
+    TEST(isEqual(q_canonical_1.canonical(),q_canonical_ref_1));
-    q_non_canonical.canonicalize();
+    q_non_canonical_1.canonicalize();
-    q_canonical.canonicalize();
+    q_canonical_1.canonicalize();
-    TEST(isEqual(q_non_canonical,q_canonical_ref));
+    TEST(isEqual(q_non_canonical_1,q_canonical_ref_1));
-    TEST(isEqual(q_canonical,q_canonical_ref));
+    TEST(isEqual(q_canonical_1,q_canonical_ref_1));
+
+    Quatf q_non_canonical_2(0.0f, -1.0f, 0.0f, 0.0f);
+    Quatf q_canonical_2(0.0f, 1.0f, 0.0f, 0.0f);
+    Quatf q_canonical_ref_2(0.0f, 1.0f, 0.0f, 0.0f);
+    TEST(isEqual(q_non_canonical_2.canonical(),q_canonical_ref_2));
+    TEST(isEqual(q_canonical_2.canonical(),q_canonical_ref_2));
+    q_non_canonical_2.canonicalize();
+    q_canonical_2.canonicalize();
+    TEST(isEqual(q_non_canonical_2,q_canonical_ref_2));
+    TEST(isEqual(q_canonical_2,q_canonical_ref_2));
+
+    Quatf q_non_canonical_3(0.0f, 0.0f, -1.0f, 0.0f);
+    Quatf q_canonical_3(0.0f, 0.0f, 1.0f, 0.0f);
+    Quatf q_canonical_ref_3(0.0f, 0.0f, 1.0f, 0.0f);
+    TEST(isEqual(q_non_canonical_3.canonical(),q_canonical_ref_3));
+    TEST(isEqual(q_canonical_3.canonical(),q_canonical_ref_3));
+    q_non_canonical_3.canonicalize();
+    q_canonical_3.canonicalize();
+    TEST(isEqual(q_non_canonical_3,q_canonical_ref_3));
+    TEST(isEqual(q_canonical_3,q_canonical_ref_3));
+
+    Quatf q_non_canonical_4(0.0f, 0.0f, 0.0f, -1.0f);
+    Quatf q_canonical_4(0.0f, 0.0f, 0.0f, 1.0f);
+    Quatf q_canonical_ref_4(0.0f, 0.0f, 0.0f, 1.0f);
+    TEST(isEqual(q_non_canonical_4.canonical(),q_canonical_ref_4));
+    TEST(isEqual(q_canonical_4.canonical(),q_canonical_ref_4));
+    q_non_canonical_4.canonicalize();
+    q_canonical_4.canonicalize();
+    TEST(isEqual(q_non_canonical_4,q_canonical_ref_4));
+    TEST(isEqual(q_canonical_4,q_canonical_ref_4));
+
+    Quatf q_non_canonical_5(0.0f, 0.0f, 0.0f, 0.0f);
+    Quatf q_canonical_5(0.0f, 0.0f, 0.0f, 0.0f);
+    Quatf q_canonical_ref_5(0.0f, 0.0f, 0.0f, 0.0f);
+    TEST(isEqual(q_non_canonical_5.canonical(),q_canonical_ref_5));
+    TEST(isEqual(q_canonical_5.canonical(),q_canonical_ref_5));
+    q_non_canonical_5.canonicalize();
+    q_canonical_5.canonicalize();
+    TEST(isEqual(q_non_canonical_5,q_canonical_ref_5));
+    TEST(isEqual(q_canonical_5,q_canonical_ref_5));
 
     // quaternion setIdentity
     Quatf q_nonIdentity(-0.7f, 0.4f, 0.5f, -0.3f);

test/copyto.cpp

@@ -15,7 +15,7 @@ int main()
     float eps = 1e-6f;
 
     // Vector3 copyTo
-    Vector3f v(1, 2, 3);
+    const Vector3f v(1, 2, 3);
     float dst3[3] = {};
     v.copyTo(dst3);
     for (size_t i = 0; i < 3; i++) {