Fix scaling in quaternion-to-matrix conversion

diff --git a/source/geometry/quaternion.h b/source/geometry/quaternion.h
index 8a43342ec4910cca172f298976dca42bd686ba44..f94e63b781a904f7e5247ec2e5aa53dff1bde324 100644 (file)
--- 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);
 }