diff options
Diffstat (limited to 'core/math/quat.cpp')
| -rw-r--r-- | core/math/quat.cpp | 55 |
1 files changed, 46 insertions, 9 deletions
diff --git a/core/math/quat.cpp b/core/math/quat.cpp index 0bea97c2e8..5984cdf657 100644 --- a/core/math/quat.cpp +++ b/core/math/quat.cpp @@ -31,10 +31,11 @@ #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) { +// 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.5; real_t half_a2 = p_euler.y * 0.5; real_t half_a3 = p_euler.z * 0.5; @@ -56,12 +57,48 @@ void Quat::set_euler(const Vector3 &p_euler) { -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 { +// 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(); + return m.get_euler_xyz(); +} + +// 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.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); + + set(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 * sin_a2 * sin_a3, + sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3); +} + +// 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 { + Basis m(*this); + return m.get_euler_yxz(); } void Quat::operator*=(const Quat &q) { |