ITABlockMath.cpp

#include <ITABlockMath.h>

#include <ITAConstants.h>

#include <memory.h>
#include <cmath>

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 * ITAConstants::PI_F * 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 * ITAConstants::PI_D * 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 ) );
}