Nixyri/godot
1
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2024-11-22 12:12:28 +00:00
Code Issues Actions 1 Packages Projects Releases Wiki Activity

Merge pull request #64860 from raulsntos/dotnet/sync-math

Browse Source 
Sync C# cubic interpolation with core
This commit is contained in:
Rémi Verschelde 2022-08-27 19:01:37 +02:00 committed by GitHub
commit 19e2f30a55
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 371 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs
View File

@ -498,6 +498,15 @@ namespace Godot
);
}
internal Basis Lerp(Basis to, real_t weight)
{
Basis b = this;
b.Row0 = Row0.Lerp(to.Row0, weight);
b.Row1 = Row1.Lerp(to.Row1, weight);
b.Row2 = Row2.Lerp(to.Row2, weight);
return b;
}
/// <summary>
/// Returns the orthonormalized version of the basis matrix (useful to
/// call occasionally to avoid rounding errors for orthogonal matrices).

90
modules/mono/glue/GodotSharp/GodotSharp/Core/Mathf.cs
View File

@ -175,7 +175,8 @@ namespace Godot
}
/// <summary>
/// Cubic interpolates between two values by a normalized value with pre and post values.
/// Cubic interpolates between two values by the factor defined in <paramref name="weight"/>
/// with pre and post values.
/// </summary>
/// <param name="from">The start value for interpolation.</param>
/// <param name="to">The destination value for interpolation.</param>
@ -192,6 +193,93 @@ namespace Godot
(-pre + 3.0f * from - 3.0f * to + post) * (weight * weight * weight));
}
/// <summary>
/// Cubic interpolates between two rotation values with shortest path
/// by the factor defined in <paramref name="weight"/> with pre and post values.
/// See also <see cref="LerpAngle"/>.
/// </summary>
/// <param name="from">The start value for interpolation.</param>
/// <param name="to">The destination value for interpolation.</param>
/// <param name="pre">The value which before "from" value for interpolation.</param>
/// <param name="post">The value which after "to" value for interpolation.</param>
/// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
/// <returns>The resulting value of the interpolation.</returns>
public static real_t CubicInterpolateAngle(real_t from, real_t to, real_t pre, real_t post, real_t weight)
{
real_t fromRot = from % Mathf.Tau;
real_t preDiff = (pre - fromRot) % Mathf.Tau;
real_t preRot = fromRot + (2.0f * preDiff) % Mathf.Tau - preDiff;
real_t toDiff = (to - fromRot) % Mathf.Tau;
real_t toRot = fromRot + (2.0f * toDiff) % Mathf.Tau - toDiff;
real_t postDiff = (post - toRot) % Mathf.Tau;
real_t postRot = toRot + (2.0f * postDiff) % Mathf.Tau - postDiff;
return CubicInterpolate(fromRot, toRot, preRot, postRot, weight);
}
/// <summary>
/// Cubic interpolates between two values by the factor defined in <paramref name="weight"/>
/// with pre and post values.
/// It can perform smoother interpolation than <see cref="CubicInterpolate"/>
/// by the time values.
/// </summary>
/// <param name="from">The start value for interpolation.</param>
/// <param name="to">The destination value for interpolation.</param>
/// <param name="pre">The value which before "from" value for interpolation.</param>
/// <param name="post">The value which after "to" value for interpolation.</param>
/// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
/// <param name="toT"></param>
/// <param name="preT"></param>
/// <param name="postT"></param>
/// <returns>The resulting value of the interpolation.</returns>
public static real_t CubicInterpolateInTime(real_t from, real_t to, real_t pre, real_t post, real_t weight, real_t toT, real_t preT, real_t postT)
{
/* Barry-Goldman method */
real_t t = Lerp(0.0f, toT, weight);
real_t a1 = Lerp(pre, from, preT == 0 ? 0.0f : (t - preT) / -preT);
real_t a2 = Lerp(from, to, toT == 0 ? 0.5f : t / toT);
real_t a3 = Lerp(to, post, postT - toT == 0 ? 1.0f : (t - toT) / (postT - toT));
real_t b1 = Lerp(a1, a2, toT - preT == 0 ? 0.0f : (t - preT) / (toT - preT));
real_t b2 = Lerp(a2, a3, postT == 0 ? 1.0f : t / postT);
return Lerp(b1, b2, toT == 0 ? 0.5f : t / toT);
}
/// <summary>
/// Cubic interpolates between two rotation values with shortest path
/// by the factor defined in <paramref name="weight"/> with pre and post values.
/// See also <see cref="LerpAngle"/>.
/// It can perform smoother interpolation than <see cref="CubicInterpolateAngle"/>
/// by the time values.
/// </summary>
/// <param name="from">The start value for interpolation.</param>
/// <param name="to">The destination value for interpolation.</param>
/// <param name="pre">The value which before "from" value for interpolation.</param>
/// <param name="post">The value which after "to" value for interpolation.</param>
/// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
/// <param name="toT"></param>
/// <param name="preT"></param>
/// <param name="postT"></param>
/// <returns>The resulting value of the interpolation.</returns>
public static real_t CubicInterpolateAngleInTime(real_t from, real_t to, real_t pre, real_t post, real_t weight,
real_t toT, real_t preT, real_t postT)
{
real_t fromRot = from % Mathf.Tau;
real_t preDiff = (pre - fromRot) % Mathf.Tau;
real_t preRot = fromRot + (2.0f * preDiff) % Mathf.Tau - preDiff;
real_t toDiff = (to - fromRot) % Mathf.Tau;
real_t toRot = fromRot + (2.0f * toDiff) % Mathf.Tau - toDiff;
real_t postDiff = (post - toRot) % Mathf.Tau;
real_t postRot = toRot + (2.0f * postDiff) % Mathf.Tau - postDiff;
return CubicInterpolateInTime(fromRot, toRot, preRot, postRot, weight, toT, preT, postT);
}
/// <summary>
/// Returns the point at the given <paramref name="t"/> on a one-dimensional Bezier curve defined by
/// the given <paramref name="control1"/>, <paramref name="control2"/> and <paramref name="end"/> points.

