From 8acd13a456050ded00f0f264ff0aa91a304f6c54 Mon Sep 17 00:00:00 2001 From: Marcel Admiraal Date: Wed, 20 Jan 2021 07:02:02 +0000 Subject: Rename Quat to Quaternion --- .../mono/glue/GodotSharp/GodotSharp/Core/Basis.cs | 66 +-- .../mono/glue/GodotSharp/GodotSharp/Core/Quat.cs | 541 --------------------- .../glue/GodotSharp/GodotSharp/Core/Quaternion.cs | 541 +++++++++++++++++++++ .../glue/GodotSharp/GodotSharp/Core/Transform3D.cs | 12 +- .../glue/GodotSharp/GodotSharp/GodotSharp.csproj | 2 +- 5 files changed, 581 insertions(+), 581 deletions(-) delete mode 100644 modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs create mode 100644 modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs (limited to 'modules/mono/glue/GodotSharp') diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs index 3f1120575f..01525e593f 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs @@ -207,7 +207,7 @@ namespace Godot } } - public Quat RotationQuat() + public Quaternion RotationQuaternion() { Basis orthonormalizedBasis = Orthonormalized(); real_t det = orthonormalizedBasis.Determinant(); @@ -218,18 +218,18 @@ namespace Godot orthonormalizedBasis = orthonormalizedBasis.Scaled(-Vector3.One); } - return orthonormalizedBasis.Quat(); + return orthonormalizedBasis.Quaternion(); } - internal void SetQuatScale(Quat quat, Vector3 scale) + internal void SetQuaternionScale(Quaternion quaternion, Vector3 scale) { SetDiagonal(scale); - Rotate(quat); + Rotate(quaternion); } - private void Rotate(Quat quat) + private void Rotate(Quaternion quaternion) { - this *= new Basis(quat); + this *= new Basis(quaternion); } private void SetDiagonal(Vector3 diagonal) @@ -263,8 +263,8 @@ namespace Godot /// 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. + /// Consider using the method instead, which + /// returns a quaternion instead of Euler angles. /// /// A Vector3 representing the basis rotation in Euler angles. public Vector3 GetEuler() @@ -486,8 +486,8 @@ namespace Godot /// The resulting basis matrix of the interpolation. public Basis Slerp(Basis target, real_t weight) { - Quat from = new Quat(this); - Quat to = new Quat(target); + Quaternion from = new Quaternion(this); + Quaternion to = new Quaternion(target); Basis b = new Basis(from.Slerp(to, weight)); b.Row0 *= Mathf.Lerp(Row0.Length(), target.Row0.Length(), weight); @@ -588,8 +588,8 @@ namespace Godot /// 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. - public Quat Quat() + /// A representing the basis's rotation. + public Quaternion Quaternion() { real_t trace = Row0[0] + Row1[1] + Row2[2]; @@ -597,7 +597,7 @@ namespace Godot { real_t s = Mathf.Sqrt(trace + 1.0f) * 2f; real_t inv_s = 1f / s; - return new Quat( + return new Quaternion( (Row2[1] - Row1[2]) * inv_s, (Row0[2] - Row2[0]) * inv_s, (Row1[0] - Row0[1]) * inv_s, @@ -609,7 +609,7 @@ namespace Godot { real_t s = Mathf.Sqrt(Row0[0] - Row1[1] - Row2[2] + 1.0f) * 2f; real_t inv_s = 1f / s; - return new Quat( + return new Quaternion( s * 0.25f, (Row0[1] + Row1[0]) * inv_s, (Row0[2] + Row2[0]) * inv_s, @@ -621,7 +621,7 @@ namespace Godot { real_t s = Mathf.Sqrt(-Row0[0] + Row1[1] - Row2[2] + 1.0f) * 2f; real_t inv_s = 1f / s; - return new Quat( + return new Quaternion( (Row0[1] + Row1[0]) * inv_s, s * 0.25f, (Row1[2] + Row2[1]) * inv_s, @@ -632,7 +632,7 @@ namespace Godot { real_t s = Mathf.Sqrt(-Row0[0] - Row1[1] + Row2[2] + 1.0f) * 2f; real_t inv_s = 1f / s; - return new Quat( + return new Quaternion( (Row0[2] + Row2[0]) * inv_s, (Row1[2] + Row2[1]) * inv_s, s * 0.25f, @@ -699,23 +699,23 @@ namespace Godot /// /// Constructs a pure rotation basis matrix from the given quaternion. /// - /// The quaternion to create the basis from. - public Basis(Quat quat) + /// The quaternion to create the basis from. + public Basis(Quaternion quaternion) { - real_t s = 2.0f / quat.LengthSquared; + real_t s = 2.0f / quaternion.LengthSquared; - real_t xs = quat.x * s; - real_t ys = quat.y * s; - real_t zs = quat.z * s; - real_t wx = quat.w * xs; - real_t wy = quat.w * ys; - real_t wz = quat.w * zs; - real_t xx = quat.x * xs; - real_t xy = quat.x * ys; - real_t xz = quat.x * zs; - real_t yy = quat.y * ys; - real_t yz = quat.y * zs; - real_t zz = quat.z * zs; + real_t xs = quaternion.x * s; + real_t ys = quaternion.y * s; + real_t zs = quaternion.z * s; + real_t wx = quaternion.w * xs; + real_t wy = quaternion.w * ys; + real_t wz = quaternion.w * zs; + real_t xx = quaternion.x * xs; + real_t xy = quaternion.x * ys; + real_t xz = quaternion.x * zs; + real_t yy = quaternion.y * ys; + real_t yz = quaternion.y * zs; + real_t zz = quaternion.z * zs; Row0 = new Vector3(1.0f - (yy + zz), xy - wz, xz + wy); Row1 = new Vector3(xy + wz, 1.0f - (xx + zz), yz - wx); @@ -727,8 +727,8 @@ namespace Godot /// (in the YXZ convention: when *composing*, first Y, then X, and Z last), /// given in the vector format as (X angle, Y angle, Z angle). /// - /// Consider using the constructor instead, which - /// uses a quaternion instead of Euler angles. + /// Consider using the constructor instead, which + /// uses a quaternion instead of Euler angles. /// /// The Euler angles to create the basis from. public Basis(Vector3 eulerYXZ) diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs deleted file mode 100644 index bd3bcb0c58..0000000000 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs +++ /dev/null @@ -1,541 +0,0 @@ -using System; -using System.Runtime.InteropServices; -#if REAL_T_IS_DOUBLE -using real_t = System.Double; -#else -using real_t = System.Single; -#endif - -namespace Godot -{ - /// - /// A unit quaternion used for representing 3D rotations. - /// Quaternions need to be normalized to be used for rotation. - /// - /// It is similar to Basis, which implements matrix representation of - /// rotations, and can be parametrized using both an axis-angle pair - /// or Euler angles. Basis stores rotation, scale, and shearing, - /// while Quat only stores rotation. - /// - /// Due to its compactness and the way it is stored in memory, certain - /// operations (obtaining axis-angle and performing SLERP, in particular) - /// are more efficient and robust against floating-point errors. - /// - [Serializable] - [StructLayout(LayoutKind.Sequential)] - public struct Quat : IEquatable - { - /// - /// X component of the quaternion (imaginary `i` axis part). - /// Quaternion components should usually not be manipulated directly. - /// - public real_t x; - - /// - /// Y component of the quaternion (imaginary `j` axis part). - /// Quaternion components should usually not be manipulated directly. - /// - public real_t y; - - /// - /// Z component of the quaternion (imaginary `k` axis part). - /// Quaternion components should usually not be manipulated directly. - /// - public real_t z; - - /// - /// W component of the quaternion (real part). - /// Quaternion components should usually not be manipulated directly. - /// - public real_t w; - - /// - /// Access quaternion components using their index. - /// - /// `[0]` is equivalent to `.x`, `[1]` is equivalent to `.y`, `[2]` is equivalent to `.z`, `[3]` is equivalent to `.w`. - public real_t this[int index] - { - get - { - switch (index) - { - case 0: - return x; - case 1: - return y; - case 2: - return z; - case 3: - return w; - default: - throw new IndexOutOfRangeException(); - } - } - set - { - switch (index) - { - case 0: - x = value; - break; - case 1: - y = value; - break; - case 2: - z = value; - break; - case 3: - w = value; - break; - default: - throw new IndexOutOfRangeException(); - } - } - } - - /// - /// Returns the length (magnitude) of the quaternion. - /// - /// Equivalent to `Mathf.Sqrt(LengthSquared)`. - public real_t Length - { - get { return Mathf.Sqrt(LengthSquared); } - } - - /// - /// Returns the squared length (squared magnitude) of the quaternion. - /// This method runs faster than , so prefer it if - /// you need to compare quaternions or need the squared length for some formula. - /// - /// Equivalent to `Dot(this)`. - public real_t LengthSquared - { - get { return Dot(this); } - } - - /// - /// Performs a cubic spherical interpolation between quaternions `preA`, - /// this vector, `b`, and `postB`, by the given amount `t`. - /// - /// The destination quaternion. - /// A quaternion before this quaternion. - /// A quaternion after `b`. - /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. - /// The interpolated quaternion. - public Quat CubicSlerp(Quat b, Quat preA, Quat postB, real_t weight) - { - real_t t2 = (1.0f - weight) * weight * 2f; - Quat sp = Slerp(b, weight); - Quat sq = preA.Slerpni(postB, weight); - return sp.Slerpni(sq, t2); - } - - /// - /// Returns the dot product of two quaternions. - /// - /// The other quaternion. - /// The dot product. - public real_t Dot(Quat b) - { - return x * b.x + y * b.y + z * b.z + w * b.w; - } - - /// - /// Returns Euler angles (in the YXZ convention: when decomposing, - /// first Z, then X, and Y last) corresponding to the rotation - /// represented by the unit quaternion. Returned vector contains - /// the rotation angles in the format (X angle, Y angle, Z angle). - /// - /// The Euler angle representation of this quaternion. - public Vector3 GetEuler() - { -#if DEBUG - if (!IsNormalized()) - { - throw new InvalidOperationException("Quat is not normalized"); - } -#endif - var basis = new Basis(this); - return basis.GetEuler(); - } - - /// - /// Returns the inverse of the quaternion. - /// - /// The inverse quaternion. - public Quat Inverse() - { -#if DEBUG - if (!IsNormalized()) - { - throw new InvalidOperationException("Quat is not normalized"); - } -#endif - return new Quat(-x, -y, -z, w); - } - - /// - /// Returns whether the quaternion is normalized or not. - /// - /// A bool for whether the quaternion is normalized or not. - public bool IsNormalized() - { - return Mathf.Abs(LengthSquared - 1) <= Mathf.Epsilon; - } - - /// - /// Returns a copy of the quaternion, normalized to unit length. - /// - /// The normalized quaternion. - public Quat Normalized() - { - return this / Length; - } - - /// - /// Returns the result of the spherical linear interpolation between - /// this quaternion and `to` by amount `weight`. - /// - /// Note: Both quaternions must be normalized. - /// - /// The destination quaternion for interpolation. Must be normalized. - /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. - /// The resulting quaternion of the interpolation. - public Quat Slerp(Quat to, real_t weight) - { -#if DEBUG - if (!IsNormalized()) - { - throw new InvalidOperationException("Quat is not normalized"); - } - if (!to.IsNormalized()) - { - throw new ArgumentException("Argument is not normalized", nameof(to)); - } -#endif - - // Calculate cosine. - real_t cosom = x * to.x + y * to.y + z * to.z + w * to.w; - - var to1 = new Quat(); - - // Adjust signs if necessary. - if (cosom < 0.0) - { - cosom = -cosom; - to1.x = -to.x; - to1.y = -to.y; - to1.z = -to.z; - to1.w = -to.w; - } - else - { - to1.x = to.x; - to1.y = to.y; - to1.z = to.z; - to1.w = to.w; - } - - real_t sinom, scale0, scale1; - - // Calculate coefficients. - if (1.0 - cosom > Mathf.Epsilon) - { - // Standard case (Slerp). - real_t omega = Mathf.Acos(cosom); - sinom = Mathf.Sin(omega); - scale0 = Mathf.Sin((1.0f - weight) * omega) / sinom; - scale1 = Mathf.Sin(weight * omega) / sinom; - } - else - { - // Quaternions are very close so we can do a linear interpolation. - scale0 = 1.0f - weight; - scale1 = weight; - } - - // Calculate final values. - return new Quat - ( - scale0 * x + scale1 * to1.x, - scale0 * y + scale1 * to1.y, - scale0 * z + scale1 * to1.z, - scale0 * w + scale1 * to1.w - ); - } - - /// - /// Returns the result of the spherical linear interpolation between - /// this quaternion and `to` by amount `weight`, but without - /// checking if the rotation path is not bigger than 90 degrees. - /// - /// The destination quaternion for interpolation. Must be normalized. - /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. - /// The resulting quaternion of the interpolation. - public Quat Slerpni(Quat to, real_t weight) - { - real_t dot = Dot(to); - - if (Mathf.Abs(dot) > 0.9999f) - { - return this; - } - - real_t theta = Mathf.Acos(dot); - real_t sinT = 1.0f / Mathf.Sin(theta); - real_t newFactor = Mathf.Sin(weight * theta) * sinT; - real_t invFactor = Mathf.Sin((1.0f - weight) * theta) * sinT; - - return new Quat - ( - invFactor * x + newFactor * to.x, - invFactor * y + newFactor * to.y, - invFactor * z + newFactor * to.z, - invFactor * w + newFactor * to.w - ); - } - - /// - /// Returns a vector transformed (multiplied) by this quaternion. - /// - /// A vector to transform. - /// The transformed vector. - public Vector3 Xform(Vector3 v) - { -#if DEBUG - if (!IsNormalized()) - { - throw new InvalidOperationException("Quat is not normalized"); - } -#endif - var u = new Vector3(x, y, z); - Vector3 uv = u.Cross(v); - return v + ((uv * w) + u.Cross(uv)) * 2; - } - - // Constants - private static readonly Quat _identity = new Quat(0, 0, 0, 1); - - /// - /// The identity quaternion, representing no rotation. - /// Equivalent to an identity matrix. If a vector is transformed by - /// an identity quaternion, it will not change. - /// - /// Equivalent to `new Quat(0, 0, 0, 1)`. - public static Quat Identity { get { return _identity; } } - - /// - /// Constructs a quaternion defined by the given values. - /// - /// X component of the quaternion (imaginary `i` axis part). - /// Y component of the quaternion (imaginary `j` axis part). - /// Z component of the quaternion (imaginary `k` axis part). - /// W component of the quaternion (real part). - public Quat(real_t x, real_t y, real_t z, real_t w) - { - this.x = x; - this.y = y; - this.z = z; - this.w = w; - } - - /// - /// Constructs a quaternion from the given quaternion. - /// - /// The existing quaternion. - public Quat(Quat q) - { - this = q; - } - - /// - /// Constructs a quaternion from the given . - /// - /// The basis to construct from. - public Quat(Basis basis) - { - this = basis.Quat(); - } - - /// - /// Constructs a quaternion that will perform a rotation specified by - /// Euler angles (in the YXZ convention: when decomposing, - /// first Z, then X, and Y last), - /// given in the vector format as (X angle, Y angle, Z angle). - /// - /// - public Quat(Vector3 eulerYXZ) - { - real_t half_a1 = eulerYXZ.y * 0.5f; - real_t half_a2 = eulerYXZ.x * 0.5f; - real_t half_a3 = eulerYXZ.z * 0.5f; - - // R = Y(a1).X(a2).Z(a3) convention for Euler angles. - // Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-6) - // a3 is the angle of the first rotation, following the notation in this reference. - - real_t cos_a1 = Mathf.Cos(half_a1); - real_t sin_a1 = Mathf.Sin(half_a1); - real_t cos_a2 = Mathf.Cos(half_a2); - real_t sin_a2 = Mathf.Sin(half_a2); - real_t cos_a3 = Mathf.Cos(half_a3); - real_t sin_a3 = Mathf.Sin(half_a3); - - x = sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3; - y = sin_a1 * cos_a2 * cos_a3 - cos_a1 * sin_a2 * sin_a3; - z = cos_a1 * cos_a2 * sin_a3 - sin_a1 * sin_a2 * cos_a3; - w = sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3; - } - - /// - /// Constructs a quaternion that will rotate around the given axis - /// by the specified angle. The axis must be a normalized vector. - /// - /// The axis to rotate around. Must be normalized. - /// The angle to rotate, in radians. - public Quat(Vector3 axis, real_t angle) - { -#if DEBUG - if (!axis.IsNormalized()) - { - throw new ArgumentException("Argument is not normalized", nameof(axis)); - } -#endif - - real_t d = axis.Length(); - - if (d == 0f) - { - x = 0f; - y = 0f; - z = 0f; - w = 0f; - } - else - { - real_t sinAngle = Mathf.Sin(angle * 0.5f); - real_t cosAngle = Mathf.Cos(angle * 0.5f); - real_t s = sinAngle / d; - - x = axis.x * s; - y = axis.y * s; - z = axis.z * s; - w = cosAngle; - } - } - - public static Quat operator *(Quat left, Quat right) - { - return new Quat - ( - left.w * right.x + left.x * right.w + left.y * right.z - left.z * right.y, - left.w * right.y + left.y * right.w + left.z * right.x - left.x * right.z, - left.w * right.z + left.z * right.w + left.x * right.y - left.y * right.x, - left.w * right.w - left.x * right.x - left.y * right.y - left.z * right.z - ); - } - - public static Quat operator +(Quat left, Quat right) - { - return new Quat(left.x + right.x, left.y + right.y, left.z + right.z, left.w + right.w); - } - - public static Quat operator -(Quat left, Quat right) - { - return new Quat(left.x - right.x, left.y - right.y, left.z - right.z, left.w - right.w); - } - - public static Quat operator -(Quat left) - { - return new Quat(-left.x, -left.y, -left.z, -left.w); - } - - public static Quat operator *(Quat left, Vector3 right) - { - return new Quat - ( - left.w * right.x + left.y * right.z - left.z * right.y, - left.w * right.y + left.z * right.x - left.x * right.z, - left.w * right.z + left.x * right.y - left.y * right.x, - -left.x * right.x - left.y * right.y - left.z * right.z - ); - } - - public static Quat operator *(Vector3 left, Quat right) - { - return new Quat - ( - right.w * left.x + right.y * left.z - right.z * left.y, - right.w * left.y + right.z * left.x - right.x * left.z, - right.w * left.z + right.x * left.y - right.y * left.x, - -right.x * left.x - right.y * left.y - right.z * left.z - ); - } - - public static Quat operator *(Quat left, real_t right) - { - return new Quat(left.x * right, left.y * right, left.z * right, left.w * right); - } - - public static Quat operator *(real_t left, Quat right) - { - return new Quat(right.x * left, right.y * left, right.z * left, right.w * left); - } - - public static Quat operator /(Quat left, real_t right) - { - return left * (1.0f / right); - } - - public static bool operator ==(Quat left, Quat right) - { - return left.Equals(right); - } - - public static bool operator !=(Quat left, Quat right) - { - return !left.Equals(right); - } - - public override bool Equals(object obj) - { - if (obj is Quat) - { - return Equals((Quat)obj); - } - - return false; - } - - public bool Equals(Quat other) - { - return x == other.x && y == other.y && z == other.z && w == other.w; - } - - /// - /// Returns true if this quaternion and `other` are approximately equal, by running - /// on each component. - /// - /// The other quaternion to compare. - /// Whether or not the quaternions are approximately equal. - public bool IsEqualApprox(Quat other) - { - return Mathf.IsEqualApprox(x, other.x) && Mathf.IsEqualApprox(y, other.y) && Mathf.IsEqualApprox(z, other.z) && Mathf.IsEqualApprox(w, other.w); - } - - public override int GetHashCode() - { - return y.GetHashCode() ^ x.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode(); - } - - public override string ToString() - { - return String.Format("({0}, {1}, {2}, {3})", x.ToString(), y.ToString(), z.ToString(), w.ToString()); - } - - public string ToString(string format) - { - return String.Format("({0}, {1}, {2}, {3})", x.ToString(format), y.ToString(format), z.ToString(format), w.ToString(format)); - } - } -} diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs new file mode 100644 index 0000000000..b087b4c200 --- /dev/null +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs @@ -0,0 +1,541 @@ +using System; +using System.Runtime.InteropServices; +#if REAL_T_IS_DOUBLE +using real_t = System.Double; +#else +using real_t = System.Single; +#endif + +namespace Godot +{ + /// + /// A unit quaternion used for representing 3D rotations. + /// Quaternions need to be normalized to be used for rotation. + /// + /// It is similar to Basis, which implements matrix representation of + /// rotations, and can be parametrized using both an axis-angle pair + /// or Euler angles. Basis stores rotation, scale, and shearing, + /// while Quaternion only stores rotation. + /// + /// Due to its compactness and the way it is stored in memory, certain + /// operations (obtaining axis-angle and performing SLERP, in particular) + /// are more efficient and robust against floating-point errors. + /// + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Quaternion : IEquatable + { + /// + /// X component of the quaternion (imaginary `i` axis part). + /// Quaternion components should usually not be manipulated directly. + /// + public real_t x; + + /// + /// Y component of the quaternion (imaginary `j` axis part). + /// Quaternion components should usually not be manipulated directly. + /// + public real_t y; + + /// + /// Z component of the quaternion (imaginary `k` axis part). + /// Quaternion components should usually not be manipulated directly. + /// + public real_t z; + + /// + /// W component of the quaternion (real part). + /// Quaternion components should usually not be manipulated directly. + /// + public real_t w; + + /// + /// Access quaternion components using their index. + /// + /// `[0]` is equivalent to `.x`, `[1]` is equivalent to `.y`, `[2]` is equivalent to `.z`, `[3]` is equivalent to `.w`. + public real_t this[int index] + { + get + { + switch (index) + { + case 0: + return x; + case 1: + return y; + case 2: + return z; + case 3: + return w; + default: + throw new IndexOutOfRangeException(); + } + } + set + { + switch (index) + { + case 0: + x = value; + break; + case 1: + y = value; + break; + case 2: + z = value; + break; + case 3: + w = value; + break; + default: + throw new IndexOutOfRangeException(); + } + } + } + + /// + /// Returns the length (magnitude) of the quaternion. + /// + /// Equivalent to `Mathf.Sqrt(LengthSquared)`. + public real_t Length + { + get { return Mathf.Sqrt(LengthSquared); } + } + + /// + /// Returns the squared length (squared magnitude) of the quaternion. + /// This method runs faster than , so prefer it if + /// you need to compare quaternions or need the squared length for some formula. + /// + /// Equivalent to `Dot(this)`. + public real_t LengthSquared + { + get { return Dot(this); } + } + + /// + /// Performs a cubic spherical interpolation between quaternions `preA`, + /// this vector, `b`, and `postB`, by the given amount `t`. + /// + /// The destination quaternion. + /// A quaternion before this quaternion. + /// A quaternion after `b`. + /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. + /// The interpolated quaternion. + public Quaternion CubicSlerp(Quaternion b, Quaternion preA, Quaternion postB, real_t weight) + { + real_t t2 = (1.0f - weight) * weight * 2f; + Quaternion sp = Slerp(b, weight); + Quaternion sq = preA.Slerpni(postB, weight); + return sp.Slerpni(sq, t2); + } + + /// + /// Returns the dot product of two quaternions. + /// + /// The other quaternion. + /// The dot product. + public real_t Dot(Quaternion b) + { + return x * b.x + y * b.y + z * b.z + w * b.w; + } + + /// + /// Returns Euler angles (in the YXZ convention: when decomposing, + /// first Z, then X, and Y last) corresponding to the rotation + /// represented by the unit quaternion. Returned vector contains + /// the rotation angles in the format (X angle, Y angle, Z angle). + /// + /// The Euler angle representation of this quaternion. + public Vector3 GetEuler() + { +#if DEBUG + if (!IsNormalized()) + { + throw new InvalidOperationException("Quaternion is not normalized"); + } +#endif + var basis = new Basis(this); + return basis.GetEuler(); + } + + /// + /// Returns the inverse of the quaternion. + /// + /// The inverse quaternion. + public Quaternion Inverse() + { +#if DEBUG + if (!IsNormalized()) + { + throw new InvalidOperationException("Quaternion is not normalized"); + } +#endif + return new Quaternion(-x, -y, -z, w); + } + + /// + /// Returns whether the quaternion is normalized or not. + /// + /// A bool for whether the quaternion is normalized or not. + public bool IsNormalized() + { + return Mathf.Abs(LengthSquared - 1) <= Mathf.Epsilon; + } + + /// + /// Returns a copy of the quaternion, normalized to unit length. + /// + /// The normalized quaternion. + public Quaternion Normalized() + { + return this / Length; + } + + /// + /// Returns the result of the spherical linear interpolation between + /// this quaternion and `to` by amount `weight`. + /// + /// Note: Both quaternions must be normalized. + /// + /// The destination quaternion for interpolation. Must be normalized. + /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. + /// The resulting quaternion of the interpolation. + public Quaternion Slerp(Quaternion to, real_t weight) + { +#if DEBUG + if (!IsNormalized()) + { + throw new InvalidOperationException("Quaternion is not normalized"); + } + if (!to.IsNormalized()) + { + throw new ArgumentException("Argument is not normalized", nameof(to)); + } +#endif + + // Calculate cosine. + real_t cosom = x * to.x + y * to.y + z * to.z + w * to.w; + + var to1 = new Quaternion(); + + // Adjust signs if necessary. + if (cosom < 0.0) + { + cosom = -cosom; + to1.x = -to.x; + to1.y = -to.y; + to1.z = -to.z; + to1.w = -to.w; + } + else + { + to1.x = to.x; + to1.y = to.y; + to1.z = to.z; + to1.w = to.w; + } + + real_t sinom, scale0, scale1; + + // Calculate coefficients. + if (1.0 - cosom > Mathf.Epsilon) + { + // Standard case (Slerp). + real_t omega = Mathf.Acos(cosom); + sinom = Mathf.Sin(omega); + scale0 = Mathf.Sin((1.0f - weight) * omega) / sinom; + scale1 = Mathf.Sin(weight * omega) / sinom; + } + else + { + // Quaternions are very close so we can do a linear interpolation. + scale0 = 1.0f - weight; + scale1 = weight; + } + + // Calculate final values. + return new Quaternion + ( + scale0 * x + scale1 * to1.x, + scale0 * y + scale1 * to1.y, + scale0 * z + scale1 * to1.z, + scale0 * w + scale1 * to1.w + ); + } + + /// + /// Returns the result of the spherical linear interpolation between + /// this quaternion and `to` by amount `weight`, but without + /// checking if the rotation path is not bigger than 90 degrees. + /// + /// The destination quaternion for interpolation. Must be normalized. + /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. + /// The resulting quaternion of the interpolation. + public Quaternion Slerpni(Quaternion to, real_t weight) + { + real_t dot = Dot(to); + + if (Mathf.Abs(dot) > 0.9999f) + { + return this; + } + + real_t theta = Mathf.Acos(dot); + real_t sinT = 1.0f / Mathf.Sin(theta); + real_t newFactor = Mathf.Sin(weight * theta) * sinT; + real_t invFactor = Mathf.Sin((1.0f - weight) * theta) * sinT; + + return new Quaternion + ( + invFactor * x + newFactor * to.x, + invFactor * y + newFactor * to.y, + invFactor * z + newFactor * to.z, + invFactor * w + newFactor * to.w + ); + } + + /// + /// Returns a vector transformed (multiplied) by this quaternion. + /// + /// A vector to transform. + /// The transformed vector. + public Vector3 Xform(Vector3 v) + { +#if DEBUG + if (!IsNormalized()) + { + throw new InvalidOperationException("Quaternion is not normalized"); + } +#endif + var u = new Vector3(x, y, z); + Vector3 uv = u.Cross(v); + return v + ((uv * w) + u.Cross(uv)) * 2; + } + + // Constants + private static readonly Quaternion _identity = new Quaternion(0, 0, 0, 1); + + /// + /// The identity quaternion, representing no rotation. + /// Equivalent to an identity matrix. If a vector is transformed by + /// an identity quaternion, it will not change. + /// + /// Equivalent to `new Quaternion(0, 0, 0, 1)`. + public static Quaternion Identity { get { return _identity; } } + + /// + /// Constructs a quaternion defined by the given values. + /// + /// X component of the quaternion (imaginary `i` axis part). + /// Y component of the quaternion (imaginary `j` axis part). + /// Z component of the quaternion (imaginary `k` axis part). + /// W component of the quaternion (real part). + public Quaternion(real_t x, real_t y, real_t z, real_t w) + { + this.x = x; + this.y = y; + this.z = z; + this.w = w; + } + + /// + /// Constructs a quaternion from the given quaternion. + /// + /// The existing quaternion. + public Quaternion(Quaternion q) + { + this = q; + } + + /// + /// Constructs a quaternion from the given . + /// + /// The basis to construct from. + public Quaternion(Basis basis) + { + this = basis.Quaternion(); + } + + /// + /// Constructs a quaternion that will perform a rotation specified by + /// Euler angles (in the YXZ convention: when decomposing, + /// first Z, then X, and Y last), + /// given in the vector format as (X angle, Y angle, Z angle). + /// + /// + public Quaternion(Vector3 eulerYXZ) + { + real_t half_a1 = eulerYXZ.y * 0.5f; + real_t half_a2 = eulerYXZ.x * 0.5f; + real_t half_a3 = eulerYXZ.z * 0.5f; + + // R = Y(a1).X(a2).Z(a3) convention for Euler angles. + // Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-6) + // a3 is the angle of the first rotation, following the notation in this reference. + + real_t cos_a1 = Mathf.Cos(half_a1); + real_t sin_a1 = Mathf.Sin(half_a1); + real_t cos_a2 = Mathf.Cos(half_a2); + real_t sin_a2 = Mathf.Sin(half_a2); + real_t cos_a3 = Mathf.Cos(half_a3); + real_t sin_a3 = Mathf.Sin(half_a3); + + x = sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3; + y = sin_a1 * cos_a2 * cos_a3 - cos_a1 * sin_a2 * sin_a3; + z = cos_a1 * cos_a2 * sin_a3 - sin_a1 * sin_a2 * cos_a3; + w = sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3; + } + + /// + /// Constructs a quaternion that will rotate around the given axis + /// by the specified angle. The axis must be a normalized vector. + /// + /// The axis to rotate around. Must be normalized. + /// The angle to rotate, in radians. + public Quaternion(Vector3 axis, real_t angle) + { +#if DEBUG + if (!axis.IsNormalized()) + { + throw new ArgumentException("Argument is not normalized", nameof(axis)); + } +#endif + + real_t d = axis.Length(); + + if (d == 0f) + { + x = 0f; + y = 0f; + z = 0f; + w = 0f; + } + else + { + real_t sinAngle = Mathf.Sin(angle * 0.5f); + real_t cosAngle = Mathf.Cos(angle * 0.5f); + real_t s = sinAngle / d; + + x = axis.x * s; + y = axis.y * s; + z = axis.z * s; + w = cosAngle; + } + } + + public static Quaternion operator *(Quaternion left, Quaternion right) + { + return new Quaternion + ( + left.w * right.x + left.x * right.w + left.y * right.z - left.z * right.y, + left.w * right.y + left.y * right.w + left.z * right.x - left.x * right.z, + left.w * right.z + left.z * right.w + left.x * right.y - left.y * right.x, + left.w * right.w - left.x * right.x - left.y * right.y - left.z * right.z + ); + } + + public static Quaternion operator +(Quaternion left, Quaternion right) + { + return new Quaternion(left.x + right.x, left.y + right.y, left.z + right.z, left.w + right.w); + } + + public static Quaternion operator -(Quaternion left, Quaternion right) + { + return new Quaternion(left.x - right.x, left.y - right.y, left.z - right.z, left.w - right.w); + } + + public static Quaternion operator -(Quaternion left) + { + return new Quaternion(-left.x, -left.y, -left.z, -left.w); + } + + public static Quaternion operator *(Quaternion left, Vector3 right) + { + return new Quaternion + ( + left.w * right.x + left.y * right.z - left.z * right.y, + left.w * right.y + left.z * right.x - left.x * right.z, + left.w * right.z + left.x * right.y - left.y * right.x, + -left.x * right.x - left.y * right.y - left.z * right.z + ); + } + + public static Quaternion operator *(Vector3 left, Quaternion right) + { + return new Quaternion + ( + right.w * left.x + right.y * left.z - right.z * left.y, + right.w * left.y + right.z * left.x - right.x * left.z, + right.w * left.z + right.x * left.y - right.y * left.x, + -right.x * left.x - right.y * left.y - right.z * left.z + ); + } + + public static Quaternion operator *(Quaternion left, real_t right) + { + return new Quaternion(left.x * right, left.y * right, left.z * right, left.w * right); + } + + public static Quaternion operator *(real_t left, Quaternion right) + { + return new Quaternion(right.x * left, right.y * left, right.z * left, right.w * left); + } + + public static Quaternion operator /(Quaternion left, real_t right) + { + return left * (1.0f / right); + } + + public static bool operator ==(Quaternion left, Quaternion right) + { + return left.Equals(right); + } + + public static bool operator !=(Quaternion left, Quaternion right) + { + return !left.Equals(right); + } + + public override bool Equals(object obj) + { + if (obj is Quaternion) + { + return Equals((Quaternion)obj); + } + + return false; + } + + public bool Equals(Quaternion other) + { + return x == other.x && y == other.y && z == other.z && w == other.w; + } + + /// + /// Returns true if this quaternion and `other` are approximately equal, by running + /// on each component. + /// + /// The other quaternion to compare. + /// Whether or not the quaternions are approximately equal. + public bool IsEqualApprox(Quaternion other) + { + return Mathf.IsEqualApprox(x, other.x) && Mathf.IsEqualApprox(y, other.y) && Mathf.IsEqualApprox(z, other.z) && Mathf.IsEqualApprox(w, other.w); + } + + public override int GetHashCode() + { + return y.GetHashCode() ^ x.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode(); + } + + public override string ToString() + { + return String.Format("({0}, {1}, {2}, {3})", x.ToString(), y.ToString(), z.ToString(), w.ToString()); + } + + public string ToString(string format) + { + return String.Format("({0}, {1}, {2}, {3})", x.ToString(format), y.ToString(format), z.ToString(format), w.ToString(format)); + } + } +} diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs index 9e5ff2b315..50cc95fb95 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs @@ -124,15 +124,15 @@ namespace Godot /* not sure if very "efficient" but good enough? */ Vector3 sourceScale = basis.Scale; - Quat sourceRotation = basis.RotationQuat(); + Quaternion sourceRotation = basis.RotationQuaternion(); Vector3 sourceLocation = origin; Vector3 destinationScale = transform.basis.Scale; - Quat destinationRotation = transform.basis.RotationQuat(); + Quaternion destinationRotation = transform.basis.RotationQuaternion(); Vector3 destinationLocation = transform.origin; var interpolated = new Transform3D(); - interpolated.basis.SetQuatScale(sourceRotation.Slerp(destinationRotation, weight).Normalized(), sourceScale.Lerp(destinationScale, weight)); + interpolated.basis.SetQuaternionScale(sourceRotation.Slerp(destinationRotation, weight).Normalized(), sourceScale.Lerp(destinationScale, weight)); interpolated.origin = sourceLocation.Lerp(destinationLocation, weight); return interpolated; @@ -324,11 +324,11 @@ namespace Godot /// /// Constructs a transformation matrix from the given quaternion and origin vector. /// - /// The to create the basis from. + /// The to create the basis from. /// The origin vector, or column index 3. - public Transform3D(Quat quat, Vector3 origin) + public Transform3D(Quaternion quaternion, Vector3 origin) { - basis = new Basis(quat); + basis = new Basis(quaternion); this.origin = origin; } diff --git a/modules/mono/glue/GodotSharp/GodotSharp/GodotSharp.csproj b/modules/mono/glue/GodotSharp/GodotSharp/GodotSharp.csproj index c3dd13d84b..1fcfe74c86 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/GodotSharp.csproj +++ b/modules/mono/glue/GodotSharp/GodotSharp/GodotSharp.csproj @@ -50,7 +50,7 @@ - + -- cgit v1.2.3