Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Sign in
Toggle navigation
ITABase
Project overview
Project overview
Details
Activity
Releases
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Locked Files
Issues
0
Issues
0
List
Boards
Labels
Service Desk
Milestones
Iterations
Merge Requests
0
Merge Requests
0
Requirements
Requirements
List
Security & Compliance
Security & Compliance
Dependency List
License Compliance
Operations
Operations
Incidents
Analytics
Analytics
Code Review
Insights
Issue
Repository
Value Stream
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Create a new issue
Commits
Issue Boards
Open sidebar
Institute of Technical Acoustics (ITA)
ITABase
Commits
6a571ffb
Commit
6a571ffb
authored
Jun 16, 2017
by
Dipl.-Ing. Jonas Stienen
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Hotfixes in numeric utils, also some refactoring
parent
2145e66c
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
with
188 additions
and
144 deletions
+188
-144
include/ITANumericUtils.h
include/ITANumericUtils.h
+75
-54
src/ITANumericUtils.cpp
src/ITANumericUtils.cpp
+113
-90
No files found.
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.5
f
)
:
floor
(
(
x
)
+
0.5
f
)
);
}
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.5
f
)
:
floor
(
(
x
)
+
0.5
f
)
);
};
#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.0
F
/
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.0
F
;
float
grad2radf
(
const
float
phi
)
{
return
phi
*
PI_F
/
180.0
f
;
}
double
grad2rad
(
double
phi
)
{
return
phi
*
PI_D
/
180.0
;
double
grad2rad
(
const
double
phi
)
{
return
phi
*
PI_D
/
180.0
f
;
}
float
correctAngle180
(
float
phi
)
{
if
(
fabs
(
phi
)
==
180.0
f
)
return
phi
;
phi
=
fmodf
(
phi
,
360.0
f
);
if
(
phi
<
-
180.0
f
)
return
(
phi
+
360.0
f
);
return
(
fmodf
(
phi
+
180.0
f
,
360.0
f
)
-
180.0
f
);
float
correctAngle180
(
const
float
phi
)
{
if
(
fabs
(
phi
)
==
180.0
f
)
return
phi
;
const
float
phi_temp
=
fmodf
(
phi
,
360.0
f
);
if
(
phi_temp
<
-
180.0
f
)
return
(
phi_temp
+
360.0
f
);
return
(
fmodf
(
phi_temp
+
180.0
f
,
360.0
f
)
-
180.0
f
);
}
float
correctAngle360
(
float
phi
)
{
float
correctAngle360
(
const
float
phi
)
{
return
fmodf
(
phi
,
360.0
f
);
}
...
...
@@ -162,25 +180,22 @@ double ratio_to_db20( const double r )
return
20.0
f
*
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
.0
f
:
-
PI_F
/
2.0
f
);
}
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.0
f
)
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.0
f
)
{
out_re
=
0.0
f
;
out_im
=
0.0
f
;
}
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
);
}