185
modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs
View File

@ -132,7 +132,7 @@ namespace Godot
}
/// <summary>
/// Performs a cubic spherical interpolation between quaternions <paramref name="preA"/>, this quaternion,
/// Performs a spherical cubic interpolation between quaternions <paramref name="preA"/>, this quaternion,
/// <paramref name="b"/>, and <paramref name="postB"/>, by the given amount <paramref name="weight"/>.
/// </summary>
/// <param name="b">The destination quaternion.</param>
@ -140,12 +140,128 @@ namespace Godot
/// <param name="postB">A quaternion after <paramref name="b"/>.</param>
/// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
/// <returns>The interpolated quaternion.</returns>
public Quaternion CubicSlerp(Quaternion b, Quaternion preA, Quaternion postB, real_t weight)
public Quaternion SphericalCubicInterpolate(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);
#if DEBUG
if (!IsNormalized())
{
throw new InvalidOperationException("Quaternion is not normalized");
}
if (!b.IsNormalized())
{
throw new ArgumentException("Argument is not normalized", nameof(b));
}
#endif
// Align flip phases.
Quaternion fromQ = new Basis(this).GetRotationQuaternion();
Quaternion preQ = new Basis(preA).GetRotationQuaternion();
Quaternion toQ = new Basis(b).GetRotationQuaternion();
Quaternion postQ = new Basis(postB).GetRotationQuaternion();
// Flip quaternions to shortest path if necessary.
bool flip1 = Math.Sign(fromQ.Dot(preQ)) < 0;
preQ = flip1 ? -preQ : preQ;
bool flip2 = Math.Sign(fromQ.Dot(toQ)) < 0;
toQ = flip2 ? -toQ : toQ;
bool flip3 = flip2 ? toQ.Dot(postQ) <= 0 : Math.Sign(toQ.Dot(postQ)) < 0;
postQ = flip3 ? -postQ : postQ;
// Calc by Expmap in fromQ space.
Quaternion lnFrom = new Quaternion(0, 0, 0, 0);
Quaternion lnTo = (fromQ.Inverse() * toQ).Log();
Quaternion lnPre = (fromQ.Inverse() * preQ).Log();
Quaternion lnPost = (fromQ.Inverse() * postQ).Log();
Quaternion ln = new Quaternion(
Mathf.CubicInterpolate(lnFrom.x, lnTo.x, lnPre.x, lnPost.x, weight),
Mathf.CubicInterpolate(lnFrom.y, lnTo.y, lnPre.y, lnPost.y, weight),
Mathf.CubicInterpolate(lnFrom.z, lnTo.z, lnPre.z, lnPost.z, weight),
0);
Quaternion q1 = fromQ * ln.Exp();
// Calc by Expmap in toQ space.
lnFrom = (toQ.Inverse() * fromQ).Log();
lnTo = new Quaternion(0, 0, 0, 0);
lnPre = (toQ.Inverse() * preQ).Log();
lnPost = (toQ.Inverse() * postQ).Log();
ln = new Quaternion(
Mathf.CubicInterpolate(lnFrom.x, lnTo.x, lnPre.x, lnPost.x, weight),
Mathf.CubicInterpolate(lnFrom.y, lnTo.y, lnPre.y, lnPost.y, weight),
Mathf.CubicInterpolate(lnFrom.z, lnTo.z, lnPre.z, lnPost.z, weight),
0);
Quaternion q2 = toQ * ln.Exp();
// To cancel error made by Expmap ambiguity, do blends.
return q1.Slerp(q2, weight);
}
/// <summary>
/// Performs a spherical cubic interpolation between quaternions <paramref name="preA"/>, this quaternion,
/// <paramref name="b"/>, and <paramref name="postB"/>, by the given amount <paramref name="weight"/>.
/// It can perform smoother interpolation than <see cref="SphericalCubicInterpolate"/>
/// by the time values.
/// </summary>
/// <param name="b">The destination quaternion.</param>
/// <param name="preA">A quaternion before this quaternion.</param>
/// <param name="postB">A quaternion after <paramref name="b"/>.</param>
/// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
/// <param name="bT"></param>
/// <param name="preAT"></param>
/// <param name="postBT"></param>
/// <returns>The interpolated quaternion.</returns>
public Quaternion SphericalCubicInterpolateInTime(Quaternion b, Quaternion preA, Quaternion postB, real_t weight, real_t bT, real_t preAT, real_t postBT)
{
#if DEBUG
if (!IsNormalized())
{
throw new InvalidOperationException("Quaternion is not normalized");
}
if (!b.IsNormalized())
{
throw new ArgumentException("Argument is not normalized", nameof(b));
}
#endif
// Align flip phases.
Quaternion fromQ = new Basis(this).GetRotationQuaternion();
Quaternion preQ = new Basis(preA).GetRotationQuaternion();
Quaternion toQ = new Basis(b).GetRotationQuaternion();
Quaternion postQ = new Basis(postB).GetRotationQuaternion();
// Flip quaternions to shortest path if necessary.
bool flip1 = Math.Sign(fromQ.Dot(preQ)) < 0;
preQ = flip1 ? -preQ : preQ;
bool flip2 = Math.Sign(fromQ.Dot(toQ)) < 0;
toQ = flip2 ? -toQ : toQ;
bool flip3 = flip2 ? toQ.Dot(postQ) <= 0 : Math.Sign(toQ.Dot(postQ)) < 0;
postQ = flip3 ? -postQ : postQ;
// Calc by Expmap in fromQ space.
Quaternion lnFrom = new Quaternion(0, 0, 0, 0);
Quaternion lnTo = (fromQ.Inverse() * toQ).Log();
Quaternion lnPre = (fromQ.Inverse() * preQ).Log();
Quaternion lnPost = (fromQ.Inverse() * postQ).Log();
Quaternion ln = new Quaternion(
Mathf.CubicInterpolateInTime(lnFrom.x, lnTo.x, lnPre.x, lnPost.x, weight, bT, preAT, postBT),
Mathf.CubicInterpolateInTime(lnFrom.y, lnTo.y, lnPre.y, lnPost.y, weight, bT, preAT, postBT),
Mathf.CubicInterpolateInTime(lnFrom.z, lnTo.z, lnPre.z, lnPost.z, weight, bT, preAT, postBT),
0);
Quaternion q1 = fromQ * ln.Exp();
// Calc by Expmap in toQ space.
lnFrom = (toQ.Inverse() * fromQ).Log();
lnTo = new Quaternion(0, 0, 0, 0);
lnPre = (toQ.Inverse() * preQ).Log();
lnPost = (toQ.Inverse() * postQ).Log();
ln = new Quaternion(
Mathf.CubicInterpolateInTime(lnFrom.x, lnTo.x, lnPre.x, lnPost.x, weight, bT, preAT, postBT),
Mathf.CubicInterpolateInTime(lnFrom.y, lnTo.y, lnPre.y, lnPost.y, weight, bT, preAT, postBT),
Mathf.CubicInterpolateInTime(lnFrom.z, lnTo.z, lnPre.z, lnPost.z, weight, bT, preAT, postBT),
0);
Quaternion q2 = toQ * ln.Exp();
// To cancel error made by Expmap ambiguity, do blends.
return q1.Slerp(q2, weight);
}
/// <summary>
@ -158,6 +274,34 @@ namespace Godot
return (x * b.x) + (y * b.y) + (z * b.z) + (w * b.w);
}
public Quaternion Exp()
{
Vector3 v = new Vector3(x, y, z);
real_t theta = v.Length();
v = v.Normalized();
if (theta < Mathf.Epsilon || !v.IsNormalized())
{
return new Quaternion(0, 0, 0, 1);
}
return new Quaternion(v, theta);
}
public real_t GetAngle()
{
return 2 * Mathf.Acos(w);
}
public Vector3 GetAxis()
{
if (Mathf.Abs(w) > 1 - Mathf.Epsilon)
{
return new Vector3(x, y, z);
}
real_t r = 1 / Mathf.Sqrt(1 - w * w);
return new Vector3(x * r, y * r, z * r);
}
/// <summary>
/// Returns Euler angles (in the YXZ convention: when decomposing,
/// first Z, then X, and Y last) corresponding to the rotation
@ -201,6 +345,12 @@ namespace Godot
return Mathf.Abs(LengthSquared - 1) <= Mathf.Epsilon;
}
public Quaternion Log()
{
Vector3 v = GetAxis() * GetAngle();
return new Quaternion(v.x, v.y, v.z, 0);
}
/// <summary>
/// Returns a copy of the quaternion, normalized to unit length.
/// </summary>
@ -233,7 +383,7 @@ namespace Godot
#endif
// Calculate cosine.
real_t cosom = x * to.x + y * to.y + z * to.z + w * to.w;
real_t cosom = Dot(to);
var to1 = new Quaternion();
@ -241,17 +391,11 @@ namespace Godot
if (cosom < 0.0)
{
cosom = -cosom;
to1.x = -to.x;
to1.y = -to.y;
to1.z = -to.z;
to1.w = -to.w;
to1 = -to;
}
else
{
to1.x = to.x;
to1.y = to.y;
to1.z = to.z;
to1.w = to.w;
to1 = to;
}
real_t sinom, scale0, scale1;
@ -292,6 +436,17 @@ namespace Godot
/// <returns>The resulting quaternion of the interpolation.</returns>
public Quaternion Slerpni(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
real_t dot = Dot(to);
if (Mathf.Abs(dot) > 0.9999f)

48
modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs
View File

@ -119,23 +119,9 @@ namespace Godot
/// <returns>The interpolated transform.</returns>
public Transform3D InterpolateWith(Transform3D transform, real_t weight)
{
/* not sure if very "efficient" but good enough? */
Vector3 sourceScale = basis.Scale;
Quaternion sourceRotation = basis.GetRotationQuaternion();
Vector3 sourceLocation = origin;
Vector3 destinationScale = transform.basis.Scale;
Quaternion destinationRotation = transform.basis.GetRotationQuaternion();
Vector3 destinationLocation = transform.origin;
var interpolated = new Transform3D();
Quaternion quaternion = sourceRotation.Slerp(destinationRotation, weight).Normalized();
Vector3 scale = sourceScale.Lerp(destinationScale, weight);
interpolated.basis.SetQuaternionScale(quaternion, scale);
interpolated.origin = sourceLocation.Lerp(destinationLocation, weight);
return interpolated;
Basis retBasis = basis.Lerp(transform.basis, weight);
Vector3 retOrigin = origin.Lerp(transform.origin, weight);
return new Transform3D(retBasis, retOrigin);
}
/// <summary>
@ -234,6 +220,34 @@ namespace Godot
return new Transform3D(basis * tmpBasis, origin);
}
/// <summary>
/// Returns a transform spherically interpolated between this transform and
/// another <paramref name="transform"/> by <paramref name="weight"/>.
/// </summary>
/// <param name="transform">The other transform.</param>
/// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
/// <returns>The interpolated transform.</returns>
public Transform3D SphericalInterpolateWith(Transform3D transform, real_t weight)
{
/* not sure if very "efficient" but good enough? */
Vector3 sourceScale = basis.Scale;
Quaternion sourceRotation = basis.GetRotationQuaternion();
Vector3 sourceLocation = origin;
Vector3 destinationScale = transform.basis.Scale;
Quaternion destinationRotation = transform.basis.GetRotationQuaternion();
Vector3 destinationLocation = transform.origin;
var interpolated = new Transform3D();
Quaternion quaternion = sourceRotation.Slerp(destinationRotation, weight).Normalized();
Vector3 scale = sourceScale.Lerp(destinationScale, weight);
interpolated.basis.SetQuaternionScale(quaternion, scale);
interpolated.origin = sourceLocation.Lerp(destinationLocation, weight);
return interpolated;
}
private void SetLookAt(Vector3 eye, Vector3 target, Vector3 up)
{
// Make rotation matrix

23
modules/mono/glue/GodotSharp/GodotSharp/Core/Vector2.cs
View File

@ -215,6 +215,29 @@ namespace Godot
);
}
/// <summary>
/// Performs a cubic interpolation between vectors <paramref name="preA"/>, this vector,
/// <paramref name="b"/>, and <paramref name="postB"/>, by the given amount <paramref name="weight"/>.
/// It can perform smoother interpolation than <see cref="CubicInterpolate"/>
/// by the time values.
/// </summary>
/// <param name="b">The destination vector.</param>
/// <param name="preA">A vector before this vector.</param>
/// <param name="postB">A vector after <paramref name="b"/>.</param>
/// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
/// <param name="t"></param>
/// <param name="preAT"></param>
/// <param name="postBT"></param>
/// <returns>The interpolated vector.</returns>
public Vector2 CubicInterpolateInTime(Vector2 b, Vector2 preA, Vector2 postB, real_t weight, real_t t, real_t preAT, real_t postBT)
{
return new Vector2
(
Mathf.CubicInterpolateInTime(x, b.x, preA.x, postB.x, weight, t, preAT, postBT),
Mathf.CubicInterpolateInTime(y, b.y, preA.y, postB.y, weight, t, preAT, postBT)
);
}
/// <summary>
/// Returns the point at the given <paramref name="t"/> on a one-dimensional Bezier curve defined by this vector
/// and the given <paramref name="control1"/>, <paramref name="control2"/> and <paramref name="end"/> points.

