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