Code: Select all
public static Vector GetYPRFromQuaternion_OutRadians(Quaternion rotation)
{
Vector res = new Vector();
double q0 = rotation.W;
double q1 = rotation.Y;
double q2 = rotation.X;
double q3 = rotation.Z;
res.X = Math.Atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1*q1 + q2*q2));
res.Y = Math.Asin(2 * (q0 * q2 - q3 * q1));
res.Z = Math.Atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3*q3));
return res;
}