24
modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs
View File

@ -208,6 +208,30 @@ namespace Godot
);
}
/// <summary>
/// Performs a cubic interpolation between vectors <paramref name="preA"/>, this vector,
/// <paramref name="b"/>, and <paramref name="postB"/>, by the given amount <paramref name="weight"/>.
/// It can perform smoother interpolation than <see cref="CubicInterpolate"/>
/// by the time values.
/// </summary>
/// <param name="b">The destination vector.</param>
/// <param name="preA">A vector before this vector.</param>
/// <param name="postB">A vector after <paramref name="b"/>.</param>
/// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
/// <param name="t"></param>
/// <param name="preAT"></param>
/// <param name="postBT"></param>
/// <returns>The interpolated vector.</returns>
public Vector3 CubicInterpolateInTime(Vector3 b, Vector3 preA, Vector3 postB, real_t weight, real_t t, real_t preAT, real_t postBT)
{
return new Vector3
(
Mathf.CubicInterpolateInTime(x, b.x, preA.x, postB.x, weight, t, preAT, postBT),
Mathf.CubicInterpolateInTime(y, b.y, preA.y, postB.y, weight, t, preAT, postBT),
Mathf.CubicInterpolateInTime(z, b.z, preA.z, postB.z, weight, t, preAT, postBT)
);
}
/// <summary>
/// Returns the point at the given <paramref name="t"/> on a one-dimensional Bezier curve defined by this vector
/// and the given <paramref name="control1"/>, <paramref name="control2"/> and <paramref name="end"/> points.

