public static MQuaternion AngleAxis(double aAngle, MVector3 aAxis)
{
aAxis = aAxis.Normalize();
double rad = aAngle * Math.PI / 180 * 0.5;
aAxis = aAxis.Multiply(Math.Sin(rad));
return(new MQuaternion(aAxis.X, aAxis.Y, aAxis.Z, Math.Cos(rad)));
}
/// <summary>
/// Creates euler angles (in degree) form the given quaternion
/// Source code from: https://stackoverflow.com/questions/12088610/conversion-between-euler-quaternion-like-in-unity3d-engine
/// </summary>
/// <param name="q"></param>
/// <returns></returns>
public static MVector3 ToEuler(MQuaternion q)
{
MVector3 euler = new MVector3();
// if the input quaternion is normalized, this is exactly one. Otherwise, this acts as a correction factor for the quaternion's not-normalizedness
double unit = (q.X * q.X) + (q.Y * q.Y) + (q.Z * q.Z) + (q.W * q.W);
// this will have a magnitude of 0.5 or greater if and only if this is a singularity case
double test = q.X * q.W - q.Y * q.Z;
if (test > 0.4995f * unit) // singularity at north pole
{
euler.X = Math.PI / 2;
euler.Y = 2f * Math.Atan2(q.Y, q.X);
euler.Z = 0;
}
else if (test < -0.4995f * unit) // singularity at south pole
{
euler.X = -Math.PI / 2;
euler.Y = -2f * Math.Atan2(q.Y, q.X);
euler.Z = 0;
}
else // no singularity - this is the majority of cases
{
euler.X = Math.Asin(2f * (q.W * q.X - q.Y * q.Z));
euler.Y = Math.Atan2(2f * q.W * q.Y + 2f * q.Z * q.X, 1 - 2f * (q.X * q.X + q.Y * q.Y));
euler.Z = Math.Atan2(2f * q.W * q.Z + 2f * q.X * q.Y, 1 - 2f * (q.Z * q.Z + q.X * q.X));
}
// all the math so far has been done in radians. Before returning, we convert to degrees...
euler = euler.Multiply(Rad2Deg);
//...and then ensure the degree values are between 0 and 360
euler.X %= 360;
euler.Y %= 360;
euler.Z %= 360;
return(euler);
}