From 0cd088ec569c42e185f1d2ef53d3b38ce0c69bce Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Sun, 26 Jan 2025 17:09:13 +0200
Subject: [PATCH] Fix scaling in quaternion-to-matrix conversion

---
 source/geometry/quaternion.h | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/source/geometry/quaternion.h b/source/geometry/quaternion.h
index 8a43342..f94e63b 100644
--- a/source/geometry/quaternion.h
+++ b/source/geometry/quaternion.h
@@ -206,12 +206,13 @@ LinAl::Vector<T, 3> Quaternion<T>::rotate(const LinAl::Vector<T, 3> &v) const
 template<typename T>
 LinAl::Matrix<T, 3, 3> Quaternion<T>::to_matrix() const
 {
-	T s = 2*(a*a+b*b+c*c+d*d);
+	T n = a*a+b*b+c*c+d*d;
+	T s = n>std::numeric_limits<T>::epsilon()*4 ? T(2)/(n) : T();
 	LinAl::Vector<T, 3> columns[3] =
 	{
-		{ 1-s*(c*c+d*d), s*(b*c+a*d), s*(b*d-a*c) },
-		{ s*(b*c-a*d), 1-s*(b*b+d*d), s*(c*d+a*b) },
-		{ s*(b*d+a*c), s*(c*d-a*b), 1-s*(b*b+c*c) }
+		{ T(1)-s*(c*c+d*d), s*(b*c+a*d), s*(b*d-a*c) },
+		{ s*(b*c-a*d), T(1)-s*(b*b+d*d), s*(c*d+a*b) },
+		{ s*(b*d+a*c), s*(c*d-a*b), T(1)-s*(b*b+c*c) }
 	};
 	return LinAl::Matrix<T, 3, 3>::from_columns(columns);
 }
-- 
2.45.2