Mono/C#: Add Basis.Slerp, update Quat.Xform and add some math checks

This commit is contained in:
Ignacio Etcheverry 2020-01-23 15:54:34 +01:00
parent ff7e7bd260
commit 82b0899e54
3 changed files with 49 additions and 7 deletions

View file

@ -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];

View file

@ -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;
}
}

View file

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