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 f94e63b781a904f7e5247ec2e5aa53dff1bde324..2e15c1acf57c2a390296112aad7856cd09334dd0 100644 (file)
--- 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 9f13ae952f7110b2d27c6272dd584971cc038fcb..4811a56f74858da1abc71aeab6c20224053de305 100644 (file)
--- 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)
 {