From 8acd13a456050ded00f0f264ff0aa91a304f6c54 Mon Sep 17 00:00:00 2001 From: Marcel Admiraal Date: Wed, 20 Jan 2021 07:02:02 +0000 Subject: Rename Quat to Quaternion --- core/math/basis.cpp | 38 +++---- core/math/basis.h | 20 ++-- core/math/math_fieldwise.cpp | 4 +- core/math/quat.cpp | 232 ----------------------------------------- core/math/quat.h | 238 ------------------------------------------- core/math/quaternion.cpp | 232 +++++++++++++++++++++++++++++++++++++++++ core/math/quaternion.h | 238 +++++++++++++++++++++++++++++++++++++++++++ core/math/transform_3d.cpp | 6 +- 8 files changed, 504 insertions(+), 504 deletions(-) delete mode 100644 core/math/quat.cpp delete mode 100644 core/math/quat.h create mode 100644 core/math/quaternion.cpp create mode 100644 core/math/quaternion.h (limited to 'core/math') diff --git a/core/math/basis.cpp b/core/math/basis.cpp index 037378b9d7..7489da34d9 100644 --- a/core/math/basis.cpp +++ b/core/math/basis.cpp @@ -345,12 +345,12 @@ void Basis::rotate(const Vector3 &p_euler) { *this = rotated(p_euler); } -Basis Basis::rotated(const Quat &p_quat) const { - return Basis(p_quat) * (*this); +Basis Basis::rotated(const Quaternion &p_quaternion) const { + return Basis(p_quaternion) * (*this); } -void Basis::rotate(const Quat &p_quat) { - *this = rotated(p_quat); +void Basis::rotate(const Quaternion &p_quaternion) { + *this = rotated(p_quaternion); } Vector3 Basis::get_rotation_euler() const { @@ -367,7 +367,7 @@ Vector3 Basis::get_rotation_euler() const { return m.get_euler(); } -Quat Basis::get_rotation_quat() const { +Quaternion Basis::get_rotation_quaternion() const { // Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S, // and returns the Euler angles corresponding to the rotation part, complementing get_scale(). // See the comment in get_scale() for further information. @@ -378,7 +378,7 @@ Quat Basis::get_rotation_quat() const { m.scale(Vector3(-1, -1, -1)); } - return m.get_quat(); + return m.get_quaternion(); } void Basis::get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const { @@ -770,9 +770,9 @@ Basis::operator String() const { return mtx; } -Quat Basis::get_quat() const { +Quaternion Basis::get_quaternion() const { #ifdef MATH_CHECKS - ERR_FAIL_COND_V_MSG(!is_rotation(), Quat(), "Basis must be normalized in order to be casted to a Quaternion. Use get_rotation_quat() or call orthonormalized() instead."); + ERR_FAIL_COND_V_MSG(!is_rotation(), Quaternion(), "Basis must be normalized in order to be casted to a Quaternion. Use get_rotation_quaternion() or call orthonormalized() instead."); #endif /* Allow getting a quaternion from an unnormalized transform */ Basis m = *this; @@ -803,7 +803,7 @@ Quat Basis::get_quat() const { temp[k] = (m.elements[k][i] + m.elements[i][k]) * s; } - return Quat(temp[0], temp[1], temp[2], temp[3]); + return Quaternion(temp[0], temp[1], temp[2], temp[3]); } static const Basis _ortho_bases[24] = { @@ -945,13 +945,13 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { r_angle = angle; } -void Basis::set_quat(const Quat &p_quat) { - real_t d = p_quat.length_squared(); +void Basis::set_quaternion(const Quaternion &p_quaternion) { + real_t d = p_quaternion.length_squared(); real_t s = 2.0 / d; - real_t xs = p_quat.x * s, ys = p_quat.y * s, zs = p_quat.z * s; - real_t wx = p_quat.w * xs, wy = p_quat.w * ys, wz = p_quat.w * zs; - real_t xx = p_quat.x * xs, xy = p_quat.x * ys, xz = p_quat.x * zs; - real_t yy = p_quat.y * ys, yz = p_quat.y * zs, zz = p_quat.z * zs; + real_t xs = p_quaternion.x * s, ys = p_quaternion.y * s, zs = p_quaternion.z * s; + real_t wx = p_quaternion.w * xs, wy = p_quaternion.w * ys, wz = p_quaternion.w * zs; + real_t xx = p_quaternion.x * xs, xy = p_quaternion.x * ys, xz = p_quaternion.x * zs; + real_t yy = p_quaternion.y * ys, yz = p_quaternion.y * zs, zz = p_quaternion.z * zs; set(1.0 - (yy + zz), xy - wz, xz + wy, xy + wz, 1.0 - (xx + zz), yz - wx, xz - wy, yz + wx, 1.0 - (xx + yy)); @@ -997,9 +997,9 @@ void Basis::set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale) { rotate(p_euler); } -void Basis::set_quat_scale(const Quat &p_quat, const Vector3 &p_scale) { +void Basis::set_quaternion_scale(const Quaternion &p_quaternion, const Vector3 &p_scale) { set_diagonal(p_scale); - rotate(p_quat); + rotate(p_quaternion); } void Basis::set_diagonal(const Vector3 &p_diag) { @@ -1018,8 +1018,8 @@ void Basis::set_diagonal(const Vector3 &p_diag) { Basis Basis::slerp(const Basis &p_to, const real_t &p_weight) const { //consider scale - Quat from(*this); - Quat to(p_to); + Quaternion from(*this); + Quaternion to(p_to); Basis b(from.slerp(to, p_weight)); b.elements[0] *= Math::lerp(elements[0].length(), p_to.elements[0].length(), p_weight); diff --git a/core/math/basis.h b/core/math/basis.h index 56f6227313..3736047dd3 100644 --- a/core/math/basis.h +++ b/core/math/basis.h @@ -31,7 +31,7 @@ #ifndef BASIS_H #define BASIS_H -#include "core/math/quat.h" +#include "core/math/quaternion.h" #include "core/math/vector3.h" class Basis { @@ -79,13 +79,13 @@ public: void rotate(const Vector3 &p_euler); Basis rotated(const Vector3 &p_euler) const; - void rotate(const Quat &p_quat); - Basis rotated(const Quat &p_quat) const; + void rotate(const Quaternion &p_quaternion); + Basis rotated(const Quaternion &p_quaternion) const; Vector3 get_rotation_euler() const; void get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const; void get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) const; - Quat get_rotation_quat() const; + Quaternion get_rotation_quaternion() const; Vector3 get_rotation() const { return get_rotation_euler(); }; Vector3 rotref_posscale_decomposition(Basis &rotref) const; @@ -108,8 +108,8 @@ public: Vector3 get_euler_zyx() const; void set_euler_zyx(const Vector3 &p_euler); - Quat get_quat() const; - void set_quat(const Quat &p_quat); + Quaternion get_quaternion() const; + void set_quaternion(const Quaternion &p_quaternion); Vector3 get_euler() const { return get_euler_yxz(); } void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); } @@ -132,7 +132,7 @@ public: void set_axis_angle_scale(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale); void set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale); - void set_quat_scale(const Quat &p_quat, const Vector3 &p_scale); + void set_quaternion_scale(const Quaternion &p_quaternion, const Vector3 &p_scale); // transposed dot products _FORCE_INLINE_ real_t tdotx(const Vector3 &v) const { @@ -240,10 +240,10 @@ public: #endif Basis diagonalize(); - operator Quat() const { return get_quat(); } + operator Quaternion() const { return get_quaternion(); } - Basis(const Quat &p_quat) { set_quat(p_quat); }; - Basis(const Quat &p_quat, const Vector3 &p_scale) { set_quat_scale(p_quat, p_scale); } + Basis(const Quaternion &p_quaternion) { set_quaternion(p_quaternion); }; + Basis(const Quaternion &p_quaternion, const Vector3 &p_scale) { set_quaternion_scale(p_quaternion, p_scale); } Basis(const Vector3 &p_euler) { set_euler(p_euler); } Basis(const Vector3 &p_euler, const Vector3 &p_scale) { set_euler_scale(p_euler, p_scale); } diff --git a/core/math/math_fieldwise.cpp b/core/math/math_fieldwise.cpp index f2baef1a59..570c57e254 100644 --- a/core/math/math_fieldwise.cpp +++ b/core/math/math_fieldwise.cpp @@ -88,8 +88,8 @@ Variant fieldwise_assign(const Variant &p_target, const Variant &p_source, const return target; } - case Variant::QUAT: { - SETUP_TYPE(Quat) + case Variant::QUATERNION: { + SETUP_TYPE(Quaternion) /**/ TRY_TRANSFER_FIELD("x", x) else TRY_TRANSFER_FIELD("y", y) diff --git a/core/math/quat.cpp b/core/math/quat.cpp deleted file mode 100644 index 3982a0b993..0000000000 --- a/core/math/quat.cpp +++ /dev/null @@ -1,232 +0,0 @@ -/*************************************************************************/ -/* quat.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#include "quat.h" - -#include "core/math/basis.h" -#include "core/string/print_string.h" - -// 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 Quat::get_euler_xyz() const { - Basis m(*this); - return m.get_euler_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 Quat::get_euler_yxz() const { -#ifdef MATH_CHECKS - 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(); -} - -void Quat::operator*=(const Quat &p_q) { - real_t xx = w * p_q.x + x * p_q.w + y * p_q.z - z * p_q.y; - real_t yy = w * p_q.y + y * p_q.w + z * p_q.x - x * p_q.z; - real_t zz = w * p_q.z + z * p_q.w + x * p_q.y - y * p_q.x; - w = w * p_q.w - x * p_q.x - y * p_q.y - z * p_q.z; - x = xx; - y = yy; - z = zz; -} - -Quat Quat::operator*(const Quat &p_q) const { - Quat r = *this; - r *= p_q; - return r; -} - -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); -} - -real_t Quat::length() const { - return Math::sqrt(length_squared()); -} - -void Quat::normalize() { - *this /= length(); -} - -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 { -#ifdef MATH_CHECKS - ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The quaternion must be normalized."); -#endif - return Quat(-x, -y, -z, w); -} - -Quat Quat::slerp(const Quat &p_to, const real_t &p_weight) const { -#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."); -#endif - Quat to1; - real_t omega, cosom, sinom, scale0, scale1; - - // calc cosine - cosom = dot(p_to); - - // adjust signs (if necessary) - if (cosom < 0.0) { - cosom = -cosom; - 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; - } - - // calculate coefficients - - if ((1.0 - cosom) > CMP_EPSILON) { - // standard case (slerp) - omega = Math::acos(cosom); - sinom = Math::sin(omega); - scale0 = Math::sin((1.0 - p_weight) * omega) / sinom; - scale1 = Math::sin(p_weight * omega) / sinom; - } else { - // "from" and "to" quaternions are very close - // ... so we can do a linear interpolation - scale0 = 1.0 - p_weight; - 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); -} - -Quat Quat::slerpni(const Quat &p_to, const real_t &p_weight) const { -#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."); -#endif - const Quat &from = *this; - - real_t dot = from.dot(p_to); - - if (Math::absf(dot) > 0.9999) { - return from; - } - - real_t theta = Math::acos(dot), - sinT = 1.0 / Math::sin(theta), - newFactor = Math::sin(p_weight * theta) * sinT, - invFactor = Math::sin((1.0 - 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); -} - -Quat Quat::cubic_slerp(const Quat &p_b, const Quat &p_pre_a, const Quat &p_post_b, const real_t &p_weight) const { -#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."); -#endif - //the only way to do slerp :| - real_t t2 = (1.0 - 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::operator String() const { - return String::num(x) + ", " + String::num(y) + ", " + String::num(z) + ", " + String::num(w); -} - -Quat::Quat(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."); -#endif - real_t d = p_axis.length(); - if (d == 0) { - x = 0; - y = 0; - z = 0; - w = 0; - } else { - real_t sin_angle = Math::sin(p_angle * 0.5); - real_t cos_angle = Math::cos(p_angle * 0.5); - real_t s = sin_angle / d; - x = p_axis.x * s; - y = p_axis.y * s; - z = p_axis.z * s; - w = cos_angle; - } -} - -// Euler constructor 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). -Quat::Quat(const Vector3 &p_euler) { - real_t half_a1 = p_euler.y * 0.5; - real_t half_a2 = p_euler.x * 0.5; - real_t half_a3 = p_euler.z * 0.5; - - // 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); - - 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; -} diff --git a/core/math/quat.h b/core/math/quat.h deleted file mode 100644 index d9b130c050..0000000000 --- a/core/math/quat.h +++ /dev/null @@ -1,238 +0,0 @@ -/*************************************************************************/ -/* quat.h */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ - -#ifndef QUAT_H -#define QUAT_H - -#include "core/math/math_defs.h" -#include "core/math/math_funcs.h" -#include "core/math/vector3.h" -#include "core/string/ustring.h" - -class Quat { -public: - union { - struct { - real_t x; - real_t y; - real_t z; - real_t w; - }; - real_t components[4] = { 0, 0, 0, 1.0 }; - }; - - _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 Quat &p_quat) const; - real_t length() const; - void normalize(); - Quat normalized() const; - bool is_normalized() const; - Quat inverse() const; - _FORCE_INLINE_ real_t dot(const Quat &p_q) const; - - Vector3 get_euler_xyz() const; - Vector3 get_euler_yxz() const; - Vector3 get_euler() const { return get_euler_yxz(); }; - - 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; - - _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; - } - - void operator*=(const Quat &p_q); - Quat operator*(const Quat &p_q) const; - - Quat operator*(const Vector3 &v) const { - return Quat(w * v.x + y * v.z - z * v.y, - w * v.y + z * v.x - x * v.z, - w * v.z + x * v.y - y * v.x, - -x * v.x - y * v.y - z * v.z); - } - - _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); - } - - _FORCE_INLINE_ Vector3 xform_inv(const Vector3 &v) const { - return inverse().xform(v); - } - - _FORCE_INLINE_ void operator+=(const Quat &p_q); - _FORCE_INLINE_ void operator-=(const Quat &p_q); - _FORCE_INLINE_ void operator*=(const real_t &s); - _FORCE_INLINE_ void operator/=(const real_t &s); - _FORCE_INLINE_ Quat operator+(const Quat &q2) const; - _FORCE_INLINE_ Quat operator-(const Quat &q2) const; - _FORCE_INLINE_ Quat operator-() const; - _FORCE_INLINE_ Quat operator*(const real_t &s) const; - _FORCE_INLINE_ Quat operator/(const real_t &s) const; - - _FORCE_INLINE_ bool operator==(const Quat &p_quat) const; - _FORCE_INLINE_ bool operator!=(const Quat &p_quat) const; - - operator String() const; - - _FORCE_INLINE_ Quat() {} - - _FORCE_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 &p_axis, real_t p_angle); - - Quat(const Vector3 &p_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.0 + CMP_EPSILON) { - x = 0; - y = 1; - z = 0; - w = 0; - } else { - real_t s = Math::sqrt((1.0 + d) * 2.0); - real_t rs = 1.0 / s; - - x = c.x * rs; - y = c.y * rs; - z = c.z * rs; - w = s * 0.5; - } - } -}; - -real_t Quat::dot(const Quat &p_q) const { - return x * p_q.x + y * p_q.y + z * p_q.z + w * p_q.w; -} - -real_t Quat::length_squared() const { - return dot(*this); -} - -void Quat::operator+=(const Quat &p_q) { - x += p_q.x; - y += p_q.y; - z += p_q.z; - w += p_q.w; -} - -void Quat::operator-=(const Quat &p_q) { - x -= p_q.x; - y -= p_q.y; - z -= p_q.z; - w -= p_q.w; -} - -void Quat::operator*=(const real_t &s) { - x *= s; - y *= s; - z *= s; - w *= s; -} - -void Quat::operator/=(const real_t &s) { - *this *= 1.0 / s; -} - -Quat Quat::operator+(const Quat &q2) const { - const Quat &q1 = *this; - return Quat(q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w); -} - -Quat Quat::operator-(const Quat &q2) const { - const Quat &q1 = *this; - return Quat(q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w); -} - -Quat Quat::operator-() const { - const Quat &q2 = *this; - return Quat(-q2.x, -q2.y, -q2.z, -q2.w); -} - -Quat Quat::operator*(const real_t &s) const { - return Quat(x * s, y * s, z * s, w * s); -} - -Quat Quat::operator/(const real_t &s) const { - return *this * (1.0 / s); -} - -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; -} - -_FORCE_INLINE_ Quat operator*(const real_t &p_real, const Quat &p_quat) { - return p_quat * p_real; -} - -#endif // QUAT_H diff --git a/core/math/quaternion.cpp b/core/math/quaternion.cpp new file mode 100644 index 0000000000..8de3d0cc2a --- /dev/null +++ b/core/math/quaternion.cpp @@ -0,0 +1,232 @@ +/*************************************************************************/ +/* quaternion.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "quaternion.h" + +#include "core/math/basis.h" +#include "core/string/print_string.h" + +// 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_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 { +#ifdef MATH_CHECKS + 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(); +} + +void Quaternion::operator*=(const Quaternion &p_q) { + real_t xx = w * p_q.x + x * p_q.w + y * p_q.z - z * p_q.y; + real_t yy = w * p_q.y + y * p_q.w + z * p_q.x - x * p_q.z; + real_t zz = w * p_q.z + z * p_q.w + x * p_q.y - y * p_q.x; + w = w * p_q.w - x * p_q.x - y * p_q.y - z * p_q.z; + x = xx; + y = yy; + z = zz; +} + +Quaternion Quaternion::operator*(const Quaternion &p_q) const { + Quaternion r = *this; + r *= p_q; + return r; +} + +bool Quaternion::is_equal_approx(const Quaternion &p_quaternion) const { + return Math::is_equal_approx(x, p_quaternion.x) && Math::is_equal_approx(y, p_quaternion.y) && Math::is_equal_approx(z, p_quaternion.z) && Math::is_equal_approx(w, p_quaternion.w); +} + +real_t Quaternion::length() const { + return Math::sqrt(length_squared()); +} + +void Quaternion::normalize() { + *this /= length(); +} + +Quaternion Quaternion::normalized() const { + return *this / length(); +} + +bool Quaternion::is_normalized() const { + return Math::is_equal_approx(length_squared(), 1, (real_t)UNIT_EPSILON); //use less epsilon +} + +Quaternion Quaternion::inverse() const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The quaternion must be normalized."); +#endif + return Quaternion(-x, -y, -z, w); +} + +Quaternion Quaternion::slerp(const Quaternion &p_to, 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_to.is_normalized(), Quaternion(), "The end quaternion must be normalized."); +#endif + Quaternion to1; + real_t omega, cosom, sinom, scale0, scale1; + + // calc cosine + cosom = dot(p_to); + + // adjust signs (if necessary) + if (cosom < 0.0) { + cosom = -cosom; + 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; + } + + // calculate coefficients + + if ((1.0 - cosom) > CMP_EPSILON) { + // standard case (slerp) + omega = Math::acos(cosom); + sinom = Math::sin(omega); + scale0 = Math::sin((1.0 - p_weight) * omega) / sinom; + scale1 = Math::sin(p_weight * omega) / sinom; + } else { + // "from" and "to" quaternions are very close + // ... so we can do a linear interpolation + scale0 = 1.0 - p_weight; + scale1 = p_weight; + } + // calculate final values + return Quaternion( + scale0 * x + scale1 * to1.x, + scale0 * y + scale1 * to1.y, + scale0 * z + scale1 * to1.z, + scale0 * w + scale1 * to1.w); +} + +Quaternion Quaternion::slerpni(const Quaternion &p_to, 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_to.is_normalized(), Quaternion(), "The end quaternion must be normalized."); +#endif + const Quaternion &from = *this; + + real_t dot = from.dot(p_to); + + if (Math::absf(dot) > 0.9999) { + return from; + } + + real_t theta = Math::acos(dot), + sinT = 1.0 / Math::sin(theta), + newFactor = Math::sin(p_weight * theta) * sinT, + invFactor = Math::sin((1.0 - p_weight) * theta) * sinT; + + return Quaternion(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); +} + +Quaternion Quaternion::cubic_slerp(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 + //the only way to do slerp :| + real_t t2 = (1.0 - p_weight) * p_weight * 2; + Quaternion sp = this->slerp(p_b, p_weight); + Quaternion sq = p_pre_a.slerpni(p_post_b, p_weight); + return sp.slerpni(sq, t2); +} + +Quaternion::operator String() const { + return String::num(x) + ", " + String::num(y) + ", " + String::num(z) + ", " + String::num(w); +} + +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."); +#endif + real_t d = p_axis.length(); + if (d == 0) { + x = 0; + y = 0; + z = 0; + w = 0; + } else { + real_t sin_angle = Math::sin(p_angle * 0.5); + real_t cos_angle = Math::cos(p_angle * 0.5); + real_t s = sin_angle / d; + x = p_axis.x * s; + y = p_axis.y * s; + z = p_axis.z * s; + w = cos_angle; + } +} + +// Euler constructor 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). +Quaternion::Quaternion(const Vector3 &p_euler) { + real_t half_a1 = p_euler.y * 0.5; + real_t half_a2 = p_euler.x * 0.5; + real_t half_a3 = p_euler.z * 0.5; + + // 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); + + 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; +} diff --git a/core/math/quaternion.h b/core/math/quaternion.h new file mode 100644 index 0000000000..796214b79e --- /dev/null +++ b/core/math/quaternion.h @@ -0,0 +1,238 @@ +/*************************************************************************/ +/* quaternion.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef QUAT_H +#define QUAT_H + +#include "core/math/math_defs.h" +#include "core/math/math_funcs.h" +#include "core/math/vector3.h" +#include "core/string/ustring.h" + +class Quaternion { +public: + union { + struct { + real_t x; + real_t y; + real_t z; + real_t w; + }; + real_t components[4] = { 0, 0, 0, 1.0 }; + }; + + _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_quaternion) const; + real_t length() const; + void normalize(); + Quaternion normalized() const; + bool is_normalized() const; + Quaternion inverse() const; + _FORCE_INLINE_ real_t dot(const Quaternion &p_q) const; + + Vector3 get_euler_xyz() const; + Vector3 get_euler_yxz() const; + Vector3 get_euler() const { return get_euler_yxz(); }; + + 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; + + _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; + } + + void operator*=(const Quaternion &p_q); + Quaternion operator*(const Quaternion &p_q) const; + + Quaternion operator*(const Vector3 &v) const { + return Quaternion(w * v.x + y * v.z - z * v.y, + w * v.y + z * v.x - x * v.z, + w * v.z + x * v.y - y * v.x, + -x * v.x - y * v.y - z * v.z); + } + + _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); + } + + _FORCE_INLINE_ Vector3 xform_inv(const Vector3 &v) const { + return inverse().xform(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_ Quaternion operator-() const; + _FORCE_INLINE_ Quaternion operator*(const real_t &s) const; + _FORCE_INLINE_ Quaternion operator/(const real_t &s) const; + + _FORCE_INLINE_ bool operator==(const Quaternion &p_quaternion) const; + _FORCE_INLINE_ bool operator!=(const Quaternion &p_quaternion) const; + + operator String() const; + + _FORCE_INLINE_ Quaternion() {} + + _FORCE_INLINE_ Quaternion(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) { + } + + 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), + z(p_q.z), + w(p_q.w) { + } + + Quaternion &operator=(const Quaternion &p_q) { + x = p_q.x; + y = p_q.y; + z = p_q.z; + w = p_q.w; + return *this; + } + + Quaternion(const Vector3 &v0, const Vector3 &v1) // shortest arc + { + Vector3 c = v0.cross(v1); + real_t d = v0.dot(v1); + + if (d < -1.0 + CMP_EPSILON) { + x = 0; + y = 1; + z = 0; + w = 0; + } else { + real_t s = Math::sqrt((1.0 + d) * 2.0); + real_t rs = 1.0 / s; + + x = c.x * rs; + y = c.y * rs; + z = c.z * rs; + w = s * 0.5; + } + } +}; + +real_t Quaternion::dot(const Quaternion &p_q) const { + return x * p_q.x + y * p_q.y + z * p_q.z + w * p_q.w; +} + +real_t Quaternion::length_squared() const { + return dot(*this); +} + +void Quaternion::operator+=(const Quaternion &p_q) { + x += p_q.x; + y += p_q.y; + z += p_q.z; + w += p_q.w; +} + +void Quaternion::operator-=(const Quaternion &p_q) { + x -= p_q.x; + y -= p_q.y; + z -= p_q.z; + w -= p_q.w; +} + +void Quaternion::operator*=(const real_t &s) { + x *= s; + y *= s; + z *= s; + w *= s; +} + +void Quaternion::operator/=(const real_t &s) { + *this *= 1.0 / s; +} + +Quaternion Quaternion::operator+(const Quaternion &q2) const { + const Quaternion &q1 = *this; + return Quaternion(q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w); +} + +Quaternion Quaternion::operator-(const Quaternion &q2) const { + const Quaternion &q1 = *this; + return Quaternion(q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - 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/(const real_t &s) const { + return *this * (1.0 / s); +} + +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; +} + +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) { + return p_quaternion * p_real; +} + +#endif // QUAT_H diff --git a/core/math/transform_3d.cpp b/core/math/transform_3d.cpp index 2611d6accf..210f0b81bb 100644 --- a/core/math/transform_3d.cpp +++ b/core/math/transform_3d.cpp @@ -112,15 +112,15 @@ Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t /* not sure if very "efficient" but good enough? */ Vector3 src_scale = basis.get_scale(); - Quat src_rot = basis.get_rotation_quat(); + Quaternion src_rot = basis.get_rotation_quaternion(); Vector3 src_loc = origin; Vector3 dst_scale = p_transform.basis.get_scale(); - Quat dst_rot = p_transform.basis.get_rotation_quat(); + Quaternion dst_rot = p_transform.basis.get_rotation_quaternion(); Vector3 dst_loc = p_transform.origin; Transform3D interp; - interp.basis.set_quat_scale(src_rot.slerp(dst_rot, p_c).normalized(), src_scale.lerp(dst_scale, p_c)); + interp.basis.set_quaternion_scale(src_rot.slerp(dst_rot, p_c).normalized(), src_scale.lerp(dst_scale, p_c)); interp.origin = src_loc.lerp(dst_loc, p_c); return interp; -- cgit v1.2.3