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