/// <summary>
/// Returns the rotation of this <see cref="Quaternion"/> in euler angles
/// </summary>
public Vector3 ToEuler()
{
/*float sinX = 2 * (this.W * this.X + this.Y * this.Z);
* float cosX = 1 - 2 * (this.X * this.X + this.Y * this.Y);
* float sinY = 2 * (this.W * this.Y - this.Z * this.X);
* float sinZ = 2 * (this.W * this.Z + this.X * this.Y);
* float cosZ = 1 - 2 * (this.Y * this.Y + this.Z * this.Z);
*
* float y = 0;
* if(Math.Abs(sinY) >= 1)
* {
* if(sinY < 0)
* {
* y = -(float)(Math.PI / 2);
* }
* else
* {
* y = (float)(Math.PI / 2);
* }
* }
* else
* {
* y = (float)Math.Asin(sinY);
* }
*
* return new Vector3()
* {
* X = (float)Math.Round(Utils.ToDegrees((float)Math.Atan2(sinX, cosX)), 4),
* Y = (float)Math.Round(Utils.ToDegrees(y), 4),
* Z = (float)Math.Round(Utils.ToDegrees((float)Math.Atan2(sinZ, cosZ)), 4)
* };*/
R3DMatrix44 m = R3DMatrix44.FromRotation(this);
float sy = (float)Math.Sqrt(m.M11 * m.M11 + m.M21 * m.M21);
bool singular = sy < 1e-6;
float x = 0;
float y = 0;
float z = 0;
if (!singular)
{
x = (float)Math.Round(Utilities.ToDegrees((float)Math.Atan2(m.M32, m.M33)), 4);
y = (float)Math.Round(Utilities.ToDegrees((float)Math.Atan2(-m.M31, sy)), 4);
z = (float)Math.Round(Utilities.ToDegrees((float)Math.Atan2(m.M21, m.M11)), 4);
}
else
{
x = (float)Math.Round(Utilities.ToDegrees((float)Math.Atan2(-m.M23, m.M22)), 4);
y = (float)Math.Round(Utilities.ToDegrees((float)Math.Atan2(-m.M31, sy)), 4);
z = 0;
}
return(new Vector3(x, y, z));
}
public static Vector3 FromRotationMatrix(R3DMatrix44 m)
{
float x = 0;
float y = Utilities.Clamp(m.M13, -1, 1);
float z = 0;
return(new Vector3()
{
X = (float)Math.Round(Utilities.ToDegrees(x), 4),
Y = (float)Math.Round(Utilities.ToDegrees(x), 4),
Z = (float)Math.Round(Utilities.ToDegrees(x), 4)
});
}
/// <summary>
/// Returns a <see cref="Quaternion"/> that represents the rotation of the given matrix
/// </summary>
public static Quaternion FromTransformationMatrix(R3DMatrix44 matrix)
{
Quaternion result = new Quaternion(0, 0, 0, 0);
float sqrt;
float half;
float scale = matrix.M11 + matrix.M22 + matrix.M33;
if (scale > 0.0f)
{
sqrt = (float)Math.Sqrt(scale + 1.0f);
result.W = sqrt * 0.5f;
sqrt = 0.5f / sqrt;
result.X = (matrix.M23 - matrix.M32) * sqrt;
result.Y = (matrix.M31 - matrix.M13) * sqrt;
result.Z = (matrix.M12 - matrix.M21) * sqrt;
}
else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
{
sqrt = (float)Math.Sqrt(1.0f + matrix.M11 - matrix.M22 - matrix.M33);
half = 0.5f / sqrt;
result.X = 0.5f * sqrt;
result.Y = (matrix.M12 + matrix.M21) * half;
result.Z = (matrix.M13 + matrix.M31) * half;
result.W = (matrix.M23 - matrix.M32) * half;
}
else if (matrix.M22 > matrix.M33)
{
sqrt = (float)Math.Sqrt(1.0f + matrix.M22 - matrix.M11 - matrix.M33);
half = 0.5f / sqrt;
result.X = (matrix.M21 + matrix.M12) * half;
result.Y = 0.5f * sqrt;
result.Z = (matrix.M32 + matrix.M23) * half;
result.W = (matrix.M31 - matrix.M13) * half;
}
else
{
sqrt = (float)Math.Sqrt(1.0f + matrix.M33 - matrix.M11 - matrix.M22);
half = 0.5f / sqrt;
result.X = (matrix.M31 + matrix.M13) * half;
result.Y = (matrix.M32 + matrix.M23) * half;
result.Z = 0.5f * sqrt;
result.W = (matrix.M12 - matrix.M21) * half;
}
return(result);
}
/// <summary>
/// Creates a clone of a <see cref="R3DMatrix44"/> object
/// </summary>
/// <param name="r3dMatrix44">The <see cref="R3DMatrix44"/> to clone</param>
public R3DMatrix44(R3DMatrix44 r3dMatrix44)
{
this.M11 = r3dMatrix44.M11;
this.M12 = r3dMatrix44.M12;
this.M13 = r3dMatrix44.M13;
this.M14 = r3dMatrix44.M14;
this.M21 = r3dMatrix44.M21;
this.M22 = r3dMatrix44.M22;
this.M23 = r3dMatrix44.M23;
this.M24 = r3dMatrix44.M24;
this.M31 = r3dMatrix44.M31;
this.M32 = r3dMatrix44.M32;
this.M33 = r3dMatrix44.M33;
this.M34 = r3dMatrix44.M34;
this.M41 = r3dMatrix44.M41;
this.M42 = r3dMatrix44.M42;
this.M43 = r3dMatrix44.M43;
this.M44 = r3dMatrix44.M44;
}
