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));
-        } else {
-            return Quaternion(q(0),q(1),q(2),q(3));
+
+        for (size_t i = 0; i < 4; i++) {
+            if (fabs(q(i)) > FLT_EPSILON) {
+                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