diff options
| -rw-r--r-- | modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs | 13 | ||||
| -rw-r--r-- | modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs | 37 | ||||
| -rw-r--r-- | modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs | 6 |
3 files changed, 49 insertions, 7 deletions
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs index c5e62b77c8..d38589013e 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs @@ -387,6 +387,19 @@ namespace Godot return b; } + public Basis Slerp(Basis target, real_t t) + { + var from = new Quat(this); + var to = new Quat(target); + + var b = new Basis(from.Slerp(to, t)); + b.Row0 *= Mathf.Lerp(Row0.Length(), target.Row0.Length(), t); + b.Row1 *= Mathf.Lerp(Row1.Length(), target.Row1.Length(), t); + b.Row2 *= Mathf.Lerp(Row2.Length(), target.Row2.Length(), t); + + return b; + } + public real_t Tdotx(Vector3 with) { return this.Row0[0] * with[0] + this.Row1[0] * with[1] + this.Row2[0] * with[2]; diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs index 8f60867ac3..6702634c51 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs @@ -82,12 +82,20 @@ namespace Godot public Vector3 GetEuler() { +#if DEBUG + if (!IsNormalized()) + throw new InvalidOperationException("Quat is not normalized"); +#endif var basis = new Basis(this); return basis.GetEuler(); } public Quat Inverse() { +#if DEBUG + if (!IsNormalized()) + throw new InvalidOperationException("Quat is not normalized"); +#endif return new Quat(-x, -y, -z, w); } @@ -125,6 +133,13 @@ namespace Godot public Quat Slerp(Quat b, real_t t) { +#if DEBUG + if (!IsNormalized()) + throw new InvalidOperationException("Quat is not normalized"); + if (!b.IsNormalized()) + throw new ArgumentException("Argument is not normalized", nameof(b)); +#endif + // Calculate cosine real_t cosom = x * b.x + y * b.y + z * b.z + w * b.w; @@ -200,9 +215,13 @@ namespace Godot public Vector3 Xform(Vector3 v) { - Quat q = this * v; - q *= Inverse(); - return new Vector3(q.x, q.y, q.z); +#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; } // Static Readonly Properties @@ -257,8 +276,12 @@ namespace Godot 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(); - real_t angle_t = angle; if (d == 0f) { @@ -269,12 +292,14 @@ namespace Godot } else { - real_t s = Mathf.Sin(angle_t * 0.5f) / d; + 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 = Mathf.Cos(angle_t * 0.5f); + w = cosAngle; } } diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs index 025b09199f..fded34002d 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs @@ -255,7 +255,7 @@ namespace Godot { #if DEBUG if (!n.IsNormalized()) - throw new ArgumentException(String.Format("{0} is not normalized", n), nameof(n)); + throw new ArgumentException("Argument is not normalized", nameof(n)); #endif return 2.0f * n * Dot(n) - this; } @@ -296,6 +296,10 @@ namespace Godot public Vector3 Slerp(Vector3 b, real_t t) { +#if DEBUG + if (!IsNormalized()) + throw new InvalidOperationException("Vector3 is not normalized"); +#endif real_t theta = AngleTo(b); return Rotated(Cross(b), theta * t); } |