godotengine/godot
1
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2025-12-03 16:55:53 +00:00
Code Releases Wiki Activity

C#: Rename and fix Quaternion.SphericalCubicInterpolate

Browse Source
This commit is contained in:
Raul Santos
2022-08-24 12:03:53 +02:00
parent f72b7a1595
commit 4c5cefe4fe
1 changed files with 42 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -132,7 +132,7 @@ namespace Godot
} }
/// <summary> /// <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"/>. /// <paramref name="b"/>, and <paramref name="postB"/>, by the given amount <paramref name="weight"/>.
/// </summary> /// </summary>
/// <param name="b">The destination quaternion.</param> /// <param name="b">The destination quaternion.</param>
@@ -140,45 +140,59 @@ namespace Godot
/// <param name="postB">A quaternion after <paramref name="b"/>.</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="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
/// <returns>The interpolated quaternion.</returns> /// <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)
{ {
#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. // Align flip phases.
Quaternion retQ = new Basis(this).GetRotationQuaternion(); Quaternion fromQ = new Basis(this).GetRotationQuaternion();
Quaternion preQ = new Basis(preA).GetRotationQuaternion(); Quaternion preQ = new Basis(preA).GetRotationQuaternion();
Quaternion toQ = new Basis(b).GetRotationQuaternion(); Quaternion toQ = new Basis(b).GetRotationQuaternion();
Quaternion postQ = new Basis(postB).GetRotationQuaternion(); Quaternion postQ = new Basis(postB).GetRotationQuaternion();
// Flip quaternions to shortest path if necessary. // Flip quaternions to shortest path if necessary.
bool flip1 = Math.Sign(retQ.Dot(preQ)) < 0; bool flip1 = Math.Sign(fromQ.Dot(preQ)) < 0;
preQ = flip1 ? -preQ : preQ; preQ = flip1 ? -preQ : preQ;
bool flip2 = Math.Sign(retQ.Dot(toQ)) < 0; bool flip2 = Math.Sign(fromQ.Dot(toQ)) < 0;
toQ = flip2 ? -toQ : toQ; toQ = flip2 ? -toQ : toQ;
bool flip3 = flip2 ? toQ.Dot(postQ) <= 0 : Math.Sign(toQ.Dot(postQ)) < 0; bool flip3 = flip2 ? toQ.Dot(postQ) <= 0 : Math.Sign(toQ.Dot(postQ)) < 0;
postQ = flip3 ? -postQ : postQ; postQ = flip3 ? -postQ : postQ;
if (flip1 || flip2 || flip3) // Calc by Expmap in fromQ space.
{ Quaternion lnFrom = new Quaternion(0, 0, 0, 0);
// Angle is too large, calc by Approximate. Quaternion lnTo = (fromQ.Inverse() * toQ).Log();
retQ.x = Mathf.CubicInterpolate(retQ.x, toQ.x, preQ.x, postQ.x, weight); Quaternion lnPre = (fromQ.Inverse() * preQ).Log();
retQ.y = Mathf.CubicInterpolate(retQ.y, toQ.y, preQ.y, postQ.y, weight); Quaternion lnPost = (fromQ.Inverse() * postQ).Log();
retQ.z = Mathf.CubicInterpolate(retQ.z, toQ.z, preQ.z, postQ.z, weight); Quaternion ln = new Quaternion(
retQ.w = Mathf.CubicInterpolate(retQ.w, toQ.w, preQ.w, postQ.w, weight); Mathf.CubicInterpolate(lnFrom.x, lnTo.x, lnPre.x, lnPost.x, weight),
retQ = retQ.Normalized(); Mathf.CubicInterpolate(lnFrom.y, lnTo.y, lnPre.y, lnPost.y, weight),
} Mathf.CubicInterpolate(lnFrom.z, lnTo.z, lnPre.z, lnPost.z, weight),
else 0);
{ Quaternion q1 = fromQ * ln.Exp();
// Calc by Expmap.
Quaternion ln_ret = retQ.Log(); // Calc by Expmap in toQ space.
Quaternion ln_to = toQ.Log(); lnFrom = (toQ.Inverse() * fromQ).Log();
Quaternion ln_pre = preQ.Log(); lnTo = new Quaternion(0, 0, 0, 0);
Quaternion ln_post = postQ.Log(); lnPre = (toQ.Inverse() * preQ).Log();
Quaternion ln = new Quaternion(0, 0, 0, 0); lnPost = (toQ.Inverse() * postQ).Log();
ln.x = Mathf.CubicInterpolate(ln_ret.x, ln_to.x, ln_pre.x, ln_post.x, weight); ln = new Quaternion(
ln.y = Mathf.CubicInterpolate(ln_ret.y, ln_to.y, ln_pre.y, ln_post.y, weight); Mathf.CubicInterpolate(lnFrom.x, lnTo.x, lnPre.x, lnPost.x, weight),
ln.z = Mathf.CubicInterpolate(ln_ret.z, ln_to.z, ln_pre.z, ln_post.z, weight); Mathf.CubicInterpolate(lnFrom.y, lnTo.y, lnPre.y, lnPost.y, weight),
retQ = ln.Exp(); Mathf.CubicInterpolate(lnFrom.z, lnTo.z, lnPre.z, lnPost.z, weight),
} 0);
return retQ; Quaternion q2 = toQ * ln.Exp();
// To cancel error made by Expmap ambiguity, do blends.
return q1.Slerp(q2, weight);
} }
/// <summary> /// <summary>