// https://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors?s_tid=mwa_osa_a
// https://github.com/mrdoob/three.js/blob/09cfc67a3f52aeb4dd0009921d82396fd5dc5172/src/math/Quaternion.js#L199-L272
public BabylonQuaternion toQuaternion(EulerRotationOrder rotationOrder = EulerRotationOrder.XYZ)
{
BabylonQuaternion quaternion = new BabylonQuaternion();
var c1 = Math.Cos(0.5 * this.X);
var c2 = Math.Cos(0.5 * this.Y);
var c3 = Math.Cos(0.5 * this.Z);
var s1 = Math.Sin(0.5 * this.X);
var s2 = Math.Sin(0.5 * this.Y);
var s3 = Math.Sin(0.5 * this.Z);
switch (rotationOrder)
{
case EulerRotationOrder.XYZ:
quaternion.X = (float)(s1 * c2 * c3 + c1 * s2 * s3);
quaternion.Y = (float)(c1 * s2 * c3 - s1 * c2 * s3);
quaternion.Z = (float)(c1 * c2 * s3 + s1 * s2 * c3);
quaternion.W = (float)(c1 * c2 * c3 - s1 * s2 * s3);
break;
case EulerRotationOrder.YZX:
quaternion.X = (float)(s1 * c2 * c3 + c1 * s2 * s3);
quaternion.Y = (float)(c1 * s2 * c3 + s1 * c2 * s3);
quaternion.Z = (float)(c1 * c2 * s3 - s1 * s2 * c3);
quaternion.W = (float)(c1 * c2 * c3 - s1 * s2 * s3);
break;
case EulerRotationOrder.ZXY:
quaternion.X = (float)(s1 * c2 * c3 - c1 * s2 * s3);
quaternion.Y = (float)(c1 * s2 * c3 + s1 * c2 * s3);
quaternion.Z = (float)(c1 * c2 * s3 + s1 * s2 * c3);
quaternion.W = (float)(c1 * c2 * c3 - s1 * s2 * s3);
break;
case EulerRotationOrder.XZY:
quaternion.X = (float)(s1 * c2 * c3 - c1 * s2 * s3);
quaternion.Y = (float)(c1 * s2 * c3 - s1 * c2 * s3);
quaternion.Z = (float)(c1 * c2 * s3 + s1 * s2 * c3);
quaternion.W = (float)(c1 * c2 * c3 + s1 * s2 * s3);
break;
case EulerRotationOrder.YXZ:
quaternion.X = (float)(s1 * c2 * c3 + c1 * s2 * s3);
quaternion.Y = (float)(c1 * s2 * c3 - s1 * c2 * s3);
quaternion.Z = (float)(c1 * c2 * s3 - s1 * s2 * c3);
quaternion.W = (float)(c1 * c2 * c3 + s1 * s2 * s3);
break;
case EulerRotationOrder.ZYX:
quaternion.X = (float)(s1 * c2 * c3 - c1 * s2 * s3);
quaternion.Y = (float)(c1 * s2 * c3 + s1 * c2 * s3);
quaternion.Z = (float)(c1 * c2 * s3 - s1 * s2 * c3);
quaternion.W = (float)(c1 * c2 * c3 + s1 * s2 * s3);
break;
}
return(quaternion);
}
/// <summary>
/// Return a quaternion as Euler angles using the supplied rotation order (ZXY corresponds to Quaternion.eulerAngles)
/// </summary>
/// <param name="q">
/// A <see cref="Quaternion"/>
/// </param>
/// <param name="order">
/// A <see cref="EulerRotationOrder"/>
/// </param>
/// <returns>
/// A <see cref="Vector3"/>
/// </returns>
public static Vector3 ToEulerAngles(Quaternion q, EulerRotationOrder order)
{
FloatMatrix rotationMatrix = new FloatMatrix(new Vector3[3] {
q *Vector3.right, q *Vector3.up, q *Vector3.forward
});
pitchAngle = Mathf.Rad2Deg * Mathf.Asin(pitchScalars[(int)order] * rotationMatrix[yawAxes[(int)order], rollAxes[(int)order]]);
if (pitchAngle < 90f)
{
if (pitchAngle > -90f)
{
yawAngle = Mathf.Rad2Deg * Mathf.Atan2(-pitchScalars[(int)order] * rotationMatrix[pitchAxes[(int)order], rollAxes[(int)order]], rotationMatrix[rollAxes[(int)order], rollAxes[(int)order]]);
rollAngle = Mathf.Rad2Deg * Mathf.Atan2(-pitchScalars[(int)order] * rotationMatrix[yawAxes[(int)order], pitchAxes[(int)order]], rotationMatrix[yawAxes[(int)order], yawAxes[(int)order]]);
}
else
{
// non-unique solution
rollAngle = 0f;
yawAngle = rollAngle - Mathf.Rad2Deg * Mathf.Atan2(pitchScalars[(int)order] * rotationMatrix[pitchAxes[(int)order], yawAxes[(int)order]], rotationMatrix[pitchAxes[(int)order], pitchAxes[(int)order]]);
}
}
else
{
// non-unique solution
rollAngle = 0f;
yawAngle = Mathf.Rad2Deg * Mathf.Atan2(pitchScalars[(int)order] * rotationMatrix[pitchAxes[(int)order], yawAxes[(int)order]], rotationMatrix[pitchAxes[(int)order], pitchAxes[(int)order]]) - rollAngle;
}
// pack the angles into a vector
Vector3 ret = Vector3.zero;
ret[rollAxes[(int)order]] = rollAngle;
ret[yawAxes[(int)order]] = yawAngle;
ret[pitchAxes[(int)order]] = pitchAngle;
// return the result
return(ret);
}
/// <summary>
/// Return a quaternion corresponding to the supplied Euler angles with the given order (ZXY corresponds to Quaternion.eulerAngles)
/// </summary>
/// <param name="eulerAngles">
/// A <see cref="Vector3"/>
/// </param>
/// <param name="order">
/// A <see cref="EulerRotationOrder"/>
/// </param>
/// <returns>
/// A <see cref="Quaternion"/>
/// </returns>
public static Quaternion FromEulerAngles(Vector3 eulerAngles, EulerRotationOrder order)
{
// build rotation matrices
rotationMatrices[0][1,1] = Mathf.Cos(Mathf.Deg2Rad*eulerAngles.x);
rotationMatrices[0][2,2] = rotationMatrices[0][1,1];
rotationMatrices[0][1,2] = Mathf.Sin(Mathf.Deg2Rad*eulerAngles.x);
rotationMatrices[0][2,1] = -rotationMatrices[0][1,2];
rotationMatrices[1][2,2] = Mathf.Cos(Mathf.Deg2Rad*eulerAngles.y);
rotationMatrices[1][0,0] = rotationMatrices[1][2,2];
rotationMatrices[1][2,0] = Mathf.Sin(Mathf.Deg2Rad*eulerAngles.y);
rotationMatrices[1][0,2] = -rotationMatrices[1][2,0];
rotationMatrices[2][0,0] = Mathf.Cos(Mathf.Deg2Rad*eulerAngles.z);
rotationMatrices[2][1,1] = rotationMatrices[2][0,0];
rotationMatrices[2][0,1] = Mathf.Sin(Mathf.Deg2Rad*eulerAngles.z);
rotationMatrices[2][1,0] = -rotationMatrices[2][0,1];
// composite rotation matrices
FloatMatrix m = rotationMatrices[yawAxes[(int)order]]*rotationMatrices[pitchAxes[(int)order]]*rotationMatrices[rollAxes[(int)order]];
// build quaternion; see Shoemake, Ken (1987) "Quaternion Calculus and Fast Animation"
float trace = m.trace;
float root = 0f;
Vector4 components = Vector4.zero;
if (trace > 0f)
{
// |w| > 0.5f, may as well choose w > 0.5f;
root = Mathf.Sqrt(trace+1f);
components.w = 0.5f*root;
root = 0.5f/root;
components.x = (m[2,1]-m[1,2])*root;
components.y = (m[0,2]-m[2,0])*root;
components.z = (m[1,0]-m[0,1])*root;
}
else
{
// |w| <= 0.5f
int[] iNext = new int[3] { 1, 2, 0 };
int i = 0;
if (m[1,1] > m[0,0]) i = 1;
if (m[2,2] > m[i,i]) i = 2;
int j = iNext[i];
int k = iNext[j];
root = Mathf.Sqrt(m[i,i]-m[j,j]-m[k,k] + 1f);
components[i] = 0.5f*root;
root = 0.5f/root;
components[3] = (m[k,j]-m[j,k])*root;
components[j] = (m[j,i]+m[i,j])*root;
components[k] = (m[k,i]+m[i,k])*root;
}
// ensure result is left-handed
return new Quaternion(components.x, components.y, components.z, -components.w);
}
/// <summary>
/// Return a quaternion as Euler angles using the supplied rotation order (ZXY corresponds to Quaternion.eulerAngles)
/// </summary>
/// <param name="q">
/// A <see cref="Quaternion"/>
/// </param>
/// <param name="order">
/// A <see cref="EulerRotationOrder"/>
/// </param>
/// <returns>
/// A <see cref="Vector3"/>
/// </returns>
public static Vector3 ToEulerAngles(Quaternion q, EulerRotationOrder order)
{
FloatMatrix rotationMatrix = new FloatMatrix(new Vector3[3] { q*Vector3.right, q*Vector3.up, q*Vector3.forward });
pitchAngle = Mathf.Rad2Deg*Mathf.Asin(pitchScalars[(int)order]*rotationMatrix[yawAxes[(int)order],rollAxes[(int)order]]);
if (pitchAngle < 90f)
{
if (pitchAngle > -90f)
{
yawAngle = Mathf.Rad2Deg*Mathf.Atan2(-pitchScalars[(int)order]*rotationMatrix[pitchAxes[(int)order],rollAxes[(int)order]],rotationMatrix[rollAxes[(int)order],rollAxes[(int)order]]);
rollAngle = Mathf.Rad2Deg*Mathf.Atan2(-pitchScalars[(int)order]*rotationMatrix[yawAxes[(int)order],pitchAxes[(int)order]],rotationMatrix[yawAxes[(int)order],yawAxes[(int)order]]);
}
else
{
// non-unique solution
rollAngle = 0f;
yawAngle = rollAngle - Mathf.Rad2Deg*Mathf.Atan2(pitchScalars[(int)order]*rotationMatrix[pitchAxes[(int)order],yawAxes[(int)order]],rotationMatrix[pitchAxes[(int)order],pitchAxes[(int)order]]);
}
}
else
{
// non-unique solution
rollAngle = 0f;
yawAngle = Mathf.Rad2Deg*Mathf.Atan2(pitchScalars[(int)order]*rotationMatrix[pitchAxes[(int)order],yawAxes[(int)order]],rotationMatrix[pitchAxes[(int)order],pitchAxes[(int)order]]) - rollAngle;
}
// pack the angles into a vector
Vector3 ret = Vector3.zero;
ret[rollAxes[(int)order]] = rollAngle;
ret[yawAxes[(int)order]] = yawAngle;
ret[pitchAxes[(int)order]] = pitchAngle;
// return the result
return ret;
}
/// <summary>
/// Return a quaternion corresponding to the supplied Euler angles with the given order (ZXY corresponds to Quaternion.eulerAngles)
/// </summary>
/// <param name="eulerAngles">
/// A <see cref="Vector3"/>
/// </param>
/// <param name="order">
/// A <see cref="EulerRotationOrder"/>
/// </param>
/// <returns>
/// A <see cref="Quaternion"/>
/// </returns>
public static Quaternion FromEulerAngles(Vector3 eulerAngles, EulerRotationOrder order)
{
// build rotation matrices
rotationMatrices[0][1, 1] = Mathf.Cos(Mathf.Deg2Rad * eulerAngles.x);
rotationMatrices[0][2, 2] = rotationMatrices[0][1, 1];
rotationMatrices[0][1, 2] = Mathf.Sin(Mathf.Deg2Rad * eulerAngles.x);
rotationMatrices[0][2, 1] = -rotationMatrices[0][1, 2];
rotationMatrices[1][2, 2] = Mathf.Cos(Mathf.Deg2Rad * eulerAngles.y);
rotationMatrices[1][0, 0] = rotationMatrices[1][2, 2];
rotationMatrices[1][2, 0] = Mathf.Sin(Mathf.Deg2Rad * eulerAngles.y);
rotationMatrices[1][0, 2] = -rotationMatrices[1][2, 0];
rotationMatrices[2][0, 0] = Mathf.Cos(Mathf.Deg2Rad * eulerAngles.z);
rotationMatrices[2][1, 1] = rotationMatrices[2][0, 0];
rotationMatrices[2][0, 1] = Mathf.Sin(Mathf.Deg2Rad * eulerAngles.z);
rotationMatrices[2][1, 0] = -rotationMatrices[2][0, 1];
// composite rotation matrices
FloatMatrix m = rotationMatrices[yawAxes[(int)order]] * rotationMatrices[pitchAxes[(int)order]] * rotationMatrices[rollAxes[(int)order]];
// build quaternion; see Shoemake, Ken (1987) "Quaternion Calculus and Fast Animation"
float trace = m.trace;
float root = 0f;
Vector4 components = Vector4.zero;
if (trace > 0f)
{
// |w| > 0.5f, may as well choose w > 0.5f;
root = Mathf.Sqrt(trace + 1f);
components.w = 0.5f * root;
root = 0.5f / root;
components.x = (m[2, 1] - m[1, 2]) * root;
components.y = (m[0, 2] - m[2, 0]) * root;
components.z = (m[1, 0] - m[0, 1]) * root;
}
else
{
// |w| <= 0.5f
int[] iNext = new int[3] {
1, 2, 0
};
int i = 0;
if (m[1, 1] > m[0, 0])
{
i = 1;
}
if (m[2, 2] > m[i, i])
{
i = 2;
}
int j = iNext[i];
int k = iNext[j];
root = Mathf.Sqrt(m[i, i] - m[j, j] - m[k, k] + 1f);
components[i] = 0.5f * root;
root = 0.5f / root;
components[3] = (m[k, j] - m[j, k]) * root;
components[j] = (m[j, i] + m[i, j]) * root;
components[k] = (m[k, i] + m[i, k]) * root;
}
// ensure result is left-handed
return(new Quaternion(components.x, components.y, components.z, -components.w));
}
