From f835d390171ae61de977d00cdaf5ed16e5361e4e Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Mon, 4 Dec 2017 13:27:58 +0100
Subject: [PATCH] Quaternion/Vector: Small refactor for review: put more
 comments, switched type conversions, took out default destination vector
 because confusing

---
 matrix/Quaternion.hpp | 6 ++++--
 matrix/Vector.hpp     | 4 ++--
 test/attitude.cpp     | 9 ++++-----
 3 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
index 891835f1f1..4961fab5ee 100644
--- a/matrix/Quaternion.hpp
+++ b/matrix/Quaternion.hpp
@@ -189,18 +189,19 @@ public:
     /**
      * 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 @@ public:
             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);
diff --git a/matrix/Vector.hpp b/matrix/Vector.hpp
index fc937c2a42..a1058f2361 100644
--- a/matrix/Vector.hpp
+++ b/matrix/Vector.hpp
@@ -73,7 +73,7 @@ public:
 
     Type norm_squared() const {
         const Vector &a(*this);
-        return Type(a.dot(a));
+        return a.dot(a);
     }
 
     inline Type length() const {
@@ -88,7 +88,7 @@ public:
         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;
diff --git a/test/attitude.cpp b/test/attitude.cpp
index 7758ac3717..6cb1d8dd70 100644
--- a/test/attitude.cpp
+++ b/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