public Quaternion FromMatrix(Matrix4 m)
{
float diag = m.GetM(0,0) + m.GetM(1,1) + m.GetM(2,2) + 1;
float scale = 0.0f;
if (diag > 0.0f)
{
scale = (float)Math.Sqrt(diag) * 2.0f; // get scale from diagonal
// TODO: speed this up
m_x = (m.GetM(2,1) - m.GetM(1,2)) / scale;
m_y = (m.GetM(0,2) - m.GetM(2,0)) / scale;
m_z = (m.GetM(1,0) - m.GetM(0,1)) / scale;
m_w = 0.25f * scale;
}
else
{
if (m.GetM(0,0) > m.GetM(1,1) && m.GetM(0,0) > m.GetM(2,2))
{
// 1st element of diag is greatest value
// find scale according to 1st element, and double it
scale = (float)Math.Sqrt(1.0f + m.GetM(0,0) - m.GetM(1,1) - m.GetM(2,2)) * 2.0f;
// TODO: speed this up
m_x = 0.25f * scale;
m_y = (m.GetM(0,1) + m.GetM(1,0)) / scale;
m_z = (m.GetM(2,0) + m.GetM(0,2)) / scale;
m_w = (m.GetM(2,1) - m.GetM(1,2)) / scale;
}
else if (m.GetM(1,1) > m.GetM(2,2))
{
// 2nd element of diag is greatest value
// find scale according to 2nd element, and double it
scale = (float)Math.Sqrt(1.0f + m.GetM(1,1) - m.GetM(0,0) - m.GetM(2,2)) * 2.0f;
// TODO: speed this up
m_x = (m.GetM(0,1) + m.GetM(1,0)) / scale;
m_y = 0.25f * scale;
m_z = (m.GetM(1,2) + m.GetM(2,1)) / scale;
m_w = (m.GetM(0,2) - m.GetM(2,0)) / scale;
}
else
{
// 3rd element of diag is greatest value
// find scale according to 3rd element, and double it
scale = (float)Math.Sqrt(1.0f + m.GetM(2,2) - m.GetM(0,0) - m.GetM(1,1)) * 2.0f;
// TODO: speed this up
m_x = (m.GetM(0,2) + m.GetM(2,0)) / scale;
m_y = (m.GetM(1,2) + m.GetM(2,1)) / scale;
m_z = 0.25f * scale;
m_w = (m.GetM(1,0) - m.GetM(0,1)) / scale;
}
}
Normalize();
return this;
}