Adding BlockMath for RavenController

CMakeLists.txt

@@ -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"

src/ITABlockMath.cpp

+#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 = sinf(fPhi);
+
+		fTmpRe = pfData[2*i];
+		fTmpIm = pfData[2*i+1];
+				
+		pfData[2*i]   = fTmpRe * fCosinusPhi - fTmpIm * fSinusPhi;
+		pfData[2*i+1] = fTmpRe * fSinusPhi   + fTmpIm * fCosinusPhi;
+	}
+}
+
+unsigned int bm_GetNextFFTLength(unsigned int uiLength) {
+	unsigned int uiFFTDegree = (unsigned int)floor(0.5 + log((float)uiLength)/log((float)2));
+	return ((unsigned int) pow(2.0f, (float) uiFFTDegree));
+}