public FixQuaternion(ref FixTrans3 matrix)
{
Fix determinant = (matrix.M11 * (matrix.M22 * matrix.M33 - matrix.M32 * matrix.M23)) -
(matrix.M12 * (matrix.M21 * matrix.M33 - matrix.M31 * matrix.M23)) +
(matrix.M13 * (matrix.M21 * matrix.M32 - matrix.M31 * matrix.M22));
Fix scale = FixMath.Pow(determinant, Fix.One / 3);
Fix x, y, z;
w = (FixMath.Sqrt(FixMath.Max(0, scale + matrix.M11 + matrix.M22 + matrix.M33)) / 2);
x = (FixMath.Sqrt(FixMath.Max(0, scale + matrix.M11 - matrix.M22 - matrix.M33)) / 2);
y = (FixMath.Sqrt(FixMath.Max(0, scale - matrix.M11 + matrix.M22 - matrix.M33)) / 2);
z = (FixMath.Sqrt(FixMath.Max(0, scale - matrix.M11 - matrix.M22 + matrix.M33)) / 2);
xyz = new FixVec3(x, y, z);
if (matrix.M32 - matrix.M23 < 0)
{
X = -X;
}
if (matrix.M13 - matrix.M31 < 0)
{
Y = -Y;
}
if (matrix.M21 - matrix.M12 < 0)
{
Z = -Z;
}
}
public Fix GetMagnitude()
{
Fix sqrMagnitude = GetSqrMagnitude();
if (sqrMagnitude == Fix.Zero)
{
return(Fix.Zero);
}
return(FixMath.Sqrt(sqrMagnitude));
}
public static Fix Angle(FixVec2 from, FixVec2 to)
{
Fix denominator = FixMath.Sqrt(from.GetMagnitudeSquared() * to.GetMagnitudeSquared());
if (denominator < Fix.Epsilon)
{
return(Fix.zero);
}
Fix dot = FixMath.Clamp(FixVec2.Dot(from, to) / denominator, -Fix.one, Fix.one);
return(FixMath.Acos(dot) * FixMath.Rad2Deg);
}
//https://gamedev.stackexchange.com/questions/50963/how-to-extract-euler-angles-from-transformation-matrix
public FixVec3 EulerAngle()
{
FixVec3 ea = new FixVec3();
ea.x = FixMath.Atan2(-m[1, 2], m[2, 2]);
Fix cosYangle = FixMath.Sqrt(FixMath.Pow(m[0, 0], 2) + FixMath.Pow(m[0, 1], 2));
ea.y = FixMath.Atan2(m[0, 2], cosYangle);
Fix sinXangle = FixMath.Sin(ea.x);
Fix cosXangle = FixMath.Cos(ea.x);
ea.z = FixMath.Atan2((cosXangle * m[1, 0]) + (sinXangle * m[2, 0]), (cosXangle * m[1, 1]) + (sinXangle * m[2, 1]));
return(ea);
}
public Fix Magnitude()
{
return(FixMath.Sqrt(x * x + y * y + z * z + w * w));
}
