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