Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Sync Quaternion with the version in Godot #1630

Merged
merged 1 commit into from
Oct 29, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
86 changes: 42 additions & 44 deletions include/godot_cpp/variant/quaternion.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
#ifndef GODOT_QUATERNION_HPP
#define GODOT_QUATERNION_HPP

#include <godot_cpp/classes/global_constants.hpp>
#include <godot_cpp/core/math.hpp>
#include <godot_cpp/variant/vector3.hpp>

Expand All @@ -47,11 +48,11 @@ struct _NO_DISCARD_ Quaternion {
real_t components[4] = { 0, 0, 0, 1.0 };
};

_FORCE_INLINE_ real_t &operator[](int idx) {
return components[idx];
_FORCE_INLINE_ real_t &operator[](int p_idx) {
return components[p_idx];
}
_FORCE_INLINE_ const real_t &operator[](int idx) const {
return components[idx];
_FORCE_INLINE_ const real_t &operator[](int p_idx) const {
return components[p_idx];
}
_FORCE_INLINE_ real_t length_squared() const;
bool is_equal_approx(const Quaternion &p_quaternion) const;
Expand All @@ -66,14 +67,13 @@ struct _NO_DISCARD_ Quaternion {
_FORCE_INLINE_ real_t dot(const Quaternion &p_q) const;
real_t angle_to(const Quaternion &p_to) const;

Vector3 get_euler_xyz() const;
Vector3 get_euler_yxz() const;
Vector3 get_euler() const { return get_euler_yxz(); }
Vector3 get_euler(EulerOrder p_order = EulerOrder::EULER_ORDER_YXZ) const;
static Quaternion from_euler(const Vector3 &p_euler);

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 spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const;
Quaternion spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const;
Quaternion slerp(const Quaternion &p_to, real_t p_weight) const;
Quaternion slerpni(const Quaternion &p_to, real_t p_weight) const;
Quaternion spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, real_t p_weight) const;
Quaternion spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const;

Vector3 get_axis() const;
real_t get_angle() const;
Expand All @@ -89,28 +89,28 @@ struct _NO_DISCARD_ Quaternion {
void operator*=(const Quaternion &p_q);
Quaternion operator*(const Quaternion &p_q) const;

_FORCE_INLINE_ Vector3 xform(const Vector3 &v) const {
_FORCE_INLINE_ Vector3 xform(const Vector3 &p_v) const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V_MSG(!is_normalized(), v, "The quaternion must be normalized.");
ERR_FAIL_COND_V_MSG(!is_normalized(), p_v, "The quaternion " + operator String() + " must be normalized.");
#endif
Vector3 u(x, y, z);
Vector3 uv = u.cross(v);
return v + ((uv * w) + u.cross(uv)) * ((real_t)2);
Vector3 uv = u.cross(p_v);
return p_v + ((uv * w) + u.cross(uv)) * ((real_t)2);
}

_FORCE_INLINE_ Vector3 xform_inv(const Vector3 &v) const {
return inverse().xform(v);
_FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_v) const {
return inverse().xform(p_v);
}

_FORCE_INLINE_ void operator+=(const Quaternion &p_q);
_FORCE_INLINE_ void operator-=(const Quaternion &p_q);
_FORCE_INLINE_ void operator*=(const real_t &s);
_FORCE_INLINE_ void operator/=(const real_t &s);
_FORCE_INLINE_ Quaternion operator+(const Quaternion &q2) const;
_FORCE_INLINE_ Quaternion operator-(const Quaternion &q2) const;
_FORCE_INLINE_ void operator*=(real_t p_s);
_FORCE_INLINE_ void operator/=(real_t p_s);
_FORCE_INLINE_ Quaternion operator+(const Quaternion &p_q2) const;
_FORCE_INLINE_ Quaternion operator-(const Quaternion &p_q2) const;
_FORCE_INLINE_ Quaternion operator-() const;
_FORCE_INLINE_ Quaternion operator*(const real_t &s) const;
_FORCE_INLINE_ Quaternion operator/(const real_t &s) const;
_FORCE_INLINE_ Quaternion operator*(real_t p_s) const;
_FORCE_INLINE_ Quaternion operator/(real_t p_s) const;

_FORCE_INLINE_ bool operator==(const Quaternion &p_quaternion) const;
_FORCE_INLINE_ bool operator!=(const Quaternion &p_quaternion) const;
Expand All @@ -128,8 +128,6 @@ struct _NO_DISCARD_ Quaternion {

Quaternion(const Vector3 &p_axis, real_t p_angle);

Quaternion(const Vector3 &p_euler);

Quaternion(const Quaternion &p_q) :
x(p_q.x),
y(p_q.y),
Expand All @@ -144,9 +142,9 @@ struct _NO_DISCARD_ Quaternion {
w = p_q.w;
}

Quaternion(const Vector3 &v0, const Vector3 &v1) { // Shortest arc.
Vector3 c = v0.cross(v1);
real_t d = v0.dot(v1);
Quaternion(const Vector3 &p_v0, const Vector3 &p_v1) { // Shortest arc.
Vector3 c = p_v0.cross(p_v1);
real_t d = p_v0.dot(p_v1);

if (d < -1.0f + (real_t)CMP_EPSILON) {
x = 0;
Expand Down Expand Up @@ -187,38 +185,38 @@ void Quaternion::operator-=(const Quaternion &p_q) {
w -= p_q.w;
}

void Quaternion::operator*=(const real_t &s) {
x *= s;
y *= s;
z *= s;
w *= s;
void Quaternion::operator*=(real_t p_s) {
x *= p_s;
y *= p_s;
z *= p_s;
w *= p_s;
}

void Quaternion::operator/=(const real_t &s) {
*this *= 1.0f / s;
void Quaternion::operator/=(real_t p_s) {
*this *= 1.0f / p_s;
}

Quaternion Quaternion::operator+(const Quaternion &q2) const {
Quaternion Quaternion::operator+(const Quaternion &p_q2) const {
const Quaternion &q1 = *this;
return Quaternion(q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w);
return Quaternion(q1.x + p_q2.x, q1.y + p_q2.y, q1.z + p_q2.z, q1.w + p_q2.w);
}

Quaternion Quaternion::operator-(const Quaternion &q2) const {
Quaternion Quaternion::operator-(const Quaternion &p_q2) const {
const Quaternion &q1 = *this;
return Quaternion(q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w);
return Quaternion(q1.x - p_q2.x, q1.y - p_q2.y, q1.z - p_q2.z, q1.w - p_q2.w);
}

Quaternion Quaternion::operator-() const {
const Quaternion &q2 = *this;
return Quaternion(-q2.x, -q2.y, -q2.z, -q2.w);
}

Quaternion Quaternion::operator*(const real_t &s) const {
return Quaternion(x * s, y * s, z * s, w * s);
Quaternion Quaternion::operator*(real_t p_s) const {
return Quaternion(x * p_s, y * p_s, z * p_s, w * p_s);
}

Quaternion Quaternion::operator/(const real_t &s) const {
return *this * (1.0f / s);
Quaternion Quaternion::operator/(real_t p_s) const {
return *this * (1.0f / p_s);
}

bool Quaternion::operator==(const Quaternion &p_quaternion) const {
Expand All @@ -229,7 +227,7 @@ bool Quaternion::operator!=(const Quaternion &p_quaternion) const {
return x != p_quaternion.x || y != p_quaternion.y || z != p_quaternion.z || w != p_quaternion.w;
}

_FORCE_INLINE_ Quaternion operator*(const real_t &p_real, const Quaternion &p_quaternion) {
_FORCE_INLINE_ Quaternion operator*(real_t p_real, const Quaternion &p_quaternion) {
return p_quaternion * p_real;
}

Expand Down
68 changes: 28 additions & 40 deletions src/variant/quaternion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,28 +37,15 @@ namespace godot {

real_t Quaternion::angle_to(const Quaternion &p_to) const {
real_t d = dot(p_to);
return Math::acos(CLAMP(d * d * 2 - 1, -1, 1));
// acos does clamping.
return Math::acos(d * d * 2 - 1);
}

// get_euler_xyz returns a vector containing the Euler angles in the format
// (ax,ay,az), where ax is the angle of rotation around x axis,
// and similar for other axes.
// This implementation uses XYZ convention (Z is the first rotation).
Vector3 Quaternion::get_euler_xyz() const {
Basis m(*this);
return m.get_euler(EULER_ORDER_XYZ);
}

// get_euler_yxz returns a vector containing the Euler angles in the format
// (ax,ay,az), where ax is the angle of rotation around x axis,
// and similar for other axes.
// This implementation uses YXZ convention (Z is the first rotation).
Vector3 Quaternion::get_euler_yxz() const {
Vector3 Quaternion::get_euler(EulerOrder p_order) const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V_MSG(!is_normalized(), Vector3(0, 0, 0), "The quaternion must be normalized.");
ERR_FAIL_COND_V_MSG(!is_normalized(), Vector3(0, 0, 0), "The quaternion " + operator String() + " must be normalized.");
#endif
Basis m(*this);
return m.get_euler(EULER_ORDER_YXZ);
return Basis(*this).get_euler(p_order);
}

void Quaternion::operator*=(const Quaternion &p_q) {
Expand Down Expand Up @@ -103,7 +90,7 @@ bool Quaternion::is_normalized() const {

Quaternion Quaternion::inverse() const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The quaternion must be normalized.");
ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The quaternion " + operator String() + " must be normalized.");
#endif
return Quaternion(-x, -y, -z, w);
}
Expand All @@ -125,10 +112,10 @@ Quaternion Quaternion::exp() const {
return Quaternion(src_v, theta);
}

Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) const {
Quaternion Quaternion::slerp(const Quaternion &p_to, 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_to.is_normalized(), Quaternion(), "The end quaternion must be normalized.");
ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion " + operator String() + " must be normalized.");
ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quaternion(), "The end quaternion " + p_to.operator String() + " must be normalized.");
#endif
Quaternion to1;
real_t omega, cosom, sinom, scale0, scale1;
Expand Down Expand Up @@ -166,10 +153,10 @@ Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) con
scale0 * w + scale1 * to1.w);
}

Quaternion Quaternion::slerpni(const Quaternion &p_to, const real_t &p_weight) const {
Quaternion Quaternion::slerpni(const Quaternion &p_to, 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_to.is_normalized(), Quaternion(), "The end quaternion must be normalized.");
ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion " + operator String() + " must be normalized.");
ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quaternion(), "The end quaternion " + p_to.operator String() + " must be normalized.");
#endif
const Quaternion &from = *this;

Expand All @@ -190,10 +177,10 @@ Quaternion Quaternion::slerpni(const Quaternion &p_to, const real_t &p_weight) c
invFactor * from.w + newFactor * p_to.w);
}

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 {
Quaternion Quaternion::spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, 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.");
ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion " + operator String() + " must be normalized.");
ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion " + p_b.operator String() + " must be normalized.");
#endif
Quaternion from_q = *this;
Quaternion pre_q = p_pre_a;
Expand Down Expand Up @@ -236,15 +223,15 @@ Quaternion Quaternion::spherical_cubic_interpolate(const Quaternion &p_b, const
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.
// To cancel error made by Expmap ambiguity, do blending.
return q1.slerp(q2, p_weight);
}

Quaternion Quaternion::spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight,
const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const {
Quaternion Quaternion::spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, real_t p_weight,
real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) 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.");
ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion " + operator String() + " must be normalized.");
ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion " + p_b.operator String() + " must be normalized.");
#endif
Quaternion from_q = *this;
Quaternion pre_q = p_pre_a;
Expand Down Expand Up @@ -287,7 +274,7 @@ Quaternion Quaternion::spherical_cubic_interpolate_in_time(const Quaternion &p_b
ln.z = Math::cubic_interpolate_in_time(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
Quaternion q2 = to_q * ln.exp();

// To cancel error made by Expmap ambiguity, do blends.
// To cancel error made by Expmap ambiguity, do blending.
return q1.slerp(q2, p_weight);
}

Expand All @@ -309,7 +296,7 @@ real_t Quaternion::get_angle() const {

Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) {
#ifdef MATH_CHECKS
ERR_FAIL_COND_MSG(!p_axis.is_normalized(), "The axis Vector3 must be normalized.");
ERR_FAIL_COND_MSG(!p_axis.is_normalized(), "The axis Vector3 " + p_axis.operator String() + " must be normalized.");
#endif
real_t d = p_axis.length();
if (d == 0) {
Expand All @@ -332,7 +319,7 @@ Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) {
// (ax, ay, az), where ax is the angle of rotation around x axis,
// and similar for other axes.
// This implementation uses YXZ convention (Z is the first rotation).
Quaternion::Quaternion(const Vector3 &p_euler) {
Quaternion Quaternion::from_euler(const Vector3 &p_euler) {
real_t half_a1 = p_euler.y * 0.5f;
real_t half_a2 = p_euler.x * 0.5f;
real_t half_a3 = p_euler.z * 0.5f;
Expand All @@ -348,10 +335,11 @@ Quaternion::Quaternion(const Vector3 &p_euler) {
real_t cos_a3 = Math::cos(half_a3);
real_t sin_a3 = Math::sin(half_a3);

x = sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3;
y = sin_a1 * cos_a2 * cos_a3 - cos_a1 * sin_a2 * sin_a3;
z = -sin_a1 * sin_a2 * cos_a3 + cos_a1 * cos_a2 * sin_a3;
w = sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3;
return Quaternion(
sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3,
sin_a1 * cos_a2 * cos_a3 - cos_a1 * sin_a2 * sin_a3,
-sin_a1 * sin_a2 * cos_a3 + cos_a1 * cos_a2 * sin_a3,
sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3);
}

} // namespace godot
Loading