From: Mikko Rasa <tdb@tdb.fi>
Date: Sun, 26 Jan 2025 15:09:52 +0000 (+0200)
Subject: Add a function to convert a matrix into a quaternion
X-Git-Url: https://git.tdb.fi/?a=commitdiff_plain;h=b6468cb3cab2253294bde436cbf753ea08aead54;p=libs%2Fmath.git

Add a function to convert a matrix into a quaternion

Currently it only accepts orthonormal matrices.
---

diff --git a/source/geometry/quaternion.h b/source/geometry/quaternion.h
index f94e63b..2e15c1a 100644
--- a/source/geometry/quaternion.h
+++ b/source/geometry/quaternion.h
@@ -23,6 +23,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> &);
+	static Quaternion from_matrix(const LinAl::Matrix<T, 3, 3> &);
 
 	T scalar() const { return a; }
 	LinAl::Vector<T, 3> vector() const { return LinAl::Vector<T, 3>(b, c, d); }
@@ -63,6 +64,44 @@ Quaternion<T> Quaternion<T>::rotation(const LinAl::Vector<T, 3> &from, const Lin
 	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>::from_matrix(const LinAl::Matrix<T, 3, 3> &matrix)
+{
+	if(!is_orthonormal(matrix))
+		throw std::domain_error("Quaternion::from_matrix");
+
+	using std::sqrt;
+
+	T xx = matrix(0, 0);
+	T yy = matrix(1, 1);
+	T zz = matrix(2, 2);
+	T t = xx+yy+zz;
+	if(t>T())
+	{
+		T r = sqrt(T(1)+t);
+		T s = T(1)/(T(2)*r);  // Equal to 1/(4*a)
+		return Quaternion(r/T(2), (matrix(2, 1)-matrix(1, 2))*s, (matrix(0, 2)-matrix(2, 0))*s, (matrix(1, 0)-matrix(0, 1))*s);
+	}
+	else if(xx>yy && xx>zz)
+	{
+		T r = sqrt(T(1)+xx-yy-zz);
+		T s = T(1)/(T(2)*r);  // Equal to 1/(4*b)
+		return Quaternion((matrix(2, 1)-matrix(1, 2))*s, r/T(2), (matrix(0, 1)+matrix(1, 0))*s, (matrix(0, 2)+matrix(2, 0))*s);
+	}
+	else if(yy>zz)
+	{
+		T r = sqrt(T(1)-xx+yy-zz);
+		T s = T(1)/(T(2)*r);  // Equal to 1/(4*c)
+		return Quaternion((matrix(0, 2)-matrix(2, 0))*s, (matrix(0, 1)+matrix(1, 0))*s, r/T(2), (matrix(1, 2)+matrix(2, 1))*s);
+	}
+	else
+	{
+		T r = sqrt(T(1)-xx-yy+zz);
+		T s = T(1)/(T(2)*r);  // Equal to 1/(4*d)
+		return Quaternion((matrix(1, 0)-matrix(0, 1))*s, (matrix(0, 2)+matrix(2, 0))*s, (matrix(1, 2)+matrix(2, 1))*s, r/T(2));
+	}
+}
+
 template<typename T>
 Quaternion<T> &Quaternion<T>::operator+=(const Quaternion<T> &q)
 {
diff --git a/source/linal/matrix.h b/source/linal/matrix.h
index 9f13ae9..4811a56 100644
--- a/source/linal/matrix.h
+++ b/source/linal/matrix.h
@@ -300,6 +300,31 @@ inline Matrix<T, N, M> transpose(const Matrix<T, M, N> &m)
 	return r;
 }
 
+template<typename T, unsigned S>
+inline bool is_orthonormal(const Matrix<T, S, S> &m, T epsilon = std::numeric_limits<T>::epsilon()*4)
+{
+	using std::abs;
+	for(unsigned j=0; j<S; ++j)
+	{
+		T l = T();
+		for(unsigned i=0; i<S; ++i)
+			l += m.element(j, i)*m.element(j, i);
+		if(abs(l-T(1))>epsilon*T(2))
+			return false;
+
+		for(unsigned k=0; k<j; ++k)
+		{
+			T p = T();
+			for(unsigned i=0; i<S; ++i)
+				p += m.element(j, i)*m.element(k, i);
+			if(abs(p)>epsilon)
+				return false;
+		}
+	}
+
+	return true;
+}
+
 template<typename T, unsigned M, unsigned N>
 inline std::ostream &operator<<(std::ostream &s, const Matrix<T, M, N> &m)
 {