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); }