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

added Quaternion to YPR function

parent 4effa1fe
......@@ -26,6 +26,11 @@
#include <limits>
#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?)
// 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
......
......@@ -6,6 +6,10 @@
#include <cassert>
#include <numeric>
#include <VistaBase/VistaQuaternion.h>
#include <VistaBase/VistaTransformMatrix.h>
#include <VistaBase/VistaVector3D.h>
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<int>& dest, const int a, const int b )
{
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