public static NitroxQuaternion ExtractRotation(ref NitroxMatrix4x4 matrix)
{
NitroxQuaternion q = new NitroxQuaternion(0f, 0f, 0f, 1f);
float trace = matrix[0, 0] + matrix[1, 1] + matrix[2, 2];
if (trace > 0)
{
float s = 0.5f / (float)Math.Sqrt(trace + 1.0f);
q.W = 0.25f / s;
q.X = (matrix[2, 1] - matrix[1, 2]) * s;
q.Y = (matrix[0, 2] - matrix[2, 0]) * s;
q.Z = (matrix[1, 0] - matrix[0, 1]) * s;
}
else
{
if (matrix[0, 0] > matrix[1, 1] && matrix[0, 0] > matrix[2, 2])
{
float s = 2.0f * (float)Math.Sqrt(1.0f + matrix[0, 0] - matrix[1, 1] - matrix[2, 2]);
q.W = (matrix[2, 1] - matrix[1, 2]) / s;
q.X = 0.25f * s;
q.Y = (matrix[0, 1] + matrix[1, 0]) / s;
q.Z = (matrix[0, 2] + matrix[2, 0]) / s;
}
else if (matrix[1, 1] > matrix[2, 2])
{
float s = 2.0f * (float)Math.Sqrt(1.0f + matrix[1, 1] - matrix[0, 0] - matrix[2, 2]);
q.W = (matrix[0, 2] - matrix[2, 0]) / s;
q.X = (matrix[0, 1] + matrix[1, 0]) / s;
q.Y = 0.25f * s;
q.Z = (matrix[1, 2] + matrix[2, 1]) / s;
}
else
{
float s = 2.0f * (float)Math.Sqrt(1.0f + matrix[2, 2] - matrix[0, 0] - matrix[1, 1]);
q.W = (matrix[1, 0] - matrix[0, 1]) / s;
q.W = (matrix[0, 2] + matrix[2, 0]) / s;
q.Y = (matrix[1, 2] + matrix[2, 1]) / s;
q.Z = 0.25f * s;
}
}
NitroxQuaternion.Normalize(q);
return(q);
}
public static NitroxMatrix4x4 SetRotation(NitroxQuaternion localRotation)
{
NitroxQuaternion rot = NitroxQuaternion.Normalize(localRotation);
NitroxMatrix4x4 rotationMatrix;
rotationMatrix.M = new float[4, 4];
rotationMatrix[3, 3] = 1f;
float sqw = rot.W * rot.W;
float sqx = rot.X * rot.X;
float sqy = rot.Y * rot.Y;
float sqz = rot.Z * rot.Z;
float invs = 1 / (sqx + sqy + sqz + sqw);
rotationMatrix[0, 0] = (sqx - sqy - sqz + sqw) * invs;
rotationMatrix[1, 1] = (-sqx + sqy - sqz + sqw) * invs;
rotationMatrix[2, 2] = (-sqx - sqy + sqz + sqw) * invs;
float tmp1 = rot.X * rot.Y;
float tmp2 = rot.Z * rot.W;
rotationMatrix[1, 0] = 2 * (tmp1 + tmp2) * invs;
rotationMatrix[0, 1] = 2 * (tmp1 - tmp2) * invs;
tmp1 = rot.X * rot.Z;
tmp2 = rot.Y * rot.W;
rotationMatrix[2, 0] = 2 * (tmp1 - tmp2) * invs;
rotationMatrix[0, 2] = 2 * (tmp1 + tmp2) * invs;
tmp1 = rot.Y * rot.Z;
tmp2 = rot.X * rot.W;
rotationMatrix[2, 1] = 2 * (tmp1 + tmp2) * invs;
rotationMatrix[1, 2] = 2 * (tmp1 - tmp2) * invs;
return(rotationMatrix);
}