Relintai/pandemonium_engine
1
0
Fork 0
mirror of https://github.com/Relintai/pandemonium_engine.git synced 2024-12-27 06:07:14 +01:00
Code Issues Packages Projects Releases Wiki Activity

Backported improvements to Quaternion from Godot4. Also bound all eligible methods.

Browse Source
This commit is contained in:
Relintai 2022-08-15 13:00:32 +02:00
parent f73af59416
commit 4cc1e75f69
3 changed files with 100 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
core/math/quaternion.cpp
View File

@ -250,6 +250,56 @@ Quaternion Quaternion::cubic_slerp(const Quaternion &p_b, const Quaternion &p_pr
return sp.slerpni(sq, t2); 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 { Vector3 Quaternion::get_axis() const {
if (Math::abs(w) > 1 - CMP_EPSILON) { if (Math::abs(w) > 1 - CMP_EPSILON) {
return Vector3(x, y, z); return Vector3(x, y, z);
@ -263,7 +313,7 @@ float Quaternion::get_angle() const {
} }
Quaternion::operator String() 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) { void Quaternion::set_axis_angle(const Vector3 &axis, const real_t &angle) {

18
core/math/quaternion.h
View File

@ -37,7 +37,22 @@
class _NO_DISCARD_CLASS_ Quaternion { class _NO_DISCARD_CLASS_ Quaternion {
public: 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; _FORCE_INLINE_ real_t length_squared() const;
bool is_equal_approx(const Quaternion &p_quat) 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 slerp(const Quaternion &p_to, const real_t &p_weight) const;
Quaternion slerpni(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 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; Vector3 get_axis() const;
float get_angle() const; float get_angle() const;

46
core/variant_call.cpp
View File

@ -455,7 +455,6 @@ struct _VariantCall {
VCALL_LOCALMEM2R(Vector2i, linear_interpolate); VCALL_LOCALMEM2R(Vector2i, linear_interpolate);
VCALL_LOCALMEM0R(Rect2, get_position); VCALL_LOCALMEM0R(Rect2, get_position);
VCALL_LOCALMEM1(Rect2, set_position); VCALL_LOCALMEM1(Rect2, set_position);
VCALL_LOCALMEM0R(Rect2, get_size); VCALL_LOCALMEM0R(Rect2, get_size);
@ -602,21 +601,31 @@ struct _VariantCall {
VCALL_LOCALMEM1R(Plane, is_equal_approx); VCALL_LOCALMEM1R(Plane, is_equal_approx);
VCALL_LOCALMEM1R(Plane, is_equal_approx_any_side); VCALL_LOCALMEM1R(Plane, is_equal_approx_any_side);
VCALL_LOCALMEM0R(Quaternion, length);
VCALL_LOCALMEM0R(Quaternion, length_squared); 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, normalized);
VCALL_LOCALMEM0R(Quaternion, is_normalized); VCALL_LOCALMEM0R(Quaternion, is_normalized);
VCALL_LOCALMEM1R(Quaternion, is_equal_approx);
VCALL_LOCALMEM0R(Quaternion, inverse); VCALL_LOCALMEM0R(Quaternion, inverse);
VCALL_LOCALMEM1R(Quaternion, angle_to); VCALL_LOCALMEM0R(Quaternion, log);
VCALL_LOCALMEM0R(Quaternion, exp);
VCALL_LOCALMEM1R(Quaternion, dot); 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, slerp);
VCALL_LOCALMEM2R(Quaternion, slerpni); VCALL_LOCALMEM2R(Quaternion, slerpni);
VCALL_LOCALMEM4R(Quaternion, cubic_slerp); VCALL_LOCALMEM4R(Quaternion, cubic_slerp);
VCALL_LOCALMEM0R(Quaternion, get_euler); VCALL_LOCALMEM4R(Quaternion, spherical_cubic_interpolate);
VCALL_LOCALMEM1(Quaternion, set_euler); VCALL_LOCALMEM0R(Quaternion, get_axis);
VCALL_LOCALMEM0R(Quaternion, get_angle);
VCALL_LOCALMEM2(Quaternion, set_axis_angle); VCALL_LOCALMEM2(Quaternion, set_axis_angle);
VCALL_LOCALMEM1R(Quaternion, xform);
VCALL_LOCALMEM0R(Color, to_rgba32); VCALL_LOCALMEM0R(Color, to_rgba32);
VCALL_LOCALMEM0R(Color, to_argb32); VCALL_LOCALMEM0R(Color, to_argb32);
@ -1042,7 +1051,6 @@ struct _VariantCall {
r_ret = reinterpret_cast<m_type *>(p_self._data._ptr)->m_method(*p_args[0], *p_args[1], *p_args[2], *p_args[3], *p_args[4]); \ r_ret = reinterpret_cast<m_type *>(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, get_volume);
VCALL_PTR0R(AABB, has_no_volume); VCALL_PTR0R(AABB, has_no_volume);
VCALL_PTR0R(AABB, has_no_surface); 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, PLANE, "plane", varray());
ADDFUNC1R(PLANE, BOOL, Plane, is_equal_approx_any_side, 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()); 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, QUATERNION, Quaternion, normalized, varray());
ADDFUNC0R(QUATERNION, BOOL, Quaternion, is_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()); 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, 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, slerp, QUATERNION, "to", REAL, "weight", varray());
ADDFUNC2R(QUATERNION, QUATERNION, Quaternion, slerpni, 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()); ADDFUNC4R(QUATERNION, QUATERNION, Quaternion, cubic_slerp, QUATERNION, "b", QUATERNION, "pre_a", QUATERNION, "post_b", REAL, "weight", varray());
ADDFUNC0R(QUATERNION, VECTOR3, Quaternion, get_euler, varray()); ADDFUNC4R(QUATERNION, QUATERNION, Quaternion, spherical_cubic_interpolate, QUATERNION, "b", QUATERNION, "pre_a", QUATERNION, "post_b", REAL, "weight", varray());
ADDFUNC1(QUATERNION, NIL, Quaternion, set_euler, VECTOR3, "euler", 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()); 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_rgba32, varray());
ADDFUNC0R(COLOR, INT, Color, to_argb32, varray()); ADDFUNC0R(COLOR, INT, Color, to_argb32, varray());