Add a function to create a rotation quaternion from two vectors

diff --git a/source/geometry/quaternion.h b/source/geometry/quaternion.h
index ef5504e5c33390aeb921fad4c12ff9fd3cae518a..8a43342ec4910cca172f298976dca42bd686ba44 100644 (file)
--- a/source/geometry/quaternion.h
+++ b/source/geometry/quaternion.h
@@ -22,6 +22,7 @@ struct Quaternion
 
        static Quaternion one() { return Quaternion(1, 0, 0, 0); }
        static Quaternion rotation(Angle<T>, const LinAl::Vector<T, 3> &);
+       static Quaternion rotation(const LinAl::Vector<T, 3> &, const LinAl::Vector<T, 3> &);
 
        T scalar() const { return a; }
        LinAl::Vector<T, 3> vector() const { return LinAl::Vector<T, 3>(b, c, d); }
@@ -51,6 +52,17 @@ Quaternion<T> Quaternion<T>::rotation(Angle<T> angle, const LinAl::Vector<T, 3>
        return Quaternion<T>(c, s*axis.x, s*axis.y, s*axis.z);
 }
 
+template<typename T>
+Quaternion<T> Quaternion<T>::rotation(const LinAl::Vector<T, 3> &from, const LinAl::Vector<T, 3> &to)
+{
+       using std::sqrt;
+
+       // https://www.xarg.org/proof/quaternion-from-two-vectors/
+       LinAl::Vector<T, 3> axis = cross(from, to);
+       T c = inner_product(from, to);
+       return Quaternion<T>(c+sqrt(inner_product(axis, axis)+c*c), axis.x, axis.y, axis.z).normalize();
+}
+
 template<typename T>
 Quaternion<T> &Quaternion<T>::operator+=(const Quaternion<T> &q)
 {