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