/// <summary> /// Creates and initializes a new quaternion from the given matrix. /// </summary> /// <param name="m">The rotation matrix to initialize from.</param> public FQuat(FMatrix m) { // If Matrix is NULL, return Identity quaternion. If any of them is 0, you won't be able to construct rotation // if you have two plane at least, we can reconstruct the frame using cross product, but that's a bit expensive op to do here // for now, if you convert to matrix from 0 scale and convert back, you'll lose rotation. Don't do that. if (m.GetScaledAxis(EAxis.X).IsNearlyZero() || m.GetScaledAxis(EAxis.Y).IsNearlyZero() || m.GetScaledAxis(EAxis.Z).IsNearlyZero()) { this = FQuat.Identity; return; } //#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST) // Make sure the Rotation part of the Matrix is unit length. // Changed to this (same as RemoveScaling) from RotDeterminant as using two different ways of checking unit length matrix caused inconsistency. if (!FMessage.Ensure( (FMath.Abs(1.0f - m.GetScaledAxis(EAxis.X).SizeSquared()) <= FMath.KindaSmallNumber) && (FMath.Abs(1.0f - m.GetScaledAxis(EAxis.Y).SizeSquared()) <= FMath.KindaSmallNumber) && (FMath.Abs(1.0f - m.GetScaledAxis(EAxis.Z).SizeSquared()) <= FMath.KindaSmallNumber) , "Make sure the Rotation part of the Matrix is unit length.")) { this = FQuat.Identity; return; } //#endif //const MeReal *const t = (MeReal *) tm; float s; // Check diagonal (trace) float tr = m[0, 0] + m[1, 1] + m[2, 2]; if (tr > 0.0f) { float invS = FMath.InvSqrt(tr + 1.0f); this.W = 0.5f * (1.0f / invS); s = 0.5f * invS; this.X = (m[1, 2] - m[2, 1]) * s; this.Y = (m[2, 0] - m[0, 2]) * s; this.Z = (m[0, 1] - m[1, 0]) * s; } else { // diagonal is negative int i = 0; if (m[1, 1] > m[0, 0]) { i = 1; } if (m[2, 2] > m[i, i]) { i = 2; } int[] nxt = { 1, 2, 0 }; int j = nxt[i]; int k = nxt[j]; s = m[i, i] - m[j, j] - m[k, k] + 1.0f; float InvS = FMath.InvSqrt(s); float[] qt = new float[4]; qt[i] = 0.5f * (1.0f / InvS); s = 0.5f * InvS; qt[3] = (m[j, k] - m[k, j]) * s; qt[j] = (m[i, j] + m[j, i]) * s; qt[k] = (m[i, k] + m[k, i]) * s; this.X = qt[0]; this.Y = qt[1]; this.Z = qt[2]; this.W = qt[3]; DiagnosticCheckNaN(); } }