From 4cc1e75f693c293a1d88aec45b463320a55e3433 Mon Sep 17 00:00:00 2001 From: Relintai Date: Mon, 15 Aug 2022 13:00:32 +0200 Subject: [PATCH] Backported improvements to Quaternion from Godot4. Also bound all eligible methods. --- core/math/quaternion.cpp | 52 +++++++++++++++++++++++++++++++++++++++- core/math/quaternion.h | 18 +++++++++++++- core/variant_call.cpp | 46 ++++++++++++++++++++++++----------- 3 files changed, 100 insertions(+), 16 deletions(-) diff --git a/core/math/quaternion.cpp b/core/math/quaternion.cpp index 709a5aa68..d8e7bec34 100644 --- a/core/math/quaternion.cpp +++ b/core/math/quaternion.cpp @@ -250,6 +250,56 @@ Quaternion Quaternion::cubic_slerp(const Quaternion &p_b, const Quaternion &p_pr return sp.slerpni(sq, t2); } +Quaternion Quaternion::spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized."); + ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion must be normalized."); +#endif + Quaternion from_q = *this; + Quaternion pre_q = p_pre_a; + Quaternion to_q = p_b; + Quaternion post_q = p_post_b; + + // Align flip phases. + from_q = Basis(from_q).get_rotation_quaternion(); + pre_q = Basis(pre_q).get_rotation_quaternion(); + to_q = Basis(to_q).get_rotation_quaternion(); + post_q = Basis(post_q).get_rotation_quaternion(); + + // Flip quaternions to shortest path if necessary. + bool flip1 = signbit(from_q.dot(pre_q)); + pre_q = flip1 ? -pre_q : pre_q; + bool flip2 = signbit(from_q.dot(to_q)); + to_q = flip2 ? -to_q : to_q; + bool flip3 = flip2 ? to_q.dot(post_q) <= 0 : signbit(to_q.dot(post_q)); + post_q = flip3 ? -post_q : post_q; + + // Calc by Expmap in from_q space. + Quaternion ln_from = Quaternion(0, 0, 0, 0); + Quaternion ln_to = (from_q.inverse() * to_q).log(); + Quaternion ln_pre = (from_q.inverse() * pre_q).log(); + Quaternion ln_post = (from_q.inverse() * post_q).log(); + Quaternion ln = Quaternion(0, 0, 0, 0); + ln.x = Math::cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight); + ln.y = Math::cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight); + ln.z = Math::cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight); + Quaternion q1 = from_q * ln.exp(); + + // Calc by Expmap in to_q space. + ln_from = (to_q.inverse() * from_q).log(); + ln_to = Quaternion(0, 0, 0, 0); + ln_pre = (to_q.inverse() * pre_q).log(); + ln_post = (to_q.inverse() * post_q).log(); + ln = Quaternion(0, 0, 0, 0); + ln.x = Math::cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight); + ln.y = Math::cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight); + ln.z = Math::cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight); + Quaternion q2 = to_q * ln.exp(); + + // To cancel error made by Expmap ambiguity, do blends. + return q1.slerp(q2, p_weight); +} + Vector3 Quaternion::get_axis() const { if (Math::abs(w) > 1 - CMP_EPSILON) { return Vector3(x, y, z); @@ -263,7 +313,7 @@ float Quaternion::get_angle() const { } Quaternion::operator String() const { - return String::num(x) + ", " + String::num(y) + ", " + String::num(z) + ", " + String::num(w); + return "(" + String::num_real(x) + ", " + String::num_real(y) + ", " + String::num_real(z) + ", " + String::num_real(w) + ")"; } void Quaternion::set_axis_angle(const Vector3 &axis, const real_t &angle) { diff --git a/core/math/quaternion.h b/core/math/quaternion.h index 3129c8ea7..6b7af0348 100644 --- a/core/math/quaternion.h +++ b/core/math/quaternion.h @@ -37,7 +37,22 @@ class _NO_DISCARD_CLASS_ Quaternion { public: - real_t x, y, z, w; + union { + struct { + real_t x; + real_t y; + real_t z; + real_t w; + }; + real_t components[4]; + }; + + _FORCE_INLINE_ real_t &operator[](int idx) { + return components[idx]; + } + _FORCE_INLINE_ const real_t &operator[](int idx) const { + return components[idx]; + } _FORCE_INLINE_ real_t length_squared() const; bool is_equal_approx(const Quaternion &p_quat) const; @@ -62,6 +77,7 @@ public: Quaternion slerp(const Quaternion &p_to, const real_t &p_weight) const; Quaternion slerpni(const Quaternion &p_to, const real_t &p_weight) const; Quaternion cubic_slerp(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const; + Quaternion spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const; Vector3 get_axis() const; float get_angle() const; diff --git a/core/variant_call.cpp b/core/variant_call.cpp index 63f3003b2..06eec1d1b 100644 --- a/core/variant_call.cpp +++ b/core/variant_call.cpp @@ -455,7 +455,6 @@ struct _VariantCall { VCALL_LOCALMEM2R(Vector2i, linear_interpolate); - VCALL_LOCALMEM0R(Rect2, get_position); VCALL_LOCALMEM1(Rect2, set_position); VCALL_LOCALMEM0R(Rect2, get_size); @@ -602,21 +601,31 @@ struct _VariantCall { VCALL_LOCALMEM1R(Plane, is_equal_approx); VCALL_LOCALMEM1R(Plane, is_equal_approx_any_side); - VCALL_LOCALMEM0R(Quaternion, length); VCALL_LOCALMEM0R(Quaternion, length_squared); + VCALL_LOCALMEM1R(Quaternion, is_equal_approx); + VCALL_LOCALMEM0R(Quaternion, length); + VCALL_LOCALMEM0(Quaternion, normalize); VCALL_LOCALMEM0R(Quaternion, normalized); VCALL_LOCALMEM0R(Quaternion, is_normalized); - VCALL_LOCALMEM1R(Quaternion, is_equal_approx); VCALL_LOCALMEM0R(Quaternion, inverse); - VCALL_LOCALMEM1R(Quaternion, angle_to); + VCALL_LOCALMEM0R(Quaternion, log); + VCALL_LOCALMEM0R(Quaternion, exp); VCALL_LOCALMEM1R(Quaternion, dot); - VCALL_LOCALMEM1R(Quaternion, xform); + VCALL_LOCALMEM1R(Quaternion, angle_to); + VCALL_LOCALMEM0R(Quaternion, get_euler_xyz); + VCALL_LOCALMEM1(Quaternion, set_euler_xyz); + VCALL_LOCALMEM0R(Quaternion, get_euler_yxz); + VCALL_LOCALMEM1(Quaternion, set_euler_yxz); + VCALL_LOCALMEM0R(Quaternion, get_euler); + VCALL_LOCALMEM1(Quaternion, set_euler); VCALL_LOCALMEM2R(Quaternion, slerp); VCALL_LOCALMEM2R(Quaternion, slerpni); VCALL_LOCALMEM4R(Quaternion, cubic_slerp); - VCALL_LOCALMEM0R(Quaternion, get_euler); - VCALL_LOCALMEM1(Quaternion, set_euler); + VCALL_LOCALMEM4R(Quaternion, spherical_cubic_interpolate); + VCALL_LOCALMEM0R(Quaternion, get_axis); + VCALL_LOCALMEM0R(Quaternion, get_angle); VCALL_LOCALMEM2(Quaternion, set_axis_angle); + VCALL_LOCALMEM1R(Quaternion, xform); VCALL_LOCALMEM0R(Color, to_rgba32); VCALL_LOCALMEM0R(Color, to_argb32); @@ -1042,7 +1051,6 @@ struct _VariantCall { r_ret = reinterpret_cast(p_self._data._ptr)->m_method(*p_args[0], *p_args[1], *p_args[2], *p_args[3], *p_args[4]); \ } - VCALL_PTR0R(AABB, get_volume); VCALL_PTR0R(AABB, has_no_volume); VCALL_PTR0R(AABB, has_no_surface); @@ -2373,21 +2381,31 @@ void register_variant_methods() { ADDFUNC1R(PLANE, BOOL, Plane, is_equal_approx, PLANE, "plane", varray()); ADDFUNC1R(PLANE, BOOL, Plane, is_equal_approx_any_side, PLANE, "plane", varray()); - ADDFUNC0R(QUATERNION, REAL, Quaternion, length, varray()); ADDFUNC0R(QUATERNION, REAL, Quaternion, length_squared, varray()); + ADDFUNC1R(QUATERNION, BOOL, Quaternion, is_equal_approx, QUATERNION, "quat", varray()); + ADDFUNC0R(QUATERNION, REAL, Quaternion, length, varray()); + ADDFUNC0(QUATERNION, NIL, Quaternion, normalize, varray()); ADDFUNC0R(QUATERNION, QUATERNION, Quaternion, normalized, varray()); ADDFUNC0R(QUATERNION, BOOL, Quaternion, is_normalized, varray()); - ADDFUNC1R(QUATERNION, BOOL, Quaternion, is_equal_approx, QUATERNION, "quat", varray()); ADDFUNC0R(QUATERNION, QUATERNION, Quaternion, inverse, varray()); - ADDFUNC1R(QUATERNION, REAL, Quaternion, angle_to, QUATERNION, "to", varray()); + ADDFUNC0R(QUATERNION, QUATERNION, Quaternion, log, varray()); + ADDFUNC0R(QUATERNION, QUATERNION, Quaternion, exp, varray()); ADDFUNC1R(QUATERNION, REAL, Quaternion, dot, QUATERNION, "b", varray()); - ADDFUNC1R(QUATERNION, VECTOR3, Quaternion, xform, VECTOR3, "v", varray()); + ADDFUNC1R(QUATERNION, REAL, Quaternion, angle_to, QUATERNION, "to", varray()); + ADDFUNC0R(QUATERNION, VECTOR3, Quaternion, get_euler_xyz, varray()); + ADDFUNC1(QUATERNION, NIL, Quaternion, set_euler_xyz, VECTOR3, "euler", varray()); + ADDFUNC0R(QUATERNION, VECTOR3, Quaternion, get_euler_yxz, varray()); + ADDFUNC1(QUATERNION, NIL, Quaternion, set_euler_yxz, VECTOR3, "euler", varray()); + ADDFUNC0R(QUATERNION, VECTOR3, Quaternion, get_euler, varray()); + ADDFUNC1(QUATERNION, NIL, Quaternion, set_euler, VECTOR3, "euler", varray()); ADDFUNC2R(QUATERNION, QUATERNION, Quaternion, slerp, QUATERNION, "to", REAL, "weight", varray()); ADDFUNC2R(QUATERNION, QUATERNION, Quaternion, slerpni, QUATERNION, "to", REAL, "weight", varray()); ADDFUNC4R(QUATERNION, QUATERNION, Quaternion, cubic_slerp, QUATERNION, "b", QUATERNION, "pre_a", QUATERNION, "post_b", REAL, "weight", varray()); - ADDFUNC0R(QUATERNION, VECTOR3, Quaternion, get_euler, varray()); - ADDFUNC1(QUATERNION, NIL, Quaternion, set_euler, VECTOR3, "euler", varray()); + ADDFUNC4R(QUATERNION, QUATERNION, Quaternion, spherical_cubic_interpolate, QUATERNION, "b", QUATERNION, "pre_a", QUATERNION, "post_b", REAL, "weight", varray()); + ADDFUNC0R(QUATERNION, VECTOR3, Quaternion, get_axis, varray()); + ADDFUNC0R(QUATERNION, REAL, Quaternion, get_angle, varray()); ADDFUNC2(QUATERNION, NIL, Quaternion, set_axis_angle, VECTOR3, "axis", REAL, "angle", varray()); + ADDFUNC1R(QUATERNION, VECTOR3, Quaternion, xform, VECTOR3, "v", varray()); ADDFUNC0R(COLOR, INT, Color, to_rgba32, varray()); ADDFUNC0R(COLOR, INT, Color, to_argb32, varray());