From 89fe5cbfc6986d740b759066c0de7781826505ab Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Sat, 28 Jan 2023 21:45:31 +0200
Subject: [PATCH] Add a function to create a rotation quaternion from two
 vectors

---
 source/geometry/quaternion.h | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/source/geometry/quaternion.h b/source/geometry/quaternion.h
index ef5504e..8a43342 100644
--- 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)
 {
-- 
2.45.2