public static float FindQuaternionTwist(this Quaternion quat, Vector3 axis) { axis.Normalize(); // Get the plane the axis is a normal of Vector3 orthoNormal1, orthoNormal2; WMath.FindOrthoNormals(axis, out orthoNormal1, out orthoNormal2); Vector3 transformed = Vector3.Transform(orthoNormal1, quat); // Project transformed vector onto a plane Vector3 flattened = transformed - (Vector3.Dot(transformed, axis) * axis); flattened.Normalize(); // Get the angle between the original vector and projected transform to get angle around normal float a = (float)Math.Acos((double)Vector3.Dot(orthoNormal1, flattened)); return(WMath.RadiansToDegrees(a)); }
/// <summary> /// Convert a Quaternion to all possible ways it can be represented as Euler Angles. /// Returns the angles in [-180, 180] space in degrees. /// </summary> public static List <Vector3> ToEulerAnglesRobust(this Quaternion quat, string rotationOrder) { var representations = new List <Vector3>(); var qx = quat.X; var qy = quat.Y; var qz = quat.Z; var qw = quat.W; // Convert the quaternion to a 3x3 matrix. // We don't use OpenTK's Matrix3 class because it stores the values as single-precision floats, which loses information. // By manually calculating the matrix elements as doubles, we can get the maximum amount of accuracy. double m11 = 1.0 - 2.0 * (qy * qy + qz * qz); double m12 = 2.0 * (qx * qy - qz * qw); double m13 = 2.0 * (qx * qz + qy * qw); double m21 = 2.0 * (qx * qy + qz * qw); double m22 = 1.0 - 2.0 * (qx * qx + qz * qz); double m23 = 2.0 * (qy * qz - qx * qw); double m31 = 2.0 * (qx * qz - qy * qw); double m32 = 2.0 * (qy * qz + qx * qw); double m33 = 1.0 - 2.0 * (qx * qx + qy * qy); double x, y, z; switch (rotationOrder) { case "ZYX": y = Math.Asin(-Math.Min(1, Math.Max(-1, m31))); if (Math.Abs(m31) < 0.999999) { x = Math.Atan2(m32, m33); z = Math.Atan2(m21, m11); } else { x = Math.Atan2(-m12, m22); z = 0f; } representations.Add(new Vector3( WMath.RadiansToDegrees((float)x), WMath.RadiansToDegrees((float)y), WMath.RadiansToDegrees((float)z) )); y = CopySign(Math.PI, y) - y; x = x - CopySign(Math.PI, x); z = z - CopySign(Math.PI, z); representations.Add(new Vector3( WMath.RadiansToDegrees((float)x), WMath.RadiansToDegrees((float)y), WMath.RadiansToDegrees((float)z) )); break; case "YXZ": x = Math.Asin(-Math.Min(1, Math.Max(-1, m23))); if (Math.Abs(m23) < 0.999999) { y = Math.Atan2(m13, m33); z = Math.Atan2(m21, m22); } else { y = Math.Atan2(-m31, m11); z = 0f; } representations.Add(new Vector3( WMath.RadiansToDegrees((float)x), WMath.RadiansToDegrees((float)y), WMath.RadiansToDegrees((float)z) )); x = CopySign(Math.PI, x) - x; y = y - CopySign(Math.PI, y); z = z - CopySign(Math.PI, z); representations.Add(new Vector3( WMath.RadiansToDegrees((float)x), WMath.RadiansToDegrees((float)y), WMath.RadiansToDegrees((float)z) )); break; default: throw new NotImplementedException($"Quaternion to euler rotation conversion not implemented for rotation order: {rotationOrder}"); } return(representations); }
/// <summary> /// Convert a Quaternion to Euler Angles. Returns the angles in [-180, 180] space in degrees. /// </summary> /// <typeparam name="T"></typeparam> /// <param name="quat"></param> /// <returns></returns> public static Vector3 ToEulerAngles(this Quaternion quat) { return(new Vector3(WMath.RadiansToDegrees(PitchFromQuat(quat)), WMath.RadiansToDegrees(YawFromQuat(quat)), WMath.RadiansToDegrees(RollFromQuat(quat)))); }
/// <summary> /// Convert a Quaternion to all possible ways it can be represented as Euler Angles. /// Returns the angles in [-180, 180] space in degrees. /// </summary> public static List <Vector3> ToEulerAnglesRobust(this Quaterniond quat, string rotationOrder) { var representations = new List <Vector3>(); var qx = quat.X; var qy = quat.Y; var qz = quat.Z; var qw = quat.W; var mat = Matrix3d.CreateFromQuaternion(quat).Inverted(); double x, y, z; switch (rotationOrder) { case "ZYX": y = Math.Asin(-Math.Min(1, Math.Max(-1, mat.M31))); if (Math.Abs(mat.M31) < 0.999999) { x = Math.Atan2(mat.M32, mat.M33); z = Math.Atan2(mat.M21, mat.M11); } else { x = Math.Atan2(-mat.M12, mat.M22); z = 0f; } representations.Add(new Vector3( WMath.RadiansToDegrees((float)x), WMath.RadiansToDegrees((float)y), WMath.RadiansToDegrees((float)z) )); y = CopySign(Math.PI, y) - y; x = x - CopySign(Math.PI, x); z = z - CopySign(Math.PI, z); representations.Add(new Vector3( WMath.RadiansToDegrees((float)x), WMath.RadiansToDegrees((float)y), WMath.RadiansToDegrees((float)z) )); break; case "YXZ": x = Math.Asin(-Math.Min(1, Math.Max(-1, mat.M23))); if (Math.Abs(mat.M23) < 0.999999) { y = Math.Atan2(mat.M13, mat.M33); z = Math.Atan2(mat.M21, mat.M22); } else { y = Math.Atan2(-mat.M31, mat.M11); z = 0f; } representations.Add(new Vector3( WMath.RadiansToDegrees((float)x), WMath.RadiansToDegrees((float)y), WMath.RadiansToDegrees((float)z) )); x = CopySign(Math.PI, x) - x; y = y - CopySign(Math.PI, y); z = z - CopySign(Math.PI, z); representations.Add(new Vector3( WMath.RadiansToDegrees((float)x), WMath.RadiansToDegrees((float)y), WMath.RadiansToDegrees((float)z) )); break; default: throw new NotImplementedException($"Quaternion to euler rotation conversion not implemented for rotation order: {rotationOrder}"); } return(representations); }