diff --git a/AirLib/include/common/VectorMath.hpp b/AirLib/include/common/VectorMath.hpp index 391a668b0d..7eaeb1a9e6 100644 --- a/AirLib/include/common/VectorMath.hpp +++ b/AirLib/include/common/VectorMath.hpp @@ -207,19 +207,20 @@ class VectorMathT { static void toEulerianAngle(const QuaternionT& q , RealT& pitch, RealT& roll, RealT& yaw) { + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles RealT ysqr = q.y() * q.y(); RealT t0 = -2.0f * (ysqr + q.z() * q.z()) + 1.0f; RealT t1 = +2.0f * (q.x() * q.y() + q.w() * q.z()); - RealT t2 = -2.0f * (q.x() * q.z() - q.w() * q.y()); - RealT t3 = +2.0f * (q.y() * q.z() + q.w() * q.x()); - RealT t4 = -2.0f * (q.x() * q.x() + ysqr) + 1.0f; + roll = std::atan2(t1, t0); + RealT t2 = -2.0f * (q.x() * q.z() - q.w() * q.y()); t2 = t2 > 1.0f ? 1.0f : t2; t2 = t2 < -1.0f ? -1.0f : t2; - pitch = std::asin(t2); - roll = std::atan2(t3, t4); - yaw = std::atan2(t1, t0); + + RealT t3 = +2.0f * (q.y() * q.z() + q.w() * q.x()); + RealT t4 = -2.0f * (q.x() * q.x() + ysqr) + 1.0f; + yaw = std::atan2(t3, t4); } static Vector3T toAngularVelocity(const QuaternionT& start, const QuaternionT& end, RealT delta_sec)