From fdce2a1dfc2d90571cb6826b009e51ac2b15ca16 Mon Sep 17 00:00:00 2001 From: Aaron Franke Date: Sat, 24 Sep 2022 14:34:00 -0500 Subject: [PATCH] [3.x] C#: Add Basis Euler angle code to match 4.x core --- .../glue/GodotSharp/GodotSharp/Core/Basis.cs | 375 ++++++++++++++++-- 1 file changed, 352 insertions(+), 23 deletions(-) diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs index dce1b11ed74..b776bbe6f7c 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs @@ -8,6 +8,21 @@ using System.Runtime.InteropServices; namespace Godot { + /// + /// Specifies which order Euler angle rotations should be in. + /// When composing, the order is the same as the letters. When decomposing, + /// the order is reversed (ex: YXZ decomposes Z first, then X, and Y last). + /// + public enum EulerOrder + { + XYZ, + XZY, + YXZ, + YZX, + ZXY, + ZYX + }; + /// /// 3×3 matrix used for 3D rotation and scale. /// Almost always used as an orthogonal basis for a Transform. @@ -272,40 +287,263 @@ namespace Godot /// The returned vector contains the rotation angles in /// the format (X angle, Y angle, Z angle). /// - /// Consider using the method instead, which + /// Consider using the method instead, which /// returns a quaternion instead of Euler angles. /// /// A representing the basis rotation in Euler angles. public Vector3 GetEuler() { - Basis m = Orthonormalized(); + return GetEuler(EulerOrder.YXZ); + } - Vector3 euler; - euler.z = 0.0f; - - real_t mzy = m.Row1[2]; - - if (mzy < 1.0f) + /// + /// Returns the basis's rotation in the form of Euler angles. + /// The Euler order depends on the [param order] parameter, + /// for example using the YXZ convention: when decomposing, + /// first Z, then X, and Y last. The returned vector contains + /// the rotation angles in the format (X angle, Y angle, Z angle). + /// + /// Consider using the method instead, which + /// returns a quaternion instead of Euler angles. + /// + /// The Euler order to use. + /// A representing the basis rotation in Euler angles. + public Vector3 GetEuler(EulerOrder order) + { + switch (order) { - if (mzy > -1.0f) + case EulerOrder.XYZ: { - euler.x = Mathf.Asin(-mzy); - euler.y = Mathf.Atan2(m.Row0[2], m.Row2[2]); - euler.z = Mathf.Atan2(m.Row1[0], m.Row1[1]); + // Euler angles in XYZ convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cy*cz -cy*sz sy + // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx + // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy + Vector3 euler; + real_t sy = Row0[2]; + if (sy < (1.0f - Mathf.Epsilon)) + { + if (sy > -(1.0f - Mathf.Epsilon)) + { + // is this a pure Y rotation? + if (Row1[0] == 0 && Row0[1] == 0 && Row1[2] == 0 && Row2[1] == 0 && Row1[1] == 1) + { + // return the simplest form (human friendlier in editor and scripts) + euler.x = 0; + euler.y = Mathf.Atan2(Row0[2], Row0[0]); + euler.z = 0; + } + else + { + euler.x = Mathf.Atan2(-Row1[2], Row2[2]); + euler.y = Mathf.Asin(sy); + euler.z = Mathf.Atan2(-Row0[1], Row0[0]); + } + } + else + { + euler.x = Mathf.Atan2(Row2[1], Row1[1]); + euler.y = -Mathf.Tau / 4.0f; + euler.z = 0.0f; + } + } + else + { + euler.x = Mathf.Atan2(Row2[1], Row1[1]); + euler.y = Mathf.Tau / 4.0f; + euler.z = 0.0f; + } + return euler; } - else + case EulerOrder.XZY: { - euler.x = Mathf.Pi * 0.5f; - euler.y = -Mathf.Atan2(-m.Row0[1], m.Row0[0]); + // Euler angles in XZY convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cz*cy -sz cz*sy + // sx*sy+cx*cy*sz cx*cz cx*sz*sy-cy*sx + // cy*sx*sz cz*sx cx*cy+sx*sz*sy + Vector3 euler; + real_t sz = Row0[1]; + if (sz < (1.0f - Mathf.Epsilon)) + { + if (sz > -(1.0f - Mathf.Epsilon)) + { + euler.x = Mathf.Atan2(Row2[1], Row1[1]); + euler.y = Mathf.Atan2(Row0[2], Row0[0]); + euler.z = Mathf.Asin(-sz); + } + else + { + // It's -1 + euler.x = -Mathf.Atan2(Row1[2], Row2[2]); + euler.y = 0.0f; + euler.z = Mathf.Tau / 4.0f; + } + } + else + { + // It's 1 + euler.x = -Mathf.Atan2(Row1[2], Row2[2]); + euler.y = 0.0f; + euler.z = -Mathf.Tau / 4.0f; + } + return euler; } - } - else - { - euler.x = -Mathf.Pi * 0.5f; - euler.y = -Mathf.Atan2(-m.Row0[1], m.Row0[0]); - } + case EulerOrder.YXZ: + { + // Euler angles in YXZ convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy + // cx*sz cx*cz -sx + // cy*sx*sz-cz*sy cy*cz*sx+sy*sz cy*cx + Vector3 euler; + real_t m12 = Row1[2]; + if (m12 < (1 - Mathf.Epsilon)) + { + if (m12 > -(1 - Mathf.Epsilon)) + { + // is this a pure X rotation? + if (Row1[0] == 0 && Row0[1] == 0 && Row0[2] == 0 && Row2[0] == 0 && Row0[0] == 1) + { + // return the simplest form (human friendlier in editor and scripts) + euler.x = Mathf.Atan2(-m12, Row1[1]); + euler.y = 0; + euler.z = 0; + } + else + { + euler.x = Mathf.Asin(-m12); + euler.y = Mathf.Atan2(Row0[2], Row2[2]); + euler.z = Mathf.Atan2(Row1[0], Row1[1]); + } + } + else + { // m12 == -1 + euler.x = Mathf.Tau / 4.0f; + euler.y = Mathf.Atan2(Row0[1], Row0[0]); + euler.z = 0; + } + } + else + { // m12 == 1 + euler.x = -Mathf.Tau / 4.0f; + euler.y = -Mathf.Atan2(Row0[1], Row0[0]); + euler.z = 0; + } - return euler; + return euler; + } + case EulerOrder.YZX: + { + // Euler angles in YZX convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx + // sz cz*cx -cz*sx + // -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx + Vector3 euler; + real_t sz = Row1[0]; + if (sz < (1.0f - Mathf.Epsilon)) + { + if (sz > -(1.0f - Mathf.Epsilon)) + { + euler.x = Mathf.Atan2(-Row1[2], Row1[1]); + euler.y = Mathf.Atan2(-Row2[0], Row0[0]); + euler.z = Mathf.Asin(sz); + } + else + { + // It's -1 + euler.x = Mathf.Atan2(Row2[1], Row2[2]); + euler.y = 0.0f; + euler.z = -Mathf.Tau / 4.0f; + } + } + else + { + // It's 1 + euler.x = Mathf.Atan2(Row2[1], Row2[2]); + euler.y = 0.0f; + euler.z = Mathf.Tau / 4.0f; + } + return euler; + } + case EulerOrder.ZXY: + { + // Euler angles in ZXY convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx + // cy*sz+cz*sx*sy cz*cx sz*sy-cz*cy*sx + // -cx*sy sx cx*cy + Vector3 euler; + real_t sx = Row2[1]; + if (sx < (1.0f - Mathf.Epsilon)) + { + if (sx > -(1.0f - Mathf.Epsilon)) + { + euler.x = Mathf.Asin(sx); + euler.y = Mathf.Atan2(-Row2[0], Row2[2]); + euler.z = Mathf.Atan2(-Row0[1], Row1[1]); + } + else + { + // It's -1 + euler.x = -Mathf.Tau / 4.0f; + euler.y = Mathf.Atan2(Row0[2], Row0[0]); + euler.z = 0; + } + } + else + { + // It's 1 + euler.x = Mathf.Tau / 4.0f; + euler.y = Mathf.Atan2(Row0[2], Row0[0]); + euler.z = 0; + } + return euler; + } + case EulerOrder.ZYX: + { + // Euler angles in ZYX convention. + // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix + // + // rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*cy + // cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx + // -sy cy*sx cy*cx + Vector3 euler; + real_t sy = Row2[0]; + if (sy < (1.0f - Mathf.Epsilon)) + { + if (sy > -(1.0f - Mathf.Epsilon)) + { + euler.x = Mathf.Atan2(Row2[1], Row2[2]); + euler.y = Mathf.Asin(-sy); + euler.z = Mathf.Atan2(Row1[0], Row0[0]); + } + else + { + // It's -1 + euler.x = 0; + euler.y = Mathf.Tau / 4.0f; + euler.z = -Mathf.Atan2(Row0[1], Row1[1]); + } + } + else + { + // It's 1 + euler.x = 0; + euler.y = -Mathf.Tau / 4.0f; + euler.z = -Mathf.Atan2(Row0[1], Row1[1]); + } + return euler; + } + default: + throw new ArgumentOutOfRangeException(nameof(order)); + } } /// @@ -635,7 +873,7 @@ namespace Godot /// /// Returns the basis's rotation in the form of a quaternion. - /// See if you need Euler angles, but keep in + /// See if you need Euler angles, but keep in /// mind that quaternions should generally be preferred to Euler angles. /// /// A representing the basis's rotation. @@ -860,6 +1098,62 @@ namespace Godot Row2 = new Vector3(xz, yz, zz); } + /// + /// Constructs a Basis matrix from Euler angles in the specified rotation order. By default, use YXZ order (most common). + /// + /// The Euler angles to use. + /// The order to compose the Euler angles. + public static Basis FromEuler(Vector3 euler, EulerOrder order = EulerOrder.YXZ) + { + real_t c, s; + + c = Mathf.Cos(euler.x); + s = Mathf.Sin(euler.x); + Basis xmat = new Basis(new Vector3(1, 0, 0), new Vector3(0, c, s), new Vector3(0, -s, c)); + + c = Mathf.Cos(euler.y); + s = Mathf.Sin(euler.y); + Basis ymat = new Basis(new Vector3(c, 0, -s), new Vector3(0, 1, 0), new Vector3(s, 0, c)); + + c = Mathf.Cos(euler.z); + s = Mathf.Sin(euler.z); + Basis zmat = new Basis(new Vector3(c, s, 0), new Vector3(-s, c, 0), new Vector3(0, 0, 1)); + + switch (order) + { + case EulerOrder.XYZ: + return xmat * ymat * zmat; + case EulerOrder.XZY: + return xmat * zmat * ymat; + case EulerOrder.YXZ: + return ymat * xmat * zmat; + case EulerOrder.YZX: + return ymat * zmat * xmat; + case EulerOrder.ZXY: + return zmat * xmat * ymat; + case EulerOrder.ZYX: + return zmat * ymat * xmat; + default: + throw new ArgumentOutOfRangeException(nameof(order)); + } + } + + /// + /// Constructs a pure scale basis matrix with no rotation or shearing. + /// The scale values are set as the main diagonal of the matrix, + /// and all of the other parts of the matrix are zero. + /// + /// The scale Vector3. + /// A pure scale Basis matrix. + public static Basis FromScale(Vector3 scale) + { + return new Basis( + scale.x, 0, 0, + 0, scale.y, 0, + 0, 0, scale.z + ); + } + /// /// Composes these two basis matrices by multiplying them /// together. This has the effect of transforming the second basis @@ -878,6 +1172,41 @@ namespace Godot ); } + /// + /// Returns a Vector3 transformed (multiplied) by the basis matrix. + /// + /// The basis matrix transformation to apply. + /// A Vector3 to transform. + /// The transformed Vector3. + public static Vector3 operator *(Basis basis, Vector3 vector) + { + return new Vector3 + ( + basis.Row0.Dot(vector), + basis.Row1.Dot(vector), + basis.Row2.Dot(vector) + ); + } + + /// + /// Returns a Vector3 transformed (multiplied) by the transposed basis matrix. + /// + /// Note: This results in a multiplication by the inverse of the + /// basis matrix only if it represents a rotation-reflection. + /// + /// A Vector3 to inversely transform. + /// The basis matrix transformation to apply. + /// The inversely transformed vector. + public static Vector3 operator *(Vector3 vector, Basis basis) + { + return new Vector3 + ( + basis.Row0[0] * vector.x + basis.Row1[0] * vector.y + basis.Row2[0] * vector.z, + basis.Row0[1] * vector.x + basis.Row1[1] * vector.y + basis.Row2[1] * vector.z, + basis.Row0[2] * vector.x + basis.Row1[2] * vector.y + basis.Row2[2] * vector.z + ); + } + /// /// Returns if the basis matrices are exactly /// equal. Note: Due to floating-point precision errors, consider using