public override void WriteJson(JsonWriter writer, object value, JsonSerializer serializer)
{
var mat = (Matrix4d)value;
writer.WriteStartObject();
writer.WriteStartArray();
writer.WritePropertyName("rotation");
writer.WriteValue(Matrix3d.CreateFromQuaternion(mat.ExtractRotation()));
writer.WritePropertyName("position");
writer.WriteValue(mat.ExtractTranslation());
writer.WriteEndObject();
}
/// <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);
}
