Skip to content

GitLab

ITABase
ITABase
Commit 6a571ffb authored by Dipl.-Ing. Jonas Stienen's avatar Dipl.-Ing. Jonas Stienen
Browse files
Options

Hotfixes in numeric utils, also some refactoring

parent 2145e66c
Hide whitespace changes
Inline Side-by-side
Showing with 188 additions and 144 deletions
include/ITANumericUtils.h
View file @ 6a571ffb
......@@ -47,8 +47,15 @@ ITA_BASE_API unsigned int nextPow2( unsigned int x );
// Rundung auf Compilern definieren die dies nicht standardmäßig mitliefern
#ifndef HAVE_ROUND
ITA_BASE_API inline double round( double x ) { return ( x < 0 ? ceil( ( x ) -0.5 ) : floor( ( x ) +0.5 ) ); }
ITA_BASE_API inline float roundf( float x ) { return ( x < 0 ? ceil( ( x ) -0.5f ) : floor( ( x ) +0.5f ) ); }
ITA_BASE_API inline double round( double x )
{
return ( x < 0 ? ceil( ( x ) -0.5 ) : floor( ( x ) +0.5 ) );
};
ITA_BASE_API inline float roundf( float x )
{
return ( x < 0 ? ceil( ( x ) -0.5f ) : floor( ( x ) +0.5f ) );
};
#endif
//! Nächstkleineres Vielfaches von mul zu einer Zahl x zurückgeben
......@@ -60,7 +67,7 @@ ITA_BASE_API inline float roundf( float x ) { return ( x < 0 ? ceil( ( x ) -0.5f
* lwrmul(-10, 6) = -12
* lwrmul(10, -6) = 12
*/
ITA_BASE_API int lwrmul( int x, int mul );
ITA_BASE_API int lwrmul( const int x, const int mul );
//! Nächstgrößeres Vielfaches von mul zu einer Zahl x zurückgeben
/**
......@@ -71,21 +78,21 @@ ITA_BASE_API int lwrmul( int x, int mul );
* uprmul(-10, 6) = -6
* uprmul(10, -6) = 12
*/
ITA_BASE_API int uprmul( int x, int mul );
ITA_BASE_API int uprmul( const int x, const int mul );
//! Nächstkleineres Vielfaches von mul zu einer Zahl x zurückgeben
/**
* Beispiele: lwrmulu(10, 5) = 10
* lwrmulu(10, 6) = 6
*/
ITA_BASE_API unsigned int lwrmulu( unsigned int x, unsigned int mul );
ITA_BASE_API unsigned int lwrmulu( const unsigned int x, const unsigned int mul );
//! Nächstgrößeres Vielfaches von mul zu einer Zahl x zurückgeben
/**
* Beispiele: uprmulu(10, 5) = 10
* uprmulu(10, 6) = 12
*/
ITA_BASE_API unsigned int uprmulu( unsigned int x, unsigned int mul );
ITA_BASE_API unsigned int uprmulu( const unsigned int x, const unsigned int mul );
//! Aufrundende Ganzzahl-Division
/**
......@@ -96,13 +103,13 @@ ITA_BASE_API unsigned int uprmulu( unsigned int x, unsigned int mul );
* Anwendungen: Wieviele Blöcke der Länge b sind erforderlich
* um ein Signal der Länge l zu beschreiben: uprdiv(l, b)
*/
ITA_BASE_API int uprdiv( int a, int b );
ITA_BASE_API int uprdiv( const int a, const int b );
//! Aufrundende Ganzzahl-Division
/**
* Variante von uprdiv für vorzeichenlose Ganzzahlen.
*/
ITA_BASE_API unsigned int uprdivu( unsigned int a, unsigned int b );
ITA_BASE_API unsigned int uprdivu( const unsigned int a, const unsigned int b );
//! Einen (beliebigen) Winkel ins Interval (-180,180] abbilden
/**
......@@ -110,13 +117,13 @@ ITA_BASE_API unsigned int uprdivu( unsigned int a, unsigned int b );
* \note Der exakte Wert -180 wird durch die Funktion erhalten
* und nicht auf +180 abgebildet (Ästethik :-))
*/
ITA_BASE_API float correctAngle180( float phi );
ITA_BASE_API float correctAngle180( const float fPhiDeg );
//! Einen (beliebigen) Winkel ins Interval [0,360) abbilden
/**
* Beispiel: correctAngle(380°) = 20°
*/
ITA_BASE_API float correctAngle360( float phi );
ITA_BASE_API float correctAngle360( const float fPhiDeg );
//! Verhältnis in Dezibel (Energie) (im 10er-Logarithmus) umrechnen
/**
......@@ -172,34 +179,32 @@ ITA_BASE_API double ratio_to_db20( const double r );
+------------------------------------+ */
//! Betrag einer komplexen Zahl berechnen
ITA_BASE_API float cabsf( float re, float im );
/**
* @param[in] fReal Real part
* @param[in] fImag Imaginary part
*/
ITA_BASE_API float cabsf( const float fReal, const float fImag );
//! Phase einer komplexen Zahl berechnen
ITA_BASE_API float canglef( float re, float im );
ITA_BASE_API float canglef( const float fReal, const float fImag );
//! Betrag einer komplexen Zahl setzen, deren Phase aber erhalten (auch in-place)
ITA_BASE_API void csabsparg( const float& in_re, const float& in_im, const float& dest_abs, float& out_re, float& out_im );
ITA_BASE_API void csabsparg( const float fInReal, const float fInImag, const float fTargetAbs, float& fOutReal, float& fOutImag );
//! Winkel einer komplexen Zahl setzen, deren Betrag aber erhalten (auch in-place)
ITA_BASE_API void csargpabs( const float& in_re, const float& in_im, const float& dest_arg, float& out_re, float& out_im );
/* +---------------------------+
| |
| Neue Winkelfunktionen |
| |
+---------------------------+ */
ITA_BASE_API void csargpabs( const float fInReal, const float fInImag, const float fTargetAbs, float& fOutReal, float& fOutImag );
//! Winkel im Bogenmaß in Grad [°] umrechnen
ITA_BASE_API float rad2gradf( float phi );
ITA_BASE_API float rad2gradf( const float fPhiRad );
//! Winkel im Bogenmaß in Grad [°] umrechnen
ITA_BASE_API double rad2grad( double phi );
ITA_BASE_API double rad2grad( const double dPhiRad );
//! Winkel in Grad [°] ins Bogenmaß umrechnen
ITA_BASE_API float grad2radf( float phi );
ITA_BASE_API float grad2radf( const float fPhiDeg );
//! Winkel im Bogenmaß in Grad [°] umrechnen
ITA_BASE_API double grad2rad( double phi );
ITA_BASE_API double grad2rad( const double dPhiDeg );
/* ---------- RAD ----------- */
......@@ -212,7 +217,7 @@ ITA_BASE_API double grad2rad( double phi );
*
* Beispiele: 0->0, 2*PI->0, (-2*PI)->0, (2*PI+0.1)->0.1, (-0.1)->(2*PI-0.1)
*/
ITA_BASE_API float anglef_proj_0_2PI( float alpha );
ITA_BASE_API float anglef_proj_0_2PI( const float fAlphaRad );
//! Einen Winkel (rad) in das Intervall (-PI, PI] projezieren
/**
......@@ -223,7 +228,7 @@ ITA_BASE_API float anglef_proj_0_2PI( float alpha );
*
* Beispiele: 0->0, 2*PI->0, (-2*PI)->0, (PI+0.1)->(-PI+0.1), (-PI-0.1)->(PI-0.1)
*/
ITA_BASE_API float anglef_proj_NPI_PI( float alpha );
ITA_BASE_API float anglef_proj_NPI_PI( const float fAlphaRad );
//! (Gerichtete) minimale Winkeldifferenz für zwei Winkel [rad] (nicht Grad) im Intervall [0,2PI)
/**
......@@ -244,7 +249,7 @@ ITA_BASE_API float anglef_proj_NPI_PI( float alpha );
*
* Beispiele: (0,PI/2) -> PI/2, (PI/5,PI/4) -> PI/20, (PI/3,PI/4) -> -PI/12, (0,2*PI-0.1) -> -0.1
*/
ITA_BASE_API float anglef_mindiff_0_2PI( float alpha, float beta );
ITA_BASE_API float anglef_mindiff_0_2PI( const float alpha, const float beta );
//! Absolute minimale Winkeldifferenz für zwei Winkel [rad] im Intervall [0,2PI)
/**
......@@ -258,7 +263,7 @@ ITA_BASE_API float anglef_mindiff_0_2PI( float alpha, float beta );
*
* Beispiele: (0,PI/2) -> PI/2, (PI/2,PI/3) -> PI/6, (PI/3,PI/2) -> PI/6, (0,2*PI-0.1) -> 0.1
*/
ITA_BASE_API float anglef_mindiff_abs_0_2PI( float alpha, float beta );
ITA_BASE_API float anglef_mindiff_abs_0_2PI( const float alpha, const float beta );
/* ---------- GRAD ---------- */
......@@ -270,7 +275,7 @@ ITA_BASE_API float anglef_mindiff_abs_0_2PI( float alpha, float beta );
*
* Beispiele: 0°->0°, 360°->0°, -360°->0°, 361°->1°, -1°->359°
*/
ITA_BASE_API float anglef_proj_0_360_DEG( float alpha );
ITA_BASE_API float anglef_proj_0_360_DEG( const float alpha );
//! Einen Winkel (°) in das Intervall (-180°, 180°] projezieren
/**
......@@ -281,7 +286,7 @@ ITA_BASE_API float anglef_proj_0_360_DEG( float alpha );
*
* Beispiele: 0°->0°, 360°->0°, -360°->0°, 181°->-179°, -181°->179°
*/
ITA_BASE_API float anglef_proj_N180_180_DEG( float alpha );
ITA_BASE_API float anglef_proj_N180_180_DEG( const float alpha );
//! (Gerichtete) minimale Winkeldifferenz für zwei Winkel [°] im Intervall [0°,360°)
/**
......@@ -302,7 +307,7 @@ ITA_BASE_API float anglef_proj_N180_180_DEG( float alpha );
*
* Beispiele: (0°,90°) -> 90°, (45°,70°) -> 25°, (70°,45°) -> -25°, (0°,359°) -> -1°
*/
ITA_BASE_API float anglef_mindiff_0_360_DEG( float alpha, float beta );
ITA_BASE_API float anglef_mindiff_0_360_DEG( const float alpha, const float beta );
//! Absolute minimale Winkeldifferenz für zwei Winkel [°] im Intervall [0°,360°)
/**
......@@ -316,7 +321,7 @@ ITA_BASE_API float anglef_mindiff_0_360_DEG( float alpha, float beta );
*
* Beispiele: (0°,90°) -> 90°, (45°,70°) -> 25°, (70°,45°) -> 25°, (0°,359°) -> 1°
*/
ITA_BASE_API float anglef_mindiff_abs_0_360_DEG( float alpha, float beta );
ITA_BASE_API float anglef_mindiff_abs_0_360_DEG( const float alpha, const float beta );
// Orientierung in Yaw-Pitch-Roll Winkeln in View-Up-Vektor umrechnen (Alle Winkel in Bogenmaß)
......@@ -340,8 +345,9 @@ ITA_BASE_API void convertVU2YPR( const float vx, const float vy, const float vz,
float& yaw, float& pitch, float& roll );
// Datenklasse für Fehlerwerte
template <typename T>
class ITA_BASE_API ErrorValues {
template< typename T >
class ITA_BASE_API ErrorValues
{
public:
T minAbsError;
T avgAbsError;
......@@ -349,12 +355,17 @@ public:
int indexMaxAbsError; // Index of element where max abs error
ErrorValues() : minAbsError( 0 ), avgAbsError( 0 ), maxAbsError( 0 ), indexMaxAbsError( 0 ) {}
inline ErrorValues()
: minAbsError( 0 )
, avgAbsError( 0 )
, maxAbsError( 0 )
, indexMaxAbsError( 0 )
{};
};
// Fehler zwischen zwei Vektoren berechnen
template <typename Tc, typename Ta, typename Tb>
ErrorValues<Tc> computeErrorValues( const Ta* A, const Tb* B, int size )
template< typename Tc, typename Ta, typename Tb >
inline ErrorValues<Tc> computeErrorValues( const Ta* A, const Tb* B, const int size )
{
ErrorValues<Tc> v;
if( size == 0 ) return v;
......@@ -362,11 +373,13 @@ ErrorValues<Tc> computeErrorValues( const Ta* A, const Tb* B, int size )
Tb absErr = std::abs( ( Tc ) A[ 0 ] - ( Tc ) B[ 0 ] );
v.minAbsError = v.avgAbsError = v.maxAbsError = absErr;
for( int i = 1; i < size; ++i ) {
for( int i = 1; i < size; ++i )
{
absErr = std::abs( ( Tc ) A[ i ] - ( Tc ) B[ i ] );
v.minAbsError = std::min( v.minAbsError, absErr );
if( absErr > v.maxAbsError ) {
if( absErr > v.maxAbsError )
{
v.maxAbsError = absErr;
v.indexMaxAbsError = i;
}
......@@ -385,45 +398,53 @@ ErrorValues<Tc> computeErrorValues( const Ta* A, const Tb* B, int size )
+-----------------------+ */
// Fills a vector with numbers dest = { a+n*s < b | n in N }
template <typename T> void linspace( std::vector<T>& dest, T a, T b, T s = 1 ) {
template< typename T > inline void linspace( std::vector< T >& dest, T a, T b, T s = 1 )
{
dest.clear();
if( s == 0 ) {
if( s == 0 )
{
dest.push_back( a );
return;
}
if( s < 0 ) {
if( a < b ) return; // No elements contained
if( s < 0 )
{
if( a < b )
return; // No elements contained
dest.reserve( ( a - b ) / s + 1 );
for( T x = a; x >= b; x += s ) dest.push_back( x );
for( T x = a; x >= b; x += s )
dest.push_back( x );
}
else {
if( a > b ) return; // No elements contained
else
{
if( a > b )
return; // No elements contained
dest.reserve( ( b - a ) / s + 1 );
for( T x = a; x <= b; x += s ) dest.push_back( x );
for( T x = a; x <= b; x += s )
dest.push_back( x );
}
};
// Fills a vector with powers of two in the range a=2^i <= 2^j <= 2^k=b
// (Note a and b must be powers of two, otherwise an exception is thrown)
ITA_BASE_API void pow2space( std::vector<int>& dest, int a, int b );
ITA_BASE_API void pow2space( std::vector< int >& dest, const int a, const int b );
// Calculates the factorial of an positive integer m
ITA_BASE_API int factorial( int m );
ITA_BASE_API int factorial( const int m );
//Calculates the normalizing constant for SHRealvaluedBasefunctions
ITA_BASE_API double SHNormalizeConst( int m, int n );
ITA_BASE_API double SHNormalizeConst( const int m, const int n );
//Calculates the Kronecker delta
ITA_BASE_API int SHKronecker( int m );
ITA_BASE_API int SHKronecker( const int m );
// Returns the index of a basefunction with degree m and order n
ITA_BASE_API int SHDegreeOrder2Linear( int m, int n );
ITA_BASE_API int SHDegreeOrder2Linear( const int m, const int n );
//Calculates the realvalued Basefunctions of SH for e.g. Ambisonics
ITA_BASE_API std::vector<double> SHRealvaluedBasefunctions( double elevation, double azimuth, int maxOrder );
ITA_BASE_API std::vector<double> SHRealvaluedBasefunctions( const double elevation, const double azimuth, const int maxOrder );
//Calculates the associated legendre polynomials
ITA_BASE_API std::vector<double> SHAssociatedLegendre( int N, double mu );
ITA_BASE_API std::vector<double> SHAssociatedLegendre( const int N, const double mu );
#endif // INCLUDE_WATCHER_ITA_NUMERIC_UTILS
src/ITANumericUtils.cpp
View file @ 6a571ffb
......@@ -3,11 +3,12 @@
#include <ITAConstants.h>
#include <ITAException.h>
#include <cassert>
#include <numeric>
using namespace ITAConstants;
int getExp2( unsigned int x )
int getExp2( const unsigned int x )
{
// Algorithmus: Bits überprüfen. Falls zwei Bits 1 sind -> keine 2er-Potenz
......@@ -30,18 +31,19 @@ int getExp2( unsigned int x )
if( k < n )
{
k++;
while( !( s = ( ( ( x >> k ) & 1 ) == 1 ) ) && k < n ) k++;
while( !( s = ( ( ( x >> k ) & 1 ) == 1 ) ) && k < n )
k++;
}
return ( s ? -1 : ( int ) l );
}
bool isPow2( unsigned int x )
bool isPow2( const unsigned int x )
{
return getExp2( x ) != -1;
}
unsigned int nextPow2( unsigned int x )
unsigned int nextPow2( const unsigned int x )
{
// Trivialer Fall:
if( x == 0 ) return 1;
......@@ -61,72 +63,88 @@ unsigned int nextPow2( unsigned int x )
if( k > 1 )
{
k--;
while( !( s = ( ( ( x >> k ) & 1 ) == 1 ) ) && k > 0 ) k--;
while( !( s = ( ( ( x >> k ) & 1 ) == 1 ) ) && k > 0 )
k--;
}
// Zweites 1-Bit gefunden? Dann keine Zweierpotenz...
return ( s ? ( ( unsigned int ) 1 ) << ( l + 1 ) : x );
}
int lwrmul( int x, int mul )
int lwrmul( const int x, const int mul )
{
int r = x % mul; // Teilungsrest
return ( r == 0 ? x : ( x > 0 ? x - r : x - r - mul ) );
}
int uprmul( int x, int mul )
int uprmul( const int x, const int mul )
{
int r = x % mul; // Teilungsrest
return ( r == 0 ? x : ( x > 0 ? x + mul - r : x - r ) );
}
unsigned int lwrmulu( unsigned int x, unsigned int mul ) {
unsigned int lwrmulu( const unsigned int x,const unsigned int mul )
{
return x - ( x % mul );
}
unsigned int uprmulu( unsigned int x, unsigned int mul ) {
unsigned int uprmulu( const unsigned int x, const unsigned int mul )
{
unsigned int r = x % mul; // Teilungsrest
return ( r == 0 ? x : x + mul - r );
}
int uprdiv( int a, int b ) {
int uprdiv( const int a, const int b )
{
// TODO: Testen mit negativen Zahlen!
int c = a / b;
if( ( a%b ) > 0 ) c++;
return c;
}
unsigned int uprdivu( unsigned int a, unsigned int b ) {
unsigned int uprdivu( const unsigned int a, const unsigned int b )
{
if( b == 0 )
return 0;
unsigned int c = a / b;
if( ( a%b ) > 0 ) c++;
return c;
}
float rad2gradf( float phi ) {
float rad2gradf( const float phi )
{
return phi * 180.0F / PI_F;
}
double rad2grad( double phi ) {
double rad2grad( const double phi )
{
return phi * 180.0 / PI_D;
}
float grad2radf( float phi ) {
return phi * PI_F / 180.0F;
float grad2radf( const float phi )
{
return phi * PI_F / 180.0f;
}
double grad2rad( double phi ) {
return phi * PI_D / 180.0;
double grad2rad( const double phi )
{
return phi * PI_D / 180.0f;
}
float correctAngle180( float phi ) {
if( fabs( phi ) == 180.0f ) return phi;
phi = fmodf( phi, 360.0f );
if( phi < -180.0f )
return ( phi + 360.0f );
return ( fmodf( phi + 180.0f, 360.0f ) - 180.0f );
float correctAngle180( const float phi )
{
if( fabs( phi ) == 180.0f )
return phi;
const float phi_temp = fmodf( phi, 360.0f );
if( phi_temp < -180.0f )
return ( phi_temp + 360.0f );
return ( fmodf( phi_temp + 180.0f, 360.0f ) - 180.0f );
}
float correctAngle360( float phi ) {
float correctAngle360( const float phi )
{
return fmodf( phi, 360.0f );
}
......@@ -162,25 +180,22 @@ double ratio_to_db20( const double r )
return 20.0f * log10( r );
}
/* +------------------------------------+
| |
| Funktionen für komplexe Zahlen |
| |
+------------------------------------+ */
float cabsf( float re, float im ) {
return sqrt( re*re + im*im );
float cabsf( const float fReal, const float fImag )
{
return sqrt( fReal * fReal + fImag * fImag );
}
float canglef( float re, float im ) {
if( re == 0 ) {
float canglef( const float re, const float im )
{
if( re == 0 )
{
// Realteil = 0
if( im == 0 )
// Komplexe Zahl 0+0i. Winkel nach Konvention 0
return 0;
else
// Rein Imaginäre Zahl, d.h. im/re -> Division durch Null!
return ( im > 0 ? PI_F / 2 : -PI_F / 2 );
return ( im > 0 ? PI_F / 2.0f : -PI_F / 2.0f );
}
else {
if( re > 0 )
......@@ -192,16 +207,30 @@ float canglef( float re, float im ) {
}
}
void csabsparg( const float& in_re, const float& in_im, const float& dest_abs, float& out_re, float& out_im ) {
void csabsparg( const float in_re, const float in_im, const float dest_abs, float& out_re, float& out_im )
{
if( dest_abs < 0.0f )
ITA_EXCEPT_INVALID_PARAMETER( "Absolute value must be greater or equal zero" );
// Nur Anpassung der Länge, d.h. Multiplikation mit konstantem
// reellen Verlängerungsfaktor c = dest_abs / abs(in)
float c = dest_abs / cabsf( in_re, in_im );
out_re = in_re * c;
out_im = in_im * c;
const float fMag = cabsf( in_re, in_im );
if( fMag == 0.0f )
{
out_re = 0.0f;
out_im = 0.0f;
}
else
{
const float c = dest_abs / fMag;
out_re = in_re * c;
out_im = in_im * c;
}
}
void csargpabs( const float& in_re, const float& in_im, const float& dest_arg, float& out_re, float& out_im ) {
void csargpabs( const float in_re, const float in_im, const float dest_arg, float& out_re, float& out_im )
{
// Nur Anpassung des Winkels: (1) Bestimmung des gegebenen
// Betrags danach (2) Auswerten der Eulerformel
......@@ -210,26 +239,23 @@ void csargpabs( const float& in_re, const float& in_im, const float& dest_arg, f
out_im = l * sin( dest_arg );
}
/* +---------------------------+
| |
| Neue Winkelfunktionen |
| |
+---------------------------+ */
/* +----------- RAD -----------+ */
float anglef_proj_0_2PI( float alpha ) {
alpha = fmodf( alpha, 2 * PI_F );
if( alpha < 0 ) alpha += 2 * PI_F;
return alpha;
float anglef_proj_0_2PI( const float alpha )
{
float alpha_temp= fmodf( alpha, 2 * PI_F );
if( alpha_temp < 0 )
alpha_temp += 2 * PI_F;
return alpha_temp;
}
float anglef_proj_NPI_PI( float alpha ) {
float anglef_proj_NPI_PI( const float alpha )
{
float x = anglef_proj_0_2PI( alpha - PI_F ) + PI_F;
return x;
}
float anglef_mindiff_0_2PI( float alpha, float beta ) {
float anglef_mindiff_0_2PI( const float alpha, const float beta )
{
float gamma = anglef_proj_0_2PI( beta ) - anglef_proj_0_2PI( alpha );
if( gamma >= 0 )
return ( gamma <= PI_F ? gamma : gamma - 2 * PI_F );
......@@ -237,36 +263,33 @@ float anglef_mindiff_0_2PI( float alpha, float beta ) {
return ( gamma >= -PI_F ? gamma : gamma + 2 * PI_F );
}
float anglef_mindiff_abs_0_2PI( float alpha, float beta ) {
float anglef_mindiff_abs_0_2PI( const float alpha, const float beta )
{
// TODO: Schnellere Implementierung möglich
return fabs( anglef_mindiff_0_2PI( alpha, beta ) );
}
/* +----------- GRAD ----------+ */
/* Benutzt sämtliche oberen Methoden, die für RAD implementiert wurden,
* unter Benutzung der grad2rad Umrechungsfunktion von GRAD zu RAD.
*/
float anglef_proj_0_360_DEG( float alpha ) {
float anglef_proj_0_360_DEG( const float alpha )
{
return rad2gradf( anglef_proj_0_2PI( grad2radf( alpha ) ) );
}
float anglef_proj_N180_180_DEG( float alpha ) {
float anglef_proj_N180_180_DEG( const float alpha ) {
return rad2gradf( anglef_proj_NPI_PI( grad2radf( alpha ) ) );
}
float anglef_mindiff_0_360_DEG( float alpha, float beta ) {
float anglef_mindiff_0_360_DEG( const float alpha, const float beta )
{
return rad2gradf( anglef_mindiff_0_2PI( grad2radf( alpha ), grad2radf( beta ) ) );
}
float anglef_mindiff_abs_0_360_DEG( float alpha, float beta ) {
float anglef_mindiff_abs_0_360_DEG( const float alpha, const float beta )
{
return rad2gradf( anglef_mindiff_abs_0_2PI( grad2radf( alpha ), grad2radf( beta ) ) );
}
void convertYPR2VU( const float yaw, const float pitch, const float roll,
float& vx, float& vy, float& vz,
float& ux, float& uy, float& uz )
void convertYPR2VU( const float yaw, const float pitch, const float roll, float& vx, float& vy, float& vz, float& ux, float& uy, float& uz )
{
double vx_, vy_, vz_, ux_, uy_, uz_;
convertYPR2VU( double( yaw ), double( pitch ), double( roll ), vx_, vy_, vz_, ux_, uy_, uz_ );
......@@ -278,9 +301,7 @@ void convertYPR2VU( const float yaw, const float pitch, const float roll,
uz = float( uz_ );
}
void convertYPR2VU( const double yaw, const double pitch, const double roll,
double& vx, double& vy, double& vz,
double& ux, double& uy, double& uz )
void convertYPR2VU( const double yaw, const double pitch, const double roll, double& vx, double& vy, double& vz, double& ux, double& uy, double& uz )
{
/*
* Yaw-pitch-roll (YPR) angles rotation order (referring to OpenGL axis)
......@@ -302,9 +323,7 @@ void convertYPR2VU( const double yaw, const double pitch, const double roll,
uy = cp*cr;
uz = -sy*sr + cy*sp*cr;
}
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 )
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 )
{
double y, p, r;
convertVU2YPR( double( vx ), double( vy ), double( vz ), double( ux ), double( uy ), double( uz ), y, p, r );
......@@ -313,9 +332,7 @@ void convertVU2YPR( const float vx, const float vy, const float vz,
roll = float( r );
}