Commit 10a26dbd authored by Sebastian Riebeling's avatar Sebastian Riebeling

added Quaternion to YPR function

parent 4effa1fe
...@@ -26,6 +26,11 @@ ...@@ -26,6 +26,11 @@
#include <limits> #include <limits>
#include <vector> #include <vector>
#include <VistaBase/VistaQuaternion.h>
#include <VistaBase/VistaTransformMatrix.h>
#include <VistaBase/VistaVector3D.h>
// Ein Makro für die Berechnung von Prozentsätzen (Wieviel Prozent p sind P von G?) // 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. // 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) #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 ...@@ -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ß) //! 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 ); 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 //! Datenklasse für Fehlerwerte
template< typename T > template< typename T >
class ITA_BASE_API ErrorValues class ITA_BASE_API ErrorValues
......
...@@ -6,6 +6,10 @@ ...@@ -6,6 +6,10 @@
#include <cassert> #include <cassert>
#include <numeric> #include <numeric>
#include <VistaBase/VistaQuaternion.h>
#include <VistaBase/VistaTransformMatrix.h>
#include <VistaBase/VistaVector3D.h>
using namespace ITAConstants; using namespace ITAConstants;
int getExp2( const unsigned int x ) int getExp2( const unsigned int x )
...@@ -426,6 +430,32 @@ void convertVU2YPR( const double vx, const double vy, const double vz, const dou ...@@ -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<int>& dest, const int a, const int b ) void pow2space( std::vector<int>& dest, const int a, const int b )
{ {
int xa = getExp2( a ); int xa = getExp2( a );
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment