void QuaternionTest::matrix() {
Vector3 axis = Vector3(-3.0f, 1.0f, 5.0f).normalized();
Quaternion q = Quaternion::rotation(Deg(37.0f), axis);
Matrix3x3 m = Matrix4::rotation(Deg(37.0f), axis).rotationScaling();
/* Verify that negated quaternion gives the same rotation */
CORRADE_COMPARE(q.toMatrix(), m);
CORRADE_COMPARE((-q).toMatrix(), m);
std::ostringstream o;
Error::setOutput(&o);
Quaternion::fromMatrix(m*2);
CORRADE_COMPARE(o.str(), "Math::Quaternion::fromMatrix(): the matrix is not orthogonal\n");
/* Trace > 0 */
CORRADE_VERIFY(m.trace() > 0.0f);
CORRADE_COMPARE(Quaternion::fromMatrix(m), q);
/* Trace < 0, max is diagonal[2] */
Matrix3x3 m2 = Matrix4::rotation(Deg(130.0f), axis).rotationScaling();
Quaternion q2 = Quaternion::rotation(Deg(130.0f), axis);
CORRADE_VERIFY(m2.trace() < 0.0f);
CORRADE_VERIFY(m2.diagonal()[2] > std::max(m2.diagonal()[0], m2.diagonal()[1]));
CORRADE_COMPARE(Quaternion::fromMatrix(m2), q2);
/* Trace < 0, max is diagonal[1] */
Vector3 axis2 = Vector3(-3.0f, 5.0f, 1.0f).normalized();
Matrix3x3 m3 = Matrix4::rotation(Deg(130.0f), axis2).rotationScaling();
Quaternion q3 = Quaternion::rotation(Deg(130.0f), axis2);
CORRADE_VERIFY(m3.trace() < 0.0f);
CORRADE_VERIFY(m3.diagonal()[1] > std::max(m3.diagonal()[0], m3.diagonal()[2]));
CORRADE_COMPARE(Quaternion::fromMatrix(m3), q3);
/* Trace < 0, max is diagonal[0] */
Vector3 axis3 = Vector3(5.0f, -3.0f, 1.0f).normalized();
Matrix3x3 m4 = Matrix4::rotation(Deg(130.0f), axis3).rotationScaling();
Quaternion q4 = Quaternion::rotation(Deg(130.0f), axis3);
CORRADE_VERIFY(m4.trace() < 0.0f);
CORRADE_VERIFY(m4.diagonal()[0] > std::max(m4.diagonal()[1], m4.diagonal()[2]));
CORRADE_COMPARE(Quaternion::fromMatrix(m4), q4);
}
void Quaternion::set(const Matrix3x3& rotation)
{
float trace = rotation.trace();
if(trace > 0.0f)
{
float s = 0.f / sqrt(trace + 1.0f);
w = 0.25f / s;
x = (rotation[2].y - rotation[1].z) * s;
y = (rotation[0].z - rotation[2].x) * s;
z = (rotation[1].x - rotation[0].y) * s;
}
else if((rotation[0].x > rotation[1].y) && (rotation[0].x > rotation[2].z))
{
float s = sqrt(rotation[0].x - rotation[1].y - rotation[2].z + 1.0f) * 2;
float invS = 1.0f / s;
w = (rotation[2].y - rotation[1].z) * invS;
x = 0.25f * s;
y = (rotation[0].y + rotation[1].x) * invS;
z = (rotation[0].z + rotation[2].x) * invS;
}
else if(rotation[1].y > rotation[2].z)
{
float s = sqrt(rotation[1].y - rotation[0].x - rotation[2].z + 1.0f) * 2;
float invS = 1.0f / s;
w = (rotation[0].z - rotation[2].x) * invS;
x = (rotation[0].y + rotation[1].x) * invS;
y = 0.25f * s;
z = (rotation[1].y + rotation[2].y) * invS;
}
else
{
float s = sqrt(rotation[2].z - rotation[0].x - rotation[1].y + 1.0f) * 2;
float invS = 1.0f / s;
w = (rotation[1].x - rotation[0].x) * invS;
x = (rotation[0].z + rotation[2].x) * invS;
y = (rotation[1].z + rotation[2].y) * invS;
z = 0.25f * s;
}
}
