[3.x] C#: Add Basis Euler angle code to match 4.x core
This commit is contained in:
Aaron Franke
parent
cbde08d452
commit
fdce2a1dfc
1 changed files with 352 additions and 23 deletions
|
|
@ -8,6 +8,21 @@ using System.Runtime.InteropServices;
|
||||||
|
|
||||||
namespace Godot
|
namespace Godot
|
||||||
{
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 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).
|
||||||
|
/// </summary>
|
||||||
|
public enum EulerOrder
|
||||||
|
{
|
||||||
|
XYZ,
|
||||||
|
XZY,
|
||||||
|
YXZ,
|
||||||
|
YZX,
|
||||||
|
ZXY,
|
||||||
|
ZYX
|
||||||
|
};
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 3×3 matrix used for 3D rotation and scale.
|
/// 3×3 matrix used for 3D rotation and scale.
|
||||||
/// Almost always used as an orthogonal basis for a Transform.
|
/// 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 returned vector contains the rotation angles in
|
||||||
/// the format (X angle, Y angle, Z angle).
|
/// the format (X angle, Y angle, Z angle).
|
||||||
///
|
///
|
||||||
/// Consider using the <see cref="Quat()"/> method instead, which
|
/// Consider using the <see cref="RotationQuat"/> method instead, which
|
||||||
/// returns a <see cref="Godot.Quat"/> quaternion instead of Euler angles.
|
/// returns a <see cref="Godot.Quat"/> quaternion instead of Euler angles.
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <returns>A <see cref="Vector3"/> representing the basis rotation in Euler angles.</returns>
|
/// <returns>A <see cref="Vector3"/> representing the basis rotation in Euler angles.</returns>
|
||||||
public Vector3 GetEuler()
|
public Vector3 GetEuler()
|
||||||
{
|
{
|
||||||
Basis m = Orthonormalized();
|
return GetEuler(EulerOrder.YXZ);
|
||||||
|
}
|
||||||
|
|
||||||
Vector3 euler;
|
/// <summary>
|
||||||
euler.z = 0.0f;
|
/// Returns the basis's rotation in the form of Euler angles.
|
||||||
|
/// The Euler order depends on the [param order] parameter,
|
||||||
real_t mzy = m.Row1[2];
|
/// for example using the YXZ convention: when decomposing,
|
||||||
|
/// first Z, then X, and Y last. The returned vector contains
|
||||||
if (mzy < 1.0f)
|
/// the rotation angles in the format (X angle, Y angle, Z angle).
|
||||||
|
///
|
||||||
|
/// Consider using the <see cref="RotationQuat"/> method instead, which
|
||||||
|
/// returns a <see cref="Godot.Quat"/> quaternion instead of Euler angles.
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="order">The Euler order to use.</param>
|
||||||
|
/// <returns>A <see cref="Vector3"/> representing the basis rotation in Euler angles.</returns>
|
||||||
|
public Vector3 GetEuler(EulerOrder order)
|
||||||
|
{
|
||||||
|
switch (order)
|
||||||
{
|
{
|
||||||
if (mzy > -1.0f)
|
case EulerOrder.XYZ:
|
||||||
{
|
{
|
||||||
euler.x = Mathf.Asin(-mzy);
|
// Euler angles in XYZ convention.
|
||||||
euler.y = Mathf.Atan2(m.Row0[2], m.Row2[2]);
|
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
|
||||||
euler.z = Mathf.Atan2(m.Row1[0], m.Row1[1]);
|
//
|
||||||
|
// 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 angles in XZY convention.
|
||||||
euler.y = -Mathf.Atan2(-m.Row0[1], m.Row0[0]);
|
// 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;
|
||||||
}
|
}
|
||||||
}
|
case EulerOrder.YXZ:
|
||||||
else
|
{
|
||||||
{
|
// Euler angles in YXZ convention.
|
||||||
euler.x = -Mathf.Pi * 0.5f;
|
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
|
||||||
euler.y = -Mathf.Atan2(-m.Row0[1], m.Row0[0]);
|
//
|
||||||
}
|
// 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));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
|
|
@ -635,7 +873,7 @@ namespace Godot
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Returns the basis's rotation in the form of a quaternion.
|
/// Returns the basis's rotation in the form of a quaternion.
|
||||||
/// See <see cref="GetEuler()"/> if you need Euler angles, but keep in
|
/// See <see cref="GetEuler"/> if you need Euler angles, but keep in
|
||||||
/// mind that quaternions should generally be preferred to Euler angles.
|
/// mind that quaternions should generally be preferred to Euler angles.
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <returns>A <see cref="Godot.Quat"/> representing the basis's rotation.</returns>
|
/// <returns>A <see cref="Godot.Quat"/> representing the basis's rotation.</returns>
|
||||||
|
|
@ -860,6 +1098,62 @@ namespace Godot
|
||||||
Row2 = new Vector3(xz, yz, zz);
|
Row2 = new Vector3(xz, yz, zz);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Constructs a Basis matrix from Euler angles in the specified rotation order. By default, use YXZ order (most common).
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="euler">The Euler angles to use.</param>
|
||||||
|
/// <param name="order">The order to compose the Euler angles.</param>
|
||||||
|
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));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 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.
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="scale">The scale Vector3.</param>
|
||||||
|
/// <returns>A pure scale Basis matrix.</returns>
|
||||||
|
public static Basis FromScale(Vector3 scale)
|
||||||
|
{
|
||||||
|
return new Basis(
|
||||||
|
scale.x, 0, 0,
|
||||||
|
0, scale.y, 0,
|
||||||
|
0, 0, scale.z
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Composes these two basis matrices by multiplying them
|
/// Composes these two basis matrices by multiplying them
|
||||||
/// together. This has the effect of transforming the second basis
|
/// together. This has the effect of transforming the second basis
|
||||||
|
|
@ -878,6 +1172,41 @@ namespace Godot
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Returns a Vector3 transformed (multiplied) by the basis matrix.
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="basis">The basis matrix transformation to apply.</param>
|
||||||
|
/// <param name="vector">A Vector3 to transform.</param>
|
||||||
|
/// <returns>The transformed Vector3.</returns>
|
||||||
|
public static Vector3 operator *(Basis basis, Vector3 vector)
|
||||||
|
{
|
||||||
|
return new Vector3
|
||||||
|
(
|
||||||
|
basis.Row0.Dot(vector),
|
||||||
|
basis.Row1.Dot(vector),
|
||||||
|
basis.Row2.Dot(vector)
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 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.
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="vector">A Vector3 to inversely transform.</param>
|
||||||
|
/// <param name="basis">The basis matrix transformation to apply.</param>
|
||||||
|
/// <returns>The inversely transformed vector.</returns>
|
||||||
|
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
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Returns <see langword="true"/> if the basis matrices are exactly
|
/// Returns <see langword="true"/> if the basis matrices are exactly
|
||||||
/// equal. Note: Due to floating-point precision errors, consider using
|
/// equal. Note: Due to floating-point precision errors, consider using
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue