Quaternion/Vector: Small refactor for review: put more comments, switched type conversions, took out default destination vector because confusing

Author: MaEtUgR

matrix/Quaternion.hpp

@@ -189,18 +189,19 @@ class Quaternion : public Vector<Type, 4>
     /**
      * Quaternion from two vectors
      * Generates shortest rotation from source to destination vector
-     * Default destination if not specified is [0,0,1]
      *
      * @param dst destination vector (no need to normalize)
      * @param src source vector (no need to normalize)
      * @param eps epsilon threshold which decides if a value is considered zero
      */
-    Quaternion(const Vector3<Type> &src, const Vector<Type, 3> dst = Vector3<Type>(0, 0, 1), const Type eps = 1e-5f) :
+    Quaternion(const Vector3<Type> &src, const Vector3<Type> &dst, const Type eps = Type(1e-5)) :
         Vector<Type, 4>()
     {
         Quaternion &q = *this;
         Vector3<Type> cr = src.cross(dst);
         float dt = src.dot(dst);
+        /* If the two vectors are parallel, cross product is zero
+         * If they point opposite, the dot product is negative */
         if (cr.norm() < eps && dt < 0) {
             cr = src.abs();
             if (cr(0) < cr(1)) {
@@ -219,6 +220,7 @@ class Quaternion : public Vector<Type, 4>
             q(0) = Type(0);
             cr = src.cross(cr);
         } else {
+            /* Half-Way Quaternion Solution */
             q(0) = src.dot(dst) + sqrt(src.norm_squared() * dst.norm_squared());
         }
         q(1) = cr(0);

matrix/Vector.hpp

@@ -88,7 +88,7 @@ class Vector : public Matrix<Type, M, 1>
         return (*this) / norm();
     }
 
-    Vector unit_or_zero(const Type eps = 1e-5f) {
+    Vector unit_or_zero(const Type eps = Type(1e-5)) {
         const Type n = norm();
         if (n > eps) {
             return (*this) / n;

test/attitude.cpp

@@ -40,12 +40,11 @@ int main()
     TEST(fabs(q(3) - 4) < eps);
 
     // quaternion ctor: vector to vector
-    Vector3f v1(0.f, 0.f, 1.f);
-    // identity & default destination vector test
-    Quatf quat_v(v1);
-    TEST(isEqual(quat_v.conjugate(v1), v1));
+    // identity test
+    Quatf quat_v(v,v);
+    TEST(isEqual(quat_v.conjugate(v), v));
     // random test (vector norm can not be preserved with a pure rotation)
-    v1 = Vector3f(-80.1f, 1.5f, -6.89f);
+    Vector3f v1(-80.1f, 1.5f, -6.89f);
     quat_v = Quatf(v1, v);
     TEST(isEqual(quat_v.conjugate(v1).normalized() * v.norm(), v));
     // special 180 degree case 1