diff options
Diffstat (limited to 'core/math/quaternion.cpp')
| -rw-r--r-- | core/math/quaternion.cpp | 43 |
1 files changed, 31 insertions, 12 deletions
diff --git a/core/math/quaternion.cpp b/core/math/quaternion.cpp index 2ce603cb13..11bfcc1a6f 100644 --- a/core/math/quaternion.cpp +++ b/core/math/quaternion.cpp @@ -102,6 +102,22 @@ Quaternion Quaternion::inverse() const { return Quaternion(-x, -y, -z, w); } +Quaternion Quaternion::log() const { + Quaternion src = *this; + Vector3 src_v = src.get_axis() * src.get_angle(); + return Quaternion(src_v.x, src_v.y, src_v.z, 0); +} + +Quaternion Quaternion::exp() const { + Quaternion src = *this; + Vector3 src_v = Vector3(src.x, src.y, src.z); + float theta = src_v.length(); + if (theta < CMP_EPSILON) { + return Quaternion(0, 0, 0, 1); + } + return Quaternion(src_v.normalized(), theta); +} + 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."); @@ -114,7 +130,7 @@ Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) con cosom = dot(p_to); // adjust signs (if necessary) - if (cosom < 0.0) { + if (cosom < 0.0f) { cosom = -cosom; to1.x = -p_to.x; to1.y = -p_to.y; @@ -129,7 +145,7 @@ Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) con // calculate coefficients - if ((1.0 - cosom) > CMP_EPSILON) { + if ((1.0f - cosom) > (real_t)CMP_EPSILON) { // standard case (slerp) omega = Math::acos(cosom); sinom = Math::sin(omega); @@ -138,7 +154,7 @@ Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) con } else { // "from" and "to" quaternions are very close // ... so we can do a linear interpolation - scale0 = 1.0 - p_weight; + scale0 = 1.0f - p_weight; scale1 = p_weight; } // calculate final values @@ -158,14 +174,14 @@ Quaternion Quaternion::slerpni(const Quaternion &p_to, const real_t &p_weight) c real_t dot = from.dot(p_to); - if (Math::absf(dot) > 0.9999) { + if (Math::absf(dot) > 0.9999f) { return from; } real_t theta = Math::acos(dot), - sinT = 1.0 / Math::sin(theta), + sinT = 1.0f / Math::sin(theta), newFactor = Math::sin(p_weight * theta) * sinT, - invFactor = Math::sin((1.0 - p_weight) * theta) * sinT; + invFactor = Math::sin((1.0f - p_weight) * theta) * sinT; return Quaternion(invFactor * from.x + newFactor * p_to.x, invFactor * from.y + newFactor * p_to.y, @@ -179,7 +195,7 @@ Quaternion Quaternion::cubic_slerp(const Quaternion &p_b, const Quaternion &p_pr 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; + real_t t2 = (1.0f - 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); @@ -190,6 +206,9 @@ Quaternion::operator String() const { } Vector3 Quaternion::get_axis() const { + if (Math::abs(w) > 1 - CMP_EPSILON) { + return Vector3(x, y, z); + } real_t r = ((real_t)1) / Math::sqrt(1 - w * w); return Vector3(x * r, y * r, z * r); } @@ -209,8 +228,8 @@ Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) { 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 sin_angle = Math::sin(p_angle * 0.5f); + real_t cos_angle = Math::cos(p_angle * 0.5f); real_t s = sin_angle / d; x = p_axis.x * s; y = p_axis.y * s; @@ -224,9 +243,9 @@ Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) { // 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; + 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) |