diff options
Diffstat (limited to 'core/math/quat.cpp')
| -rw-r--r-- | core/math/quat.cpp | 93 |
1 files changed, 56 insertions, 37 deletions
diff --git a/core/math/quat.cpp b/core/math/quat.cpp index 73124e5e8e..055e2b7c35 100644 --- a/core/math/quat.cpp +++ b/core/math/quat.cpp @@ -5,7 +5,7 @@ /* GODOT ENGINE */ /* http://www.godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -27,22 +27,40 @@ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ #include "quat.h" +#include "matrix3.h" #include "print_string.h" +// set_euler expects a vector containing the Euler angles in the format +// (c,b,a), where a is the angle of the first rotation, and c is the last. +// The current implementation uses XYZ convention (Z is the first rotation). void Quat::set_euler(const Vector3& p_euler) { - real_t half_yaw = p_euler.x * 0.5; - real_t half_pitch = p_euler.y * 0.5; - real_t half_roll = p_euler.z * 0.5; - real_t cos_yaw = Math::cos(half_yaw); - real_t sin_yaw = Math::sin(half_yaw); - real_t cos_pitch = Math::cos(half_pitch); - real_t sin_pitch = Math::sin(half_pitch); - real_t cos_roll = Math::cos(half_roll); - real_t sin_roll = Math::sin(half_roll); - set(cos_roll * sin_pitch * cos_yaw+sin_roll * cos_pitch * sin_yaw, - cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw, - sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw, - cos_roll * cos_pitch * cos_yaw+sin_roll * sin_pitch * sin_yaw); + real_t half_a1 = p_euler.x * 0.5; + real_t half_a2 = p_euler.y * 0.5; + real_t half_a3 = p_euler.z * 0.5; + + // 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); + + set(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); +} + +// get_euler returns a vector containing the Euler angles in the format +// (a1,a2,a3), where a3 is the angle of the first rotation, and a1 is the last. +// The current implementation uses XYZ convention (Z is the first rotation). +Vector3 Quat::get_euler() const { + Basis m(*this); + return m.get_euler(); } void Quat::operator*=(const Quat& q) { @@ -126,26 +144,25 @@ Quat Quat::slerp(const Quat& q, const real_t& t) const { } #else - real_t to1[4]; + Quat to1; real_t omega, cosom, sinom, scale0, scale1; // calc cosine - cosom = x * q.x + y * q.y + z * q.z - + w * q.w; - + cosom = dot(q); // adjust signs (if necessary) if ( cosom <0.0 ) { - cosom = -cosom; to1[0] = - q.x; - to1[1] = - q.y; - to1[2] = - q.z; - to1[3] = - q.w; + cosom = -cosom; + to1.x = - q.x; + to1.y = - q.y; + to1.z = - q.z; + to1.w = - q.w; } else { - to1[0] = q.x; - to1[1] = q.y; - to1[2] = q.z; - to1[3] = q.w; + to1.x = q.x; + to1.y = q.y; + to1.z = q.z; + to1.w = q.w; } @@ -165,10 +182,10 @@ Quat Quat::slerp(const Quat& q, const real_t& t) const { } // calculate final values return Quat( - scale0 * x + scale1 * to1[0], - scale0 * y + scale1 * to1[1], - scale0 * z + scale1 * to1[2], - scale0 * w + scale1 * to1[3] + scale0 * x + scale1 * to1.x, + scale0 * y + scale1 * to1.y, + scale0 * z + scale1 * to1.z, + scale0 * w + scale1 * to1.w ); #endif } @@ -186,10 +203,10 @@ Quat Quat::slerpni(const Quat& q, const real_t& t) const { newFactor = Math::sin(t * theta) * sinT, invFactor = Math::sin((1.0f - t) * theta) * sinT; - return Quat( invFactor * from.x + newFactor * q.x, - invFactor * from.y + newFactor * q.y, - invFactor * from.z + newFactor * q.z, - invFactor * from.w + newFactor * q.w ); + return Quat(invFactor * from.x + newFactor * q.x, + invFactor * from.y + newFactor * q.y, + invFactor * from.z + newFactor * q.z, + invFactor * from.w + newFactor * q.w); #if 0 real_t to1[4]; @@ -203,7 +220,7 @@ Quat Quat::slerpni(const Quat& q, const real_t& t) const { // adjust signs (if necessary) if ( cosom <0.0 && false) { - cosom = -cosom; to1[0] = - q.x; + cosom = -cosom;to1[0] = - q.x; to1[1] = - q.y; to1[2] = - q.z; to1[3] = - q.w; @@ -260,8 +277,10 @@ Quat::Quat(const Vector3& axis, const real_t& angle) { if (d==0) set(0,0,0,0); else { - real_t s = Math::sin(-angle * 0.5) / d; + real_t sin_angle = Math::sin(angle * 0.5); + real_t cos_angle = Math::cos(angle * 0.5); + real_t s = sin_angle / d; set(axis.x * s, axis.y * s, axis.z * s, - Math::cos(-angle * 0.5)); + cos_angle); } } |