From 10a26dbd4394242f236406f47603c80183f55170 Mon Sep 17 00:00:00 2001 From: Sebastian Riebeling Date: Thu, 22 Aug 2019 16:16:58 +0200 Subject: [PATCH] added Quaternion to YPR function --- include/ITANumericUtils.h | 10 ++++++++++ src/ITANumericUtils.cpp | 30 ++++++++++++++++++++++++++++++ 2 files changed, 40 insertions(+) diff --git a/include/ITANumericUtils.h b/include/ITANumericUtils.h index d40b7fe..088a985 100644 --- a/include/ITANumericUtils.h +++ b/include/ITANumericUtils.h @@ -26,6 +26,11 @@ #include #include +#include +#include +#include + + // Ein Makro für die Berechnung von Prozentsätzen (Wieviel Prozent p sind P von G?) // Wichtiges Feature: Behandlung des Falles G = 0. Rückgabewert in diesem Fall 0. #define PERCENT_OF(P, G) (G == 0 ? 0 : 100*(double) P/(double) G) @@ -342,6 +347,11 @@ ITA_BASE_API void convertVU2YPR( const double vx, const double vy, const double //! Orientierung in View-Up-Vektor in Yaw-Pitch-Roll Winkeln umrechnen (Alle Winkel in Bogenmaß) ITA_BASE_API void convertVU2YPR( const float vx, const float vy, const float vz, const float ux, const float uy, const float uz, float& yaw, float& pitch, float& roll ); + +//! Orientation in Quaternion: calculate Yaw-Pitch-Roll angles (all angles in radian) +ITA_BASE_API void calculateYPRAnglesFromQuaternion(VistaQuaternion q, double& dRoll, double& dPitch, double& dYaw); + + //! Datenklasse für Fehlerwerte template< typename T > class ITA_BASE_API ErrorValues diff --git a/src/ITANumericUtils.cpp b/src/ITANumericUtils.cpp index b32fdf4..a58bc12 100644 --- a/src/ITANumericUtils.cpp +++ b/src/ITANumericUtils.cpp @@ -6,6 +6,10 @@ #include #include +#include +#include +#include + using namespace ITAConstants; int getExp2( const unsigned int x ) @@ -426,6 +430,32 @@ void convertVU2YPR( const double vx, const double vy, const double vz, const dou */ } + + +void calculateYPRAnglesFromQuaternion(VistaQuaternion q, double& dRoll, double& dPitch, double& dYaw) { + //Quaternion: [qx, qy, qz, qw] as described/defined in VistaQuaternion + // [0, 1, 2, 3] + //Taken from http://www.tu-berlin.de/fileadmin/fg169/miscellaneous/Quaternions.pdf + //First convert Quaternion to a DCM, then transform DCM to euler + double a21 = 2 * (q[0] * q[1] + q[2] * q[3]);; + double a22 = pow(q[3], 2) - pow(q[0], 2) + pow(q[1], 2) - pow(q[2], 2); + + double a13 = 2 * (q[0] * q[2] + q[1] * q[3]); + double a33 = pow(q[3], 2) - pow(q[0], 2) - pow(q[1], 2) + pow(q[2], 2); + + double a23 = 2 * (q[1] * q[2] - q[0] * q[3]); + + //Calculation is based on Roll around z; Yaw around negative x; Pitch around y + dRoll = atan2(-a21, a22); + dYaw = -atan2(-a13, a33); + dPitch = asin(-a23); +} + + + + + + void pow2space( std::vector& dest, const int a, const int b ) { int xa = getExp2( a ); -- GitLab