25
modules/mono/glue/GodotSharp/GodotSharp/Core/Vector4.cs
View File

@ -192,6 +192,31 @@ namespace Godot
);
}
/// <summary>
/// Performs a cubic interpolation between vectors <paramref name="preA"/>, this vector,
/// <paramref name="b"/>, and <paramref name="postB"/>, by the given amount <paramref name="weight"/>.
/// It can perform smoother interpolation than <see cref="CubicInterpolate"/>
/// by the time values.
/// </summary>
/// <param name="b">The destination vector.</param>
/// <param name="preA">A vector before this vector.</param>
/// <param name="postB">A vector after <paramref name="b"/>.</param>
/// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
/// <param name="t"></param>
/// <param name="preAT"></param>
/// <param name="postBT"></param>
/// <returns>The interpolated vector.</returns>
public Vector4 CubicInterpolateInTime(Vector4 b, Vector4 preA, Vector4 postB, real_t weight, real_t t, real_t preAT, real_t postBT)
{
return new Vector4
(
Mathf.CubicInterpolateInTime(x, b.x, preA.x, postB.x, weight, t, preAT, postBT),
Mathf.CubicInterpolateInTime(y, b.y, preA.y, postB.y, weight, t, preAT, postBT),
Mathf.CubicInterpolateInTime(y, b.z, preA.z, postB.z, weight, t, preAT, postBT),
Mathf.CubicInterpolateInTime(w, b.w, preA.w, postB.w, weight, t, preAT, postBT)
);
}
/// <summary>
/// Returns the normalized vector pointing from this vector to <paramref name="to"/>.
/// </summary>