Adding BlockMath for RavenController

parent 68f88956
......@@ -45,6 +45,7 @@ include_directories( "include" )
set( ITABaseHeader
"include/ITAASCIITable.h"
"include/ITABaseDefinitions.h"
"include/ITABlockMath.h"
"include/ITABufferedAudioFileWriter.h"
"include/ITAClock.h"
"include/ITAConstants.h"
......@@ -77,6 +78,7 @@ set( ITABaseHeader
)
set( ITABaseSources
"src/ITAASCIITable.cpp"
"src/ITABlockMath.cpp"
"src/ITABufferedAudioFileWriter.cpp"
"src/ITAClock.cpp"
"src/ITACriticalSection.cpp"
......
/*
* ----------------------------------------------------------------
*
* ITA core libs
* (c) Copyright Institute of Technical Acoustics (ITA)
* RWTH Aachen University, Germany, 2015-2016
*
* ----------------------------------------------------------------
* ____ __________ _______
* // / //__ ___/ // _ |
* // / // / // /_| |
* // / // / // ___ |
* //__/ //__/ //__/ |__|
*
* ----------------------------------------------------------------
*
*/
#ifndef INCLUDE_WATCHER_ITA_BLOCK_MATH
#define INCLUDE_WATCHER_ITA_BLOCK_MATH
#include <ITABaseDefinitions.h>
//! Komplexwertige, out-of-place Multiplikation auf Float-Arrays
/**
* Komplexwertige, out-of-place Multiplikation auf Float-Arrays
* Diese Funktion führt eine komplexwertige, Multiplikation der
* Felder zweier komplexwertig interpretierter Float-Arrays durch und speichert
* das Ergebnis in einem dritten, komplexwertig interpretierten Float-Array
* Real und Imaginärteile sind nicht interleaved sondern jeweils für sich nacheinander in einem Block.
* z.B.: Re[0], Re[1], .... Re[n-1] Im[0], Im[1], .... Re[n-1]
*
* \param dest Ziel-Array
* \param Re1, Im1 Arrays der ersten komplexen Faktoren
* \param Re1, Im1 Arrays der zweiten komplexen Faktoren
* \param count Anzahl der zu bearbeitenden <u>komplexen</u> Felder (siehe Anmerkung)
*
* \note count bezieht sich hier auf die Anzahl der komplexen Zahlen die multipliziert
* werden sollen und <u>nicht</u> auf die Anzahl an Float-Feldern.
*/
void ITA_BASE_API bm_CMul(float* pfDestRe, float* pfDestIm,
const float* pfRe1, const float* pfIm1,
const float* pfRe2, const float* pfIm2, unsigned int uiCount);
//! Komplexwertige, out-of-place Multiplikation auf CSS Float-Arrays
/**
* Komplexwertige, out-of-place Multiplikation auf Float-Arrays
* Diese Funktion führt eine komplexwertige, Multiplikation der
* Felder zweier komplexwertig interpretierter Float-Arrays durch und speichert
* das Ergebnis in einem dritten, komplexwertig interpretierten Float-Array
* Real und Imaginärteile sind im CSS Format jeweils für sich nacheinander in einem Block gespeichert.
* z.B.: Re[0], Re[1], .... Re[n-1] Im[0], Im[1], .... Re[n-1]
*
* \param Dest Ziel-Array
* \param Source1, Array der ersten komplexen Faktoren
* \param Source2, Array der zweiten komplexen Faktoren
* \param CssArraySize Anzahl der zu bearbeitenden Felder (siehe Anmerkung)
*
* \note CssArraySize bezieht sich hier auf die komplette Anzahl allocierter Float-Felder.
*/
void ITA_BASE_API bm_CMul_css( float* pfDest,
const float* pfSource1,
const float* pfSource2,
unsigned int uiCssArraySize);
//! Einfache (reale), out-of-place Multiplikation auf Float-Arrays
/**
* Diese Funktion führt eine Multiplikation von zwei Float-Arrays durch und speichert
* das Ergebnis in einem dritten Float-Array
*
* \param Dest Ziel-Array
* \param Source1, Array der ersten Faktoren
* \param Source2, Array der zweiten Faktoren
* \param uiCount Anzahl der zu bearbeitenden Felder
*/
void ITA_BASE_API bm_CMul( float* pfDest, const float* pfSrc1, const float* pfSrc2, unsigned int uiCount );
//! Komplexwertige, out-of-place Multiplikation auf CSS Float-Arrays
/**
* Diese Funktion führt eine Multiplikation eines komplexwertigen Feldes mit einem realwertigen
* Feld durch und speichert das Ergebnis in einem dritten, komplexwertig interpretierten Float-Array
* Real und Imaginärteile sind im CSS Format jeweils für sich nacheinander in einem Block gespeichert.
* z.B.: Re[0], Re[1], .... Re[n-1] Im[0], Im[1], .... Re[n-1]
*
* \param Dest Ziel-Array
* \param pfSrcCmx, Array der komplexen Faktoren
* \param pfSrcMag, Array der reellen Faktoren (Betrag)
* \param CssArraySize Anzahl der zu bearbeitenden Felder (siehe Anmerkung)
*
* \note CssArraySize bezieht sich hier auf die komplette Anzahl allocierter Float-Felder.
*/
void ITA_BASE_API bm_CMulMag_css( float* pfDest,
const float* pfSrcCmx,
const float* pfSrcMag,
unsigned int uiCssArraySize,
float fFactor = 1.0f);
/*
void bm_CMulMag_css(float *pfDest,
const float *pfSrcCmx,
const std::vector<float> &pvSrcMag,
unsigned int uiCssArraySize,
float fFactor = 1.0f);
*/
//! Komplexwertige, out-of-place Multiplikation und Addition auf Float-Arrays
/**
* Komplexwertige, Multiplikation und Addition auf das Ergebnisfeld
* Diese Funktion führt eine komplexwertige, Multiplikation der
* Felder zweier komplexwertig interpretierter Float-Arrays durch und addiert
* das Ergebnis in einem dritten, komplexwertig interpretierten Float-Array
* Real und Imaginärteile sind nicht interleaved sondern jeweils für sich nacheinander in einem Block.
* z.B.: Re[0], Re[1], .... Re[n-1] Im[0], Im[1], .... Re[n-1]
*
* \param dest Ziel-Array
* \param Re1, Im1 Arrays der ersten komplexen Faktoren
* \param Re1, Im1 Arrays der ersten komplexen Faktoren
* \param count Anzahl der zu bearbeitenden <u>komplexen</u> Felder (siehe Anmerkung)
*
* \note count bezieht sich hier auf die Anzahl der komplexen Zahlen die multipliziert
* werden sollen und <u>nicht</u> auf die Anzahl an Float-Feldern.
*/
void ITA_BASE_API bm_CMulAdd( float* pfDestRe, float* pfDestIm,
const float* pfRe1, const float* pfIm1,
const float* pfRe2, const float* pfIm2, unsigned int uiCount);
//! Komplexwertige, out-of-place Multiplikation und Addition auf Float-Arrays
/**
* Komplexwertige, Multiplikation und Addition auf das Ergebnisfeld
* Diese Funktion führt eine komplexwertige, Multiplikation der
* Felder zweier komplexwertig interpretierter Float-Arrays durch und addiert
* das Ergebnis in einem dritten, komplexwertig interpretierten Float-Array
* Real und Imaginärteile sind im CSS Format jeweils für sich nacheinander in einem Block gespeichert.
* z.B.: Re[0], Re[1], .... Re[n-1] Im[0], Im[1], .... Re[n-1]
*
* \param Dest Ziel-Array
* \param Source1, Array der ersten komplexen Faktoren
* \param Source2, Array der zweiten komplexen Faktoren
* \param CssArraySize Anzahl der zu bearbeitenden Felder (siehe Anmerkung)
*
* \note CssArraySize bezieht sich hier auf die komplette Anzahl allocierter Float-Felder.
*/
void ITA_BASE_API bm_CMulAdd_css( float* pfDest,
const float* pfSource1,
const float* pfSource2,
unsigned int uiCssArraySize);
//! Komplexwertige, out-of-place Multiplikation und Subtraktion auf Float-Arrays
/**
* Komplexwertige, Multiplikation und Subtraktion vom Ergebnisfeld
* Diese Funktion führt eine komplexwertige, Multiplikation der
* Felder zweier komplexwertig interpretierter Float-Arrays durch und addiert
* das Ergebnis in einem dritten, komplexwertig interpretierten Float-Array
* Real und Imaginärteile sind nicht interleaved sondern jeweils für sich nacheinander in einem Block.
* z.B.: Re[0], Re[1], .... Re[n-1] Im[0], Im[1], .... Re[n-1]
*
* \param dest Ziel-Array
* \param Re1, Im1 Arrays der ersten komplexen Faktoren
* \param Re1, Im1 Arrays der zweiten komplexen Faktoren
* \param count Anzahl der zu bearbeitenden <u>komplexen</u> Felder (siehe Anmerkung)
*
* \note count bezieht sich hier auf die Anzahl der komplexen Zahlen die multipliziert
* werden sollen und <u>nicht</u> auf die Anzahl an Float-Feldern.
*/
void ITA_BASE_API bm_CMulSub( float* pfDestRe, float* pfDestIm,
const float* pfRe1, const float* pfIm1,
const float* pfRe2, const float* pfIm2, unsigned int uiCount);
//! Komplexwertige, out-of-place Multiplikation und Subtraktion auf Float-Arrays
/**
* Komplexwertige, Multiplikation und Subtraktion vom Ergebnisfeld
* Diese Funktion führt eine komplexwertige, Multiplikation der
* Felder zweier komplexwertig interpretierter Float-Arrays durch und addiert
* das Ergebnis in einem dritten, komplexwertig interpretierten Float-Array
* Real und Imaginärteile sind im CSS Format jeweils für sich nacheinander in einem Block gespeichert.
* z.B.: Re[0], Re[1], .... Re[n-1] Im[0], Im[1], .... Re[n-1]
*
* \param Dest Ziel-Array
* \param Source1, Array der ersten komplexen Faktoren
* \param Source2, Array der zweiten komplexen Faktoren
* \param CssArraySize Anzahl der zu bearbeitenden Felder (siehe Anmerkung)
*
* \note CssArraySize bezieht sich hier auf die komplette Anzahl allocierter Float-Felder.
*/
void ITA_BASE_API bm_CMulSub_css( float* pfDest,
const float* pfSource1,
const float* pfSource2,
unsigned int uiCssArraySize);
//! Komplexwertige, out-of-place Division auf Float-Arrays
/**
// Diese Funktion führt eine komplexwertige, Division der
// Felder zweier komplexwertig interpretierter Float-Arrays durch und speichert
// das Ergebnis in einem dritten, komplexwertig interpretiertem Float-Array
// Real und Imaginärteile sind nicht interleaved sondern jeweils für sich nacheinander in einem Block.
// z.B.: Re[0], Re[1], .... Re[n-1] Im[0], Im[1], .... Re[n-1]
*
* \param dest Ziel-Array
* \param ZRe, ZRe Zähler
* \param NRe, NRe Nenner
* \param count Anzahl der zu bearbeitenden <u>komplexen</u> Felder (siehe Anmerkung)
*
* \note count bezieht sich hier auf die Anzahl der komplexen Zahlen die multipliziert
* werden sollen und <u>nicht</u> auf die Anzahl an Float-Feldern.
*/
void ITA_BASE_API bm_CDiv( float* pfDestRe, float* pfDestIm,
const float* pfZRe, const float* pfZIm,
const float* pfNRe, const float* pfNIm, unsigned int uiCount);
//! Komplexwertige, out-of-place Division auf Float-Arrays
/**
// Diese Funktion führt eine komplexwertige, Division der
// Felder zweier komplexwertig interpretierter Float-Arrays durch und speichert
// das Ergebnis in einem dritten, komplexwertig interpretiertem Float-Array
// Real und Imaginärteile sind nicht interleaved sondern jeweils für sich nacheinander in einem Block.
// z.B.: Re[0], Re[1], .... Re[n-1] Im[0], Im[1], .... Re[n-1]
*
* \param dest Ziel-Array
* \param Z, ZRe Zähler
* \param N, NRe Nenner
* \param CssArraySize Anzahl der zu bearbeitenden Felder (siehe Anmerkung)
*
* \note CssArraySize bezieht sich hier auf die komplette Anzahl allocierter Float-Felder.
*/
void ITA_BASE_API bm_CDiv_css( float* pfDest,
const float* pfZ,
const float* pfN,
unsigned int uiCssArraySize);
//! In-place Vektoraddition auf Float-Arrays
/**
* Diese Funktion führt eine feldweise Addition zweier Float-Arrays durch
* entsprechend folgender Vorschrift durch:
*
* for i from 0 to count-1:
* dest[i] += summand[i]
*
* \param dest Ziel-Array
* \param summand Summanden-Array
* \param count Anzahl der zu bearbeitenden Felder
*/
void ITA_BASE_API bm_Add( float* pfDest, const float* pfSummand, unsigned int uiCount );
//! ContinueMagnitudePreservePhase auf Float-Arrays
/**
// Der Betrag der aller komplexen Zahlen wird mit dem Bertrag der aktuellen Zahl überschrieben,
// wobei allerdings die Phase der jeweiligen Komplexen Zahl erhalten bleibt.
// uiCount gibt die Anzahl der überschriebenen Felder an, also hinter dem aktuellen Feld, das übergeben wurde.
// Real und Imaginärteile sind nicht interleaved sondern jeweils für sich nacheinander in einem Block.
// z.B.: Re[0], Re[1], .... Re[n-1] Im[0], Im[1], .... Re[n-1]
*
* \param pfRe Zeiger auf den Realteil
* \param pfIm Zeiger auf den Imaginärteil
* \param count Anzahl der zu bearbeitenden Felder
*/
void ITA_BASE_API bm_ContinueMagnitudePreservePhase( float *pfRe, float *pfIm, unsigned int uiCount );
//! ContinueMagnitudePreservePhase auf Float-Arrays
/**
// Der Betrag der aller komplexen Zahlen ab der Startfrequenz wird mit dem Bertrag
// bei dieser Frequenz überschrieben, wobei allerdings die Phase der jeweiligen
// Komplexen Zahl erhalten bleibt.
// fStartFrequency gibt an, ab wann die Fortsetzung gelten soll.
// Real und Imaginärteile sind nicht interleaved sondern jeweils für sich nacheinander in einem Block.
// z.B.: Re[0], Re[1], .... Re[n-1] Im[0], Im[1], .... Re[n-1]
*
* \param pfData Zeiger auf die Daten
* \param fStartFrequency: Ab dieser Frequenz fortsetzen
* \param fSamplerate Abtastrate mit der die Daten vorliegen
* \param CssArraySize Anzahl der zu bearbeitenden Felder (siehe Anmerkung)
*
* \note CssArraySize bezieht sich hier auf die komplette Anzahl allocierter Float-Felder.
*/
void ITA_BASE_API bm_ContinueMagnitudePreservePhase_css( float *pfData,
float fStartFrequency,
float fSamplerate,
unsigned int uiCssArraySize);
//! MaxMag auf Float-Arrays
/**
// Es wird das absolute Maximum gesucht und der Wert (hier jetzt nicht Betrag) sowie die Position zurückgegeben
// Hat nur Sinn bei Zeitbereichsdaten
*
* \param pfData Zeiger auf das Array
* \param uiCount Anzahl der zu bearbeitenden Felder
* \param fValue enthält den Maximalen Wert
* \param uiPosition enthält die Position des Maximums
*/
void ITA_BASE_API bm_MaxMag( float *pfData, unsigned int uiCount, float &fValue, unsigned int &uiPosition );
//! Max auf Float-Arrays
/**
// Es wird das Maximum gesucht und der Wert sowie die Position zurückgegeben
// Hat nur Sinn bei Zeitbereichsdaten
*
* \param pfData Zeiger auf das Array
* \param uiCount Anzahl der zu bearbeitenden Felder
* \param fValue enthält den Maximalen Wert
* \param uiPosition enthält die Position des Maximums
*/
void ITA_BASE_API bm_Max( float *pfData, unsigned int uiCount, float &fValue, unsigned int &uiPosition );
//! CyclicMove auf Float-Arrays
/**
// Die vorder und hintere Hälfte des Filters werden getauscht
// Hat nur Sinn bei Zeitbereichsdaten
*
* \param pfData Zeiger auf das Array
* \param uiCount Anzahl der zu bearbeitenden Felder
*/
void ITA_BASE_API bm_CyclicMove( float *pfData, unsigned int uiCount );
//! FreqIdent auf Float-Arrays
/**
// Ein EinsSpektrum erzeugen (Realteile=1 Imaginärteile=0)
// Hat nur Sinn bei Frequenzdaten
*
* \param pfData Zeiger auf das Array
* \param CssArraySize Anzahl der zu bearbeitenden Felder (siehe Anmerkung)
*
* \note CssArraySize bezieht sich hier auf die komplette Anzahl allocierter Float-Felder.
*/
void ITA_BASE_API bm_FreqIdent_css( float *pfData, unsigned int uiCssArraySize );
//! SubSampleShift auf Float-Arrays
/**
// Das Signal wird im Sub-Samplebereich nach rechts geschoben
// Dies geschieht durch Manipulation der Pase im Frequenzbereich
// Hat nur Sinn bei Frequenzdaten
*
* \param pfData Zeiger auf das Array. Frequenzdaten!!
* \param SubSampleShift: Verschiebung in Samples ( <1 )
* \param CssArraySize: Anzahl der zu bearbeitenden Felder (siehe Anmerkung)
*
* \note CssArraySize bezieht sich hier auf die komplette Anzahl allocierter Float-Felder.
*/
void ITA_BASE_API bm_SubSampleShift_css( float *pfData, float fSubSampleShift, unsigned int uiCssArraySize );
//! SubSampleShift auf symetrischen Half-Size DFT-Spektren (interleaved format)
/**
* Identisch zur Funktion bm_SubSampleShift_css. Datenformat allerdings Interleaved (Re, Im, Re, Im, ...)
*
* \param pfData Datenarray mit den komplexwertigen DFT-Koeffizienten
* \param fSubSampleShift Verschiebung in Samples (<1)
* \param uiCoeffs Anzahl komplexwertiger DFT-Koeffizienten (!) im Datenarray
*/
void ITA_BASE_API bm_SubSampleShift_SHDI( float *pfData, float fSubSampleShift, unsigned int uiCoeffs );
//! GetNextFFTLength
/**
// Gibt die nächstgrößere FFT-Länge zur Basis 2 zurück
*
* \param uiLength: Länge, zu der die nächste FFT-Länge gesucht ist
*/
unsigned int ITA_BASE_API bm_GetNextFFTLength( unsigned int uiLength );
#endif // INCLUDE_WATCHER_ITA_BLOCK_MATH
#include <ITABlockMath.h>
#include <memory.h>
#include <cmath>
#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif
void bm_CMul(float* pfDestRe, float* pfDestIm, const float* pfRe1, const float* pfIm1, const float* pfRe2, const float* pfIm2, unsigned int uiCount) {
for (unsigned int i=0; i<uiCount; i++) {
*pfDestRe++ = *pfRe1 * *pfRe2 - *pfIm1 * *pfIm2;
*pfDestIm++ = *pfRe1++ * *pfIm2++ + *pfIm1++ * *pfRe2++;
}
}
void bm_CMul(float* pfDest, const float* pfSrc1, const float* pfSrc2, unsigned int uiCount) {
for (unsigned int i=0; i<uiCount; i++)
*pfDest++ = *pfSrc1++ * *pfSrc2++;
}
void bm_CMul_css(float* pfDest, const float* pfSource1, const float* pfSource2, unsigned int uiCssArraySize) {
float *pfDestRe = pfDest;
float *pfDestIm = pfDest + uiCssArraySize/2;
const float *pfRe1 = pfSource1;
const float *pfIm1 = pfSource1 + uiCssArraySize/2;
const float *pfRe2 = pfSource2;
const float *pfIm2 = pfSource2 + uiCssArraySize/2;
for (unsigned int i=0; i<uiCssArraySize/2; i++) {
*pfDestRe++ = *pfRe1 * *pfRe2 - *pfIm1 * *pfIm2;
*pfDestIm++ = *pfRe1++ * *pfIm2++ + *pfIm1++ * *pfRe2++;
}
}
void bm_CMulMag_css(float* pfDest, const float* pfSrcCmx, const float* pfSrcMag, unsigned int uiCssArraySize, float fFactor) {
float *pfDestRe = pfDest;
float *pfDestIm = pfDest + uiCssArraySize/2;
const float *pfRe1 = pfSrcCmx;
const float *pfIm1 = pfSrcCmx + uiCssArraySize/2;
for (unsigned int i=0; i<uiCssArraySize/2; i++) {
*pfDestRe++ = *pfRe1++ * *pfSrcMag * fFactor;
*pfDestIm++ = *pfIm1++ * *pfSrcMag++ * fFactor;
}
}
void bm_CMulAdd(float* pfDestRe, float* pfDestIm, const float* pfRe1, const float* pfIm1, const float* pfRe2, const float* pfIm2, unsigned int uiCount) {
for (unsigned int i=0; i<uiCount; i++) {
*pfDestRe++ += *pfRe1 * *pfRe2 - *pfIm1 * *pfIm2;
*pfDestIm++ += *pfRe1++ * *pfIm2++ + *pfIm1++ * *pfRe2++;
}
}
void bm_CMulAdd_css(float* pfDest, const float* pfSource1, const float* pfSource2, unsigned int uiCssArraySize) {
float *pfDestRe = pfDest;
float *pfDestIm = pfDest + uiCssArraySize/2;
const float *pfRe1 = pfSource1;
const float *pfIm1 = pfSource1 + uiCssArraySize/2;
const float *pfRe2 = pfSource2;
const float *pfIm2 = pfSource2 + uiCssArraySize/2;
for (unsigned int i=0; i<uiCssArraySize/2; i++) {
*pfDestRe++ += *pfRe1 * *pfRe2 - *pfIm1 * *pfIm2;
*pfDestIm++ += *pfRe1++ * *pfIm2++ + *pfIm1++ * *pfRe2++;
}
}
void bm_CMulSub(float* pfDestRe, float* pfDestIm, const float* pfRe1, const float* pfIm1, const float* pfRe2, const float* pfIm2, unsigned int uiCount) {
for (unsigned int i = 0; i < uiCount; i++ ) {
*pfDestRe++ -= *pfRe1 * *pfRe2 - *pfIm1 * *pfIm2;
*pfDestIm++ -= *pfRe1++ * *pfIm2++ + *pfIm1++ * *pfRe2++;
}
}
void bm_CMulSub_css(float* pfDest, const float* pfSource1, const float* pfSource2, unsigned int uiCssArraySize) {
float *pfDestRe = pfDest;
float *pfDestIm = pfDest + uiCssArraySize/2;
const float *pfRe1 = pfSource1;
const float *pfIm1 = pfSource1 + uiCssArraySize/2;
const float *pfRe2 = pfSource2;
const float *pfIm2 = pfSource2 + uiCssArraySize/2;
for (unsigned int i=0; i<uiCssArraySize/2; i++ ) {
*pfDestRe++ -= *pfRe1 * *pfRe2 - *pfIm1 * *pfIm2;
*pfDestIm++ -= *pfRe1++ * *pfIm2++ + *pfIm1++ * *pfRe2++;
}
}
void bm_CDiv(float* pfDestRe, float* pfDestIm, const float* pfZRe, const float* pfZIm, const float* pfNRe, const float* pfNIm, unsigned int uiCount) {
float fNenner = 0.0;
for (unsigned int i=0; i<uiCount; i++) {
fNenner = *pfNRe * *pfNRe + *pfNIm * *pfNIm;
*pfDestRe = *pfZRe * *pfNRe + *pfZIm * *pfNIm;
*pfDestRe++ /= fNenner;
*pfDestIm = *pfNRe++ * *pfZIm++ - *pfZRe++ * *pfNIm++;
*pfDestIm++ /= fNenner;
}
}
/*
void bm_CMulMag_css(float *pfDest, const float *pfSrcCmx, const std::vector<float> &pvSrcMag, unsigned int uiCssArraySize, float fFactor) {
float *pfDestRe = pfDest;
float *pfDestIm = pfDest + uiCssArraySize/2;
const float *pfRe1 = pfSrcCmx;
const float *pfIm1 = pfSrcCmx + uiCssArraySize/2;
for (unsigned int i=0; i<uiCssArraySize/2; i++) {
*pfDestRe++ = *pfRe1++ * pvSrcMag[i] * fFactor;
*pfDestIm++ = *pfIm1++ * pvSrcMag[i] * fFactor;
}
}
*/
void bm_CDiv_css(float* pfDest, const float* pfZ, const float* pfN, unsigned int uiCssArraySize) {
float *pfDestRe = pfDest;
float *pfDestIm = pfDest + uiCssArraySize/2;
const float *pfZRe = pfZ;
const float *pfZIm = pfZ + uiCssArraySize/2;
const float *pfNRe = pfN;
const float *pfNIm = pfN + uiCssArraySize/2;
float fNenner = 0.0;
for (unsigned int i=0; i<uiCssArraySize/2; i++) {
fNenner = *pfNRe * *pfNRe + *pfNIm * *pfNIm;
*pfDestRe = *pfZRe * *pfNRe + *pfZIm * *pfNIm;
*pfDestRe++ /= fNenner;
*pfDestIm = *pfNRe++ * *pfZIm++ - *pfZRe++ * *pfNIm++;
*pfDestIm++ /= fNenner;
}
}
void bm_Add(float* pfDest, const float* pfSummand, unsigned int uiCount) {
for (unsigned int i=0; i<uiCount; i++)
*pfDest++ += *pfSummand++;
}
void bm_ContinueMagnitudePreservePhase(float *pfRe, float *pfIm, unsigned int uiCount) {
float fFactor = 0.0;
float fConstMag = sqrtf((powf(*pfRe,2)) + (powf(*pfIm,2)));
for (unsigned int i=0 ; i<uiCount; i++) {
fFactor = fConstMag / sqrtf((powf(*pfRe,2)) + (powf(*pfIm,2))) ;
*pfRe = fFactor * *pfRe++;
*pfIm = fFactor * *pfIm++;
}
}
void bm_ContinueMagnitudePreservePhase_css(float *pfData, float fStartFrequency, float fSamplerate, unsigned int uiCssArraySize) {
unsigned int uiStartBin = (unsigned int)floor(fStartFrequency * uiCssArraySize / fSamplerate );
float *pfRe = pfData;
float *pfIm = pfData + uiCssArraySize/2;
float fFactor = 0.0;
float fConstMag = sqrtf( powf(pfRe[uiStartBin],2) + powf(pfIm[uiStartBin],2) );
for (unsigned int i=uiStartBin ; i<((uiCssArraySize/2) - 1); i++) {
fFactor = fConstMag / sqrtf( powf(pfRe[i],2) + powf(pfIm[i],2) ) ;
pfRe[i] = fFactor * pfRe[i];
pfIm[i] = fFactor * pfIm[i];
}
pfRe[(uiCssArraySize/2) - 1] = 0;
pfIm[(uiCssArraySize/2) - 1] = 0;
}
void bm_MaxMag(float *pfData, unsigned int uiCount, float &fValue, unsigned int &uiPosition) {
float fTmp = 0.0;
uiPosition = 0;
for (unsigned int i=0 ; i<uiCount; i++) {
if (fabs(pfData[i]) > fTmp) {
uiPosition = i;
fTmp= pfData[i];
}
}
fValue = fTmp;
}
void bm_Max(float *pfData, unsigned int uiCount, float &fValue, unsigned int &uiPosition) {
float fTmp = 0.0;
uiPosition = 0;
for (unsigned int i=0 ; i<uiCount; i++) {
if (pfData[i] > fTmp) {
uiPosition = i;
fTmp= pfData[i];
}
}
fValue = fTmp ;
}
void bm_CyclicMove(float *pfData, unsigned int uiCount) {
float fTmp = 0.0;
unsigned int uiOffset = uiCount/2;
for (unsigned int i=0 ; i<uiOffset; i++) {
fTmp = pfData[i];
pfData[i] = pfData[i + uiOffset];
pfData[i + uiOffset] = fTmp;
}
}
void bm_FreqIdent_css(float *pfData, unsigned int uiCssArraySize) {
for (unsigned int i=0 ; i<uiCssArraySize/2; i++)
pfData[i] = 1.0;
memset(pfData + uiCssArraySize/2, 0, (uiCssArraySize/2) * sizeof(float));
}
void bm_SubSampleShift_css(float *pfData, float fSubSampleShift, unsigned int uiCssArraySize) {
float fPhi = 0.0;
float fCosinusPhi = 0.0;
float fSinusPhi = 0.0;
float fTmpRe = 0.0;
float fTmpIm = 0.0;
float *pfRe = pfData;
float *pfIm = pfData + uiCssArraySize / 2;
float fDeltaPhi = (float)-2.0 * (float)M_PI * fSubSampleShift / (float)uiCssArraySize;
for (unsigned int i=0; i<uiCssArraySize/2; i++) {
fPhi = fDeltaPhi * i;
fCosinusPhi = cosf(fPhi);
fSinusPhi = sinf(fPhi);
fTmpRe = pfRe[i];
fTmpIm = pfIm[i];
pfRe[i] = fTmpRe * fCosinusPhi - fTmpIm * fSinusPhi ;
pfIm[i] = fTmpRe * fSinusPhi + fTmpIm * fCosinusPhi ;
}
}
void bm_SubSampleShift_SHDI(float *pfData, float fSubSampleShift, unsigned int uiCoeffs) {
unsigned int uiDFTSize = (uiCoeffs-1)*2;
float fPhi, fCosinusPhi, fSinusPhi;
float fTmpRe, fTmpIm;
//float *pfRe = pfData;
//float *pfIm = pfData + uiCssArraySize / 2;
float fDeltaPhi = (float) -1.0 * (float) M_PI * fSubSampleShift / (float) uiDFTSize;
for (unsigned int i=0; i<uiCoeffs; i++) {
fPhi = fDeltaPhi * i;
fCosinusPhi = cosf(fPhi);
fSinusPhi =