From 9be45046c1a183331e2cc07131bfcbdb62e6c5cd Mon Sep 17 00:00:00 2001 From: Relintai Date: Mon, 14 Mar 2022 16:17:02 +0100 Subject: [PATCH] Finished up quat as much as I can. --- core/math/quat.c | 241 ++++++++++++++++++++++----------------------- core/math/quat.h | 247 ++++++++++++++++++++++------------------------- 2 files changed, 237 insertions(+), 251 deletions(-) diff --git a/core/math/quat.c b/core/math/quat.c index 79ba35f..c63e03a 100644 --- a/core/math/quat.c +++ b/core/math/quat.c @@ -33,34 +33,60 @@ //#include "core/math/basis.h" //#include "core/print_string.h" -/* +void quat_set_axis_angle(Quat *self, const Vector3 *axis, const real_t angle) { +#ifdef MATH_CHECKS + //ERR_FAIL_COND_MSG(!axis.is_normalized(), "The axis Vector3 must be normalized."); +#endif + real_t d = vector3_length(axis); + if (d == 0) { + quat_set(self, 0, 0, 0, 0); + } else { + real_t sin_angle = math_sinf(angle * 0.5f); + real_t cos_angle = math_cosf(angle * 0.5f); + real_t s = sin_angle / d; + quat_set(self, axis->x * s, axis->y * s, axis->z * s, cos_angle); + } +} -real_t Quat::angle_to(const Quat &p_to) const { - real_t d = dot(p_to); - return Math::acos(CLAMP(d * d * 2 - 1, -1, 1)); +Quat quat_create_ae(const Vector3 *axis, const real_t angle) { + Quat q; + quat_set_axis_angle(&q, axis, angle); + return q; +} + +Quat quat_create_euler(const Vector3 *euler) { + Quat q; + quat_set_euler(&q, euler); + return q; +} + +real_t quat_angle_to(const Quat *self, const Quat *p_to) { + real_t d = quat_dot(self, p_to); + return math_acosf(CLAMP(d * d * 2 - 1, -1, 1)); } // set_euler_xyz expects 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). -void Quat::set_euler_xyz(const Vector3 &p_euler) { - real_t half_a1 = p_euler.x * 0.5f; - real_t half_a2 = p_euler.y * 0.5f; - real_t half_a3 = p_euler.z * 0.5f; +void quat_set_euler_xyz(Quat *self, const Vector3 *p_euler) { + real_t half_a1 = p_euler->x * 0.5f; + real_t half_a2 = p_euler->y * 0.5f; + real_t half_a3 = p_euler->z * 0.5f; // R = X(a1).Y(a2).Z(a3) convention for Euler angles. // Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-2) // a3 is the angle of the first rotation, following the notation in this reference. - real_t cos_a1 = Math::cos(half_a1); - real_t sin_a1 = Math::sin(half_a1); - real_t cos_a2 = Math::cos(half_a2); - real_t sin_a2 = Math::sin(half_a2); - real_t cos_a3 = Math::cos(half_a3); - real_t sin_a3 = Math::sin(half_a3); + real_t cos_a1 = math_cosf(half_a1); + real_t sin_a1 = math_sinf(half_a1); + real_t cos_a2 = math_cosf(half_a2); + real_t sin_a2 = math_sinf(half_a2); + real_t cos_a3 = math_cosf(half_a3); + real_t sin_a3 = math_sinf(half_a3); - set(sin_a1 * cos_a2 * cos_a3 + sin_a2 * sin_a3 * cos_a1, + quat_set(self, + sin_a1 * cos_a2 * cos_a3 + sin_a2 * sin_a3 * cos_a1, -sin_a1 * sin_a3 * cos_a2 + sin_a2 * cos_a1 * cos_a3, sin_a1 * sin_a2 * cos_a3 + sin_a3 * cos_a1 * cos_a2, -sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3); @@ -70,32 +96,35 @@ void Quat::set_euler_xyz(const Vector3 &p_euler) { // (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 Quat::get_euler_xyz() const { - Basis m(*this); - return m.get_euler_xyz(); +Vector3 quat_get_euler_xyz(const Quat *self) { + //Basis m(*this); + //return m.get_euler_xyz(); + + return vector3_create(0, 0, 0); } // set_euler_yxz expects 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). -void Quat::set_euler_yxz(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; +void quat_set_euler_yxz(Quat *self, 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; // R = Y(a1).X(a2).Z(a3) convention for Euler angles. // Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-6) // a3 is the angle of the first rotation, following the notation in this reference. - real_t cos_a1 = Math::cos(half_a1); - real_t sin_a1 = Math::sin(half_a1); - real_t cos_a2 = Math::cos(half_a2); - real_t sin_a2 = Math::sin(half_a2); - real_t cos_a3 = Math::cos(half_a3); - real_t sin_a3 = Math::sin(half_a3); + real_t cos_a1 = math_cosf(half_a1); + real_t sin_a1 = math_sinf(half_a1); + real_t cos_a2 = math_cosf(half_a2); + real_t sin_a2 = math_sinf(half_a2); + real_t cos_a3 = math_cosf(half_a3); + real_t sin_a3 = math_sinf(half_a3); - set(sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3, + quat_set(self, + 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); @@ -105,87 +134,76 @@ void Quat::set_euler_yxz(const Vector3 &p_euler) { // (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 Quat::get_euler_yxz() const { +Vector3 quat_get_euler_yxz(const Quat *self) { #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 must be normalized."); #endif - Basis m(*this); - return m.get_euler_yxz(); + //Basis m(*this); + //return m.get_euler_yxz(); + + return vector3_create(0, 0, 0); } -void Quat::operator*=(const Quat &p_q) { - set(w * p_q.x + x * p_q.w + y * p_q.z - z * p_q.y, - w * p_q.y + y * p_q.w + z * p_q.x - x * p_q.z, - w * p_q.z + z * p_q.w + x * p_q.y - y * p_q.x, - w * p_q.w - x * p_q.x - y * p_q.y - z * p_q.z); +bool quat_is_equal_approx(const Quat *self, const Quat *p_quat) { + return math_is_equal_approxf(self->x, p_quat->x) && math_is_equal_approxf(self->y, p_quat->y) && math_is_equal_approxf(self->z, p_quat->z) && math_is_equal_approxf(self->w, p_quat->w); } -Quat Quat::operator*(const Quat &p_q) const { - Quat r = *this; - r *= p_q; - return r; +real_t quat_length(const Quat *self) { + return math_sqrtf(quat_length_squared(self)); } -bool Quat::is_equal_approx(const Quat &p_quat) const { - return Math::is_equal_approx(x, p_quat.x) && Math::is_equal_approx(y, p_quat.y) && Math::is_equal_approx(z, p_quat.z) && Math::is_equal_approx(w, p_quat.w); +void quat_normalize(Quat *self) { + quat_div_eqs(self, quat_length(self)); } -real_t Quat::length() const { - return Math::sqrt(length_squared()); +Quat quat_normalized(const Quat *self) { + return quat_divs(self, quat_length(self)); } -void Quat::normalize() { - *this /= length(); +bool quat_is_normalized(const Quat *self) { + return math_is_equal_approxft(quat_length_squared(self), 1, (real_t)UNIT_EPSILON); //use less epsilon } -Quat Quat::normalized() const { - return *this / length(); -} - -bool Quat::is_normalized() const { - return Math::is_equal_approx(length_squared(), 1, (real_t)UNIT_EPSILON); //use less epsilon -} - -Quat Quat::inverse() const { +Quat quat_inverse(const Quat *self) { #ifdef MATH_CHECKS - ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The quaternion must be normalized."); + //ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The quaternion must be normalized."); #endif - return Quat(-x, -y, -z, w); + return quat_creater(-self->x, -self->y, -self->z, self->w); } -Quat Quat::slerp(const Quat &p_to, const real_t &p_weight) const { +Quat quat_slerp(const Quat *self, const Quat *p_to, const real_t p_weight) { #ifdef MATH_CHECKS - ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The start quaternion must be normalized."); - ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quat(), "The end quaternion must be normalized."); + //ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The start quaternion must be normalized."); + //ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quat(), "The end quaternion must be normalized."); #endif Quat to1; real_t omega, cosom, sinom, scale0, scale1; // calc cosine - cosom = dot(p_to); + cosom = quat_dot(self, p_to); // adjust signs (if necessary) if (cosom < 0) { cosom = -cosom; - to1.x = -p_to.x; - to1.y = -p_to.y; - to1.z = -p_to.z; - to1.w = -p_to.w; + to1.x = -p_to->x; + to1.y = -p_to->y; + to1.z = -p_to->z; + to1.w = -p_to->w; } else { - to1.x = p_to.x; - to1.y = p_to.y; - to1.z = p_to.z; - to1.w = p_to.w; + to1.x = p_to->x; + to1.y = p_to->y; + to1.z = p_to->z; + to1.w = p_to->w; } // calculate coefficients if ((1 - cosom) > (real_t)CMP_EPSILON) { // standard case (slerp) - omega = Math::acos(cosom); - sinom = Math::sin(omega); - scale0 = Math::sin((1 - p_weight) * omega) / sinom; - scale1 = Math::sin(p_weight * omega) / sinom; + omega = math_acosf(cosom); + sinom = math_sinf(omega); + scale0 = math_sinf((1 - p_weight) * omega) / sinom; + scale1 = math_sinf(p_weight * omega) / sinom; } else { // "from" and "to" quaternions are very close // ... so we can do a linear interpolation @@ -193,66 +211,49 @@ Quat Quat::slerp(const Quat &p_to, const real_t &p_weight) const { scale1 = p_weight; } // calculate final values - return Quat( - scale0 * x + scale1 * to1.x, - scale0 * y + scale1 * to1.y, - scale0 * z + scale1 * to1.z, - scale0 * w + scale1 * to1.w); + return quat_creater( + scale0 * self->x + scale1 * to1.x, + scale0 * self->y + scale1 * to1.y, + scale0 * self->z + scale1 * to1.z, + scale0 * self->w + scale1 * to1.w); } -Quat Quat::slerpni(const Quat &p_to, const real_t &p_weight) const { +Quat quat_slerpni(const Quat *self, const Quat *p_to, const real_t p_weight) { #ifdef MATH_CHECKS - ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The start quaternion must be normalized."); - ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quat(), "The end quaternion must be normalized."); + //ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The start quaternion must be normalized."); + //ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quat(), "The end quaternion must be normalized."); #endif - const Quat &from = *this; + real_t dot = quat_dot(self, p_to); - real_t dot = from.dot(p_to); - - if (Math::absf(dot) > 0.9999f) { - return from; + if (math_absf(dot) > 0.9999f) { + return quat_createq(self); } - real_t theta = Math::acos(dot), - sinT = 1 / Math::sin(theta), - newFactor = Math::sin(p_weight * theta) * sinT, - invFactor = Math::sin((1 - p_weight) * theta) * sinT; + real_t theta = math_acosf(dot), + sinT = 1 / math_sinf(theta), + newFactor = math_sinf(p_weight * theta) * sinT, + invFactor = math_sinf((1 - p_weight) * theta) * sinT; - return Quat(invFactor * from.x + newFactor * p_to.x, - invFactor * from.y + newFactor * p_to.y, - invFactor * from.z + newFactor * p_to.z, - invFactor * from.w + newFactor * p_to.w); + return quat_creater(invFactor * self->x + newFactor * p_to->x, + invFactor * self->y + newFactor * p_to->y, + invFactor * self->z + newFactor * p_to->z, + invFactor * self->w + newFactor * p_to->w); } -Quat Quat::cubic_slerp(const Quat &p_b, const Quat &p_pre_a, const Quat &p_post_b, const real_t &p_weight) const { +Quat quat_cubic_slerp(const Quat *self, const Quat *p_b, const Quat *p_pre_a, const Quat *p_post_b, const real_t p_weight) { #ifdef MATH_CHECKS - ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The start quaternion must be normalized."); - ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quat(), "The end quaternion must be normalized."); + //ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The start quaternion must be normalized."); + //ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quat(), "The end quaternion must be normalized."); #endif //the only way to do slerp :| real_t t2 = (1 - p_weight) * p_weight * 2; - Quat sp = this->slerp(p_b, p_weight); - Quat sq = p_pre_a.slerpni(p_post_b, p_weight); - return sp.slerpni(sq, t2); + Quat sp = quat_slerp(self, p_b, p_weight); + Quat sq = quat_slerpni(p_pre_a, p_post_b, p_weight); + return quat_slerpni(&sp, &sq, t2); } -Quat::operator String() const { +/* +quat_operator String() const { return String::num(x) + ", " + String::num(y) + ", " + String::num(z) + ", " + String::num(w); } - -void Quat::set_axis_angle(const Vector3 &axis, const real_t &angle) { -#ifdef MATH_CHECKS - ERR_FAIL_COND_MSG(!axis.is_normalized(), "The axis Vector3 must be normalized."); -#endif - real_t d = axis.length(); - if (d == 0) { - set(0, 0, 0, 0); - } else { - real_t sin_angle = Math::sin(angle * 0.5f); - real_t cos_angle = Math::cos(angle * 0.5f); - real_t s = sin_angle / d; - set(axis.x * s, axis.y * s, axis.z * s, - cos_angle); - } -} -*/ \ No newline at end of file +*/ diff --git a/core/math/quat.h b/core/math/quat.h index 23f768e..b90fb56 100644 --- a/core/math/quat.h +++ b/core/math/quat.h @@ -47,7 +47,25 @@ extern _FORCE_INLINE_ void quat_set(Quat *self, real_t p_x, real_t p_y, real_t p self->w = p_z; } -extern _FORCE_INLINE_ Quat quat_create(real_t p_x, real_t p_y, real_t p_z, real_t p_w) { +extern _FORCE_INLINE_ void quat_setq(Quat *self, const Quat *p_q) { + self->x = p_q->x; + self->y = p_q->y; + self->z = p_q->z; + self->w = p_q->w; +} + +extern _FORCE_INLINE_ Quat quat_create() { + Quat q; + + q.x = 0; + q.y = 0; + q.z = 0; + q.w = 1; + + return q; +} + +extern _FORCE_INLINE_ Quat quat_creater(real_t p_x, real_t p_y, real_t p_z, real_t p_w) { Quat q; q.x = p_x; @@ -58,7 +76,7 @@ extern _FORCE_INLINE_ Quat quat_create(real_t p_x, real_t p_y, real_t p_z, real_ return q; } -extern _FORCE_INLINE_ Quat quat_createv(const Quat *other) { +extern _FORCE_INLINE_ Quat quat_createq(const Quat *other) { Quat q; q.x = other->x; @@ -69,6 +87,33 @@ extern _FORCE_INLINE_ Quat quat_createv(const Quat *other) { return q; } +extern _FORCE_INLINE_ Quat quat_createv(const Vector3 *v0, const Vector3 *v1) { + Quat q; + + Vector3 c = vector3_cross(v0, v1); + real_t d = vector3_dot(v0, v1); + + if (d < -1 + (real_t)CMP_EPSILON) { + q.x = 0; + q.y = 1; + q.z = 0; + q.w = 0; + } else { + real_t s = math_sqrtf((1 + d) * 2); + real_t rs = 1 / s; + + q.x = c.x * rs; + q.y = c.y * rs; + q.z = c.z * rs; + q.w = s * 0.5f; + } + + return q; +} + +Quat quat_create_ae(const Vector3 *axis, const real_t angle); +Quat quat_create_euler(const Vector3 *euler); + real_t quat_dot(const Quat *self, const Quat *p_q) { return self->x * p_q->x + self->y * p_q->y + self->z * p_q->z + self->w * p_q->w; } @@ -84,7 +129,7 @@ real_t quat_length_squaredc(const Quat self) { } extern _FORCE_INLINE_ Quat quat_add(const Quat *self, const Quat *p_q) { - return quat_create(self->x + p_q->x, self->y + p_q->y, self->z + p_q->z, self->w + p_q->w); + return quat_creater(self->x + p_q->x, self->y + p_q->y, self->z + p_q->z, self->w + p_q->w); } extern _FORCE_INLINE_ void quat_add_eq(Quat *self, const Quat *p_q) { self->x += p_q->x; @@ -94,10 +139,10 @@ extern _FORCE_INLINE_ void quat_add_eq(Quat *self, const Quat *p_q) { } extern _FORCE_INLINE_ Quat quat_subv(const Quat *self, const Quat *p_v) { - return quat_create(self->x - p_v->x, self->y - p_v->y, self->z - p_v->z, self->w - p_v->w); + return quat_creater(self->x - p_v->x, self->y - p_v->y, self->z - p_v->z, self->w - p_v->w); } extern _FORCE_INLINE_ Quat quat_subvc(const Quat self, const Quat p_v) { - return quat_create(self.x - p_v.x, self.y - p_v.y, self.z - p_v.z, self.w - p_v.w); + return quat_creater(self.x - p_v.x, self.y - p_v.y, self.z - p_v.z, self.w - p_v.w); } extern _FORCE_INLINE_ void quat_sub_eqv(Quat *self, const Quat *p_q) { self->x -= p_q->x; @@ -108,43 +153,44 @@ extern _FORCE_INLINE_ void quat_sub_eqv(Quat *self, const Quat *p_q) { // Other extern _FORCE_INLINE_ Quat quat_neg(const Quat *self) { - return quat_create(-(self->x), -(self->y), -(self->z), -(self->w)); + return quat_creater(-(self->x), -(self->y), -(self->z), -(self->w)); } extern _FORCE_INLINE_ Quat quat_negc(const Quat self) { - return quat_create(-(self.x), -(self.y), -(self.z), -(self.w)); + return quat_creater(-(self.x), -(self.y), -(self.z), -(self.w)); } extern _FORCE_INLINE_ Quat quat_mulq(const Quat *self, const Quat *p_v1) { - return quat_create(self->x * p_v1->x, self->y * p_v1->y, self->z * p_v1->z, self->w * p_v1->w); + return quat_creater(self->x * p_v1->x, self->y * p_v1->y, self->z * p_v1->z, self->w * p_v1->w); } extern _FORCE_INLINE_ Quat quat_mulqc(const Quat self, const Quat p_q) { - return quat_create(self.x * p_q.x, self.y * p_q.y, self.z * p_q.z, self.w * p_q.w); + return quat_creater(self.x * p_q.x, self.y * p_q.y, self.z * p_q.z, self.w * p_q.w); } -extern _FORCE_INLINE_ void quat_mul_eqq(Quat *self, const Quat *rvalue) { - self->x *= rvalue->x; - self->y *= rvalue->y; - self->z *= rvalue->z; - self->w *= rvalue->w; +extern _FORCE_INLINE_ void quat_mul_eqq(Quat *self, const Quat *p_q) { + quat_set(self, + self->w * p_q->x + self->x * p_q->w + self->y * p_q->z - self->z * p_q->y, + self->w * p_q->y + self->y * p_q->w + self->z * p_q->x - self->x * p_q->z, + self->w * p_q->z + self->z * p_q->w + self->x * p_q->y - self->y * p_q->x, + self->w * p_q->w - self->x * p_q->x - self->y * p_q->y - self->z * p_q->z); } extern _FORCE_INLINE_ Quat quat_mulv(const Quat *self, const Vector3 *v) { - return quat_create(self->w * v->x + self->y * v->z - self->z * v->y, + return quat_creater(self->w * v->x + self->y * v->z - self->z * v->y, self->w * v->y + self->z * v->x - self->x * v->z, self->w * v->z + self->x * v->y - self->y * v->x, -self->x * v->x - self->y * v->y - self->z * v->z); } extern _FORCE_INLINE_ Quat quat_mulvc(const Quat self, Vector3 v) { - return quat_create(self.w * v.x + self.y * v.z - self.z * v.y, + return quat_creater(self.w * v.x + self.y * v.z - self.z * v.y, self.w * v.y + self.z * v.x - self.x * v.z, self.w * v.z + self.x * v.y - self.y * v.x, -self.x * v.x - self.y * v.y - self.z * v.z); } extern _FORCE_INLINE_ Quat quat_muls(const Quat *self, const real_t rvalue) { - return quat_create(self->x * rvalue, self->y * rvalue, self->z * rvalue, self->w * rvalue); + return quat_creater(self->x * rvalue, self->y * rvalue, self->z * rvalue, self->w * rvalue); }; extern _FORCE_INLINE_ Quat quat_mulsc(Quat self, const real_t rvalue) { - return quat_create(self.x * rvalue, self.y * rvalue, self.z * rvalue, self.w * rvalue); + return quat_creater(self.x * rvalue, self.y * rvalue, self.z * rvalue, self.w * rvalue); }; extern _FORCE_INLINE_ void quat_mul_eqs(Quat *self, const real_t rvalue) { self->x *= rvalue; @@ -155,11 +201,11 @@ extern _FORCE_INLINE_ void quat_mul_eqs(Quat *self, const real_t rvalue) { extern _FORCE_INLINE_ Quat quat_divs(const Quat *self, const real_t rvalue) { real_t r = 1 / rvalue; - return quat_create(self->x * r, self->y * r, self->z * r, self->w * r); + return quat_creater(self->x * r, self->y * r, self->z * r, self->w * r); }; extern _FORCE_INLINE_ Quat quat_divsc(Quat self, const real_t rvalue) { real_t r = 1 / rvalue; - return quat_create(self.x * r, self.y * r, self.z * r, self.w * r); + return quat_creater(self.x * r, self.y * r, self.z * r, self.w * r); }; extern _FORCE_INLINE_ void quat_div_eqs(Quat *self, const real_t rvalue) { real_t r = 1 / rvalue; @@ -170,124 +216,63 @@ extern _FORCE_INLINE_ void quat_div_eqs(Quat *self, const real_t rvalue) { self->w *= r; }; - /* +//operator== +extern _FORCE_INLINE_ bool quat_eq(const Quat *self, const Quat *p_q) { + return self->x == p_q->x && self->y == p_q->y && self->z == p_q->z && self->w == p_q->w; +} +//operator!= +extern _FORCE_INLINE_ bool quat_neq(const Quat *self, const Quat *p_q) { + return self->x != p_q->x || self->y != p_q->y || self->z != p_q->z || self->w != p_q->w; +} +void quat_set_axis_angle(Quat *self, const Vector3 *axis, const real_t angle); +extern _FORCE_INLINE_ void quat_get_axis_angle(const Quat *self, Vector3 *r_axis, real_t *r_angle) { + *r_angle = 2 * math_acosf(self->w); + real_t r = ((real_t)1) / math_sqrtf(1 - self->w * self->w); + r_axis->x = self->x * r; + r_axis->y = self->y * r; + r_axis->z = self->z * r; +} - bool is_equal_approx(const Quat &p_quat) const; - real_t length() const; - void normalize(); - Quat normalized() const; - bool is_normalized() const; - Quat inverse() const; +extern _FORCE_INLINE_ Vector3 quat_xform(const Quat *self, const Vector3 *v) { +#ifdef MATH_CHECKS + //ERR_FAIL_COND_V_MSG(!is_normalized(), v, "The quaternion must be normalized."); +#endif + Vector3 u = vector3_create(self->x, self->y, self->z); + Vector3 uv = vector3_cross(&u, v); + Vector3 uvc = vector3_cross(&u, &uv); + Vector3 uvw = vector3_muls(&uv, self->w); + Vector3 f = vector3_mulsc(vector3_addv(&uvw, &uvc), ((real_t)2)); + return vector3_addv(v, &f); +} - real_t angle_to(const Quat &p_to) const; +real_t quat_angle_to(const Quat *self, const Quat *p_to); - void set_euler_xyz(const Vector3 &p_euler); - Vector3 get_euler_xyz() const; - void set_euler_yxz(const Vector3 &p_euler); - Vector3 get_euler_yxz() const; +bool quat_is_equal_approx(const Quat *self, const Quat *p_quat); +real_t quat_length(const Quat *self); +void quat_normalize(Quat *self); +Quat quat_normalized(const Quat *self); +bool quat_is_normalized(const Quat *self); +Quat quat_inverse(const Quat *self); - void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); }; - Vector3 get_euler() const { return get_euler_yxz(); }; +void quat_set_euler_xyz(Quat *self, const Vector3 *p_euler); +Vector3 quat_get_euler_xyz(const Quat *self); +void quat_set_euler_yxz(Quat *self, const Vector3 *p_euler); +Vector3 quat_get_euler_yxz(const Quat *self); - Quat slerp(const Quat &p_to, const real_t &p_weight) const; - Quat slerpni(const Quat &p_to, const real_t &p_weight) const; - Quat cubic_slerp(const Quat &p_b, const Quat &p_pre_a, const Quat &p_post_b, const real_t &p_weight) const; +void quat_set_euler(Quat *self, const Vector3 *p_euler) { + quat_set_euler_yxz(self, p_euler); +}; +Vector3 quat_get_euler(const Quat *self) { + return quat_get_euler_yxz(self); +}; - void set_axis_angle(const Vector3 &axis, const real_t &angle); - _FORCE_INLINE_ void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { - r_angle = 2 * Math::acos(w); - real_t r = ((real_t)1) / Math::sqrt(1 - w * w); - r_axis.x = x * r; - r_axis.y = y * r; - r_axis.z = z * r; - } +Quat quat_slerp(const Quat *self, const Quat *p_to, const real_t p_weight); +Quat quat_slerpni(const Quat *self, const Quat *p_to, const real_t p_weight); +Quat quat_cubic_slerp(const Quat *self, const Quat *p_b, const Quat *p_pre_a, const Quat *p_post_b, const real_t p_weight); - - - _FORCE_INLINE_ Vector3 xform(const Vector3 &v) const { - #ifdef MATH_CHECKS - ERR_FAIL_COND_V_MSG(!is_normalized(), v, "The quaternion must be normalized."); - #endif - Vector3 u(x, y, z); - Vector3 uv = u.cross(v); - return v + ((uv * w) + u.cross(uv)) * ((real_t)2); - } - - operator String() const; - - inline void set(real_t p_x, real_t p_y, real_t p_z, real_t p_w) { - x = p_x; - y = p_y; - z = p_z; - w = p_w; - } - inline Quat(real_t p_x, real_t p_y, real_t p_z, real_t p_w) : - x(p_x), - y(p_y), - z(p_z), - w(p_w) { - } - Quat(const Vector3 &axis, const real_t &angle) { set_axis_angle(axis, angle); } - - Quat(const Vector3 &euler) { set_euler(euler); } - Quat(const Quat &p_q) : - x(p_q.x), - y(p_q.y), - z(p_q.z), - w(p_q.w) { - } - - Quat &operator=(const Quat &p_q) { - x = p_q.x; - y = p_q.y; - z = p_q.z; - w = p_q.w; - return *this; - } - - Quat(const Vector3 &v0, const Vector3 &v1) // shortest arc - { - Vector3 c = v0.cross(v1); - real_t d = v0.dot(v1); - - if (d < -1 + (real_t)CMP_EPSILON) { - x = 0; - y = 1; - z = 0; - w = 0; - } else { - real_t s = Math::sqrt((1 + d) * 2); - real_t rs = 1 / s; - - x = c.x * rs; - y = c.y * rs; - z = c.z * rs; - w = s * 0.5f; - } - } - - inline Quat() : - x(0), - y(0), - z(0), - w(1) { - } - //----- - - - - - _FORCE_INLINE_ bool operator==(const Quat &p_quat) const; - _FORCE_INLINE_ bool operator!=(const Quat &p_quat) const; - - bool Quat::operator==(const Quat &p_quat) const { - return x == p_quat.x && y == p_quat.y && z == p_quat.z && w == p_quat.w; - } - - bool Quat::operator!=(const Quat &p_quat) const { - return x != p_quat.x || y != p_quat.y || z != p_quat.z || w != p_quat.w; - } - */ +/* + operator String() const; +*/ #endif