Quaternion: Shortest rotation between two vectors

matrix/Quaternion.hpp

@@ -186,6 +186,49 @@ class Quaternion : public Vector<Type, 4>
         }
     }
 
+    /**
+     * Quaternion from two vectors
+     * Generates shortest rotation from source to destination vector
+     *
+     * @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 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)) {
+                if (cr(0) < cr(2)) {
+                    cr = Vector3<Type>(1, 0, 0);
+                } else {
+                    cr = Vector3<Type>(0, 0, 1);
+                }
+            } else {
+                if (cr(1) < cr(2)) {
+                    cr = Vector3<Type>(0, 1, 0);
+                } else {
+                    cr = Vector3<Type>(0, 0, 1);
+                }
+            }
+            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);
+        q(2) = cr(1);
+        q(3) = cr(2);
+        q.normalize();
+    }
+
 
     /**
      * Constructor from quaternion values

matrix/Vector.hpp

@@ -71,6 +71,11 @@ class Vector : public Matrix<Type, M, 1>
         return Type(sqrt(a.dot(a)));
     }
 
+    Type norm_squared() const {
+        const Vector &a(*this);
+        return a.dot(a);
+    }
+
     inline Type length() const {
         return norm();
     }
@@ -83,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

@@ -27,10 +27,7 @@ int main()
     TEST(isEqual(e, e));
 
     // euler vector ctor
-    Vector<float, 3> v;
-    v(0) = 0.1f;
-    v(1) = 0.2f;
-    v(2) = 0.3f;
+    Vector3f v(0.1f, 0.2f, 0.3f);
     Eulerf euler_copy(v);
     TEST(isEqual(euler_copy, euler_check));
 
@@ -42,6 +39,32 @@ int main()
     TEST(fabs(q(2) - 3) < eps);
     TEST(fabs(q(3) - 4) < eps);
 
+    // quaternion ctor: vector to vector
+    // 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)
+    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
+    v1 = Vector3f(0.f, 1.f, 1.f);
+    quat_v = Quatf(v1, -v1);
+    TEST(isEqual(quat_v.conjugate(v1), -v1));
+    // special 180 degree case 2
+    v1 = Vector3f(1.f, 2.f, 0.f);
+    quat_v = Quatf(v1, -v1);
+    TEST(isEqual(quat_v.conjugate(v1), -v1));
+    // special 180 degree case 3
+    v1 = Vector3f(0.f, 0.f, 1.f);
+    quat_v = Quatf(v1, -v1);
+    TEST(isEqual(quat_v.conjugate(v1), -v1));
+    // special 180 degree case 4
+    v1 = Vector3f(1.f, 1.f, 1.f);
+    quat_v = Quatf(v1, -v1);
+    TEST(isEqual(quat_v.conjugate(v1), -v1));
+
+
     // quat normalization
     q.normalize();
     TEST(isEqual(q, Quatf(0.18257419f,  0.36514837f,
@@ -297,7 +320,7 @@ int main()
     TEST(isEqual(Dcmf(q), q.to_dcm()));
 
     // conjugate
-    Vector3f v1(1.5f, 2.2f, 3.2f);
+    v = Vector3f(1.5f, 2.2f, 3.2f);
     TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q).T()*v1));
     TEST(isEqual(q.conjugate(v1), Dcmf(q)*v1));
 

test/vector.cpp

@@ -18,6 +18,7 @@ int main()
 
     // norm, dot product
     TEST(isEqualF(v1.norm(), 7.416198487095663f));
+    TEST(isEqualF(v1.norm_squared(), v1.norm() * v1.norm()));
     TEST(isEqualF(v1.norm(), v1.length()));
     TEST(isEqualF(v1.dot(v2), 130.0f));
     TEST(isEqualF(v1.dot(v2), v1 * v2));