/// Converts a rotation matrix to a unit Quaternion
Quatd to_Quat(const Matrix3d& m)
{
const unsigned X = 0, Y = 1, Z = 2;
// core computation
Quatd q;
q.w = std::sqrt(std::max(0.0, 1.0 + m(X,X) + m(Y,Y) + m(Z,Z))) * 0.5;
q.x = std::sqrt(std::max(0.0, 1.0 + m(X,X) - m(Y,Y) - m(Z,Z))) * 0.5;
q.y = std::sqrt(std::max(0.0, 1.0 - m(X,X) + m(Y,Y) - m(Z,Z))) * 0.5;
q.z = std::sqrt(std::max(0.0, 1.0 - m(X,X) - m(Y,Y) + m(Z,Z))) * 0.5;
// sign computation
if (m(Z,Y) - m(Y,Z) < 0.0)
q.x = -q.x;
if (m(X,Z) - m(Z,X) < 0.0)
q.y = -q.y;
if (m(Y,X) - m(X,Y) < 0.0)
q.z = -q.z;
#ifndef NEXCEPT
if (!q.unit())
std::cerr << "Quatd::set() warning! not a unit quaternion!" << std::endl;
#endif
return q;
}
/// Sets quaternion to that represented by an axis-angle representation
Quatd to_Quat(const AAngled& a)
{
const double half = a.angle*0.5;
double sina = std::sin(half);
Quatd q;
q.x = a.x * sina;
q.y = a.y * sina;
q.z = a.z * sina;
q.w = std::cos(half);
#ifndef NEXCEPT
if (!q.unit())
std::cerr << "Quatd::set() warning! not a unit quaternion!" << std::endl;
#endif
return q;
}