ITANCTC.cpp 17.5 KB
Newer Older
Jonas Stienen's avatar
Jonas Stienen committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14
#include <ITANCTC.h>

#include <assert.h>
#include <complex>
#include <cmath>
#include <iostream>

#include <DAFF.h>

#include <ITAConstants.h>
#include <ITAException.h>
#include <ITAHDFTSpectrum.h>
#include <ITANumericUtils.h>
#include <ITAStringUtils.h>
15
#include <ITAFFTUtils.h>
Jonas Stienen's avatar
Jonas Stienen committed
16 17 18 19 20 21

ITANCTC::ITANCTC( const Config& oNCTCConfig )
	: m_oConfig( oNCTCConfig )
	, m_pHRIR( NULL )
{
	m_fBeta = float( 1e-4 );
22

23 24 25
	if( oNCTCConfig.N <= 0 )
		ITA_EXCEPT1( INVALID_PARAMETER, "Wrong configuration, N-CTC requires a valid number of loudspeakers" );

Jonas Stienen's avatar
Jonas Stienen committed
26
	m_iOptimization = m_oConfig.iOptimization;
27 28
	m_fCrossTalkCancellationFactor = m_oConfig.fCrossTalkCancellationFactor;
	m_fWaveIncidenceAngleCompensationFactor = m_oConfig.fWaveIncidenceAngleCompensationFactor;
Jonas Stienen's avatar
Jonas Stienen committed
29 30 31

	m_oHeadPose.vPos.SetToZeroVector();
	m_oHeadPose.vView.SetValues( 0, 0, -1.0f );
32
	m_oHeadPose.vUp.SetValues( 0, 1.0f, 0 );
Jonas Stienen's avatar
Jonas Stienen committed
33 34 35
	//m_oHeadPose.qOrient.SetToNeutralQuaternion();

	m_sfCTC_temp.init( 2, m_oConfig.iCTCFilterLength, true );
36 37 38

	int iDFTSize = m_oConfig.iCTCFilterLength + 1;
	for( int n = 0; n < GetN(); n++ )
Jonas Stienen's avatar
Jonas Stienen committed
39 40 41
	{
		m_vdWeights.push_back( 1.0f );
		m_vpHRTFs.push_back( new ITAHDFTSpectra( m_oConfig.dSampleRate, 2, iDFTSize, true ) );
42
		m_vfDelayTime.push_back( float( m_oConfig.iCTCFilterLength / m_oConfig.dSampleRate / 2.0f ) );
Jonas Stienen's avatar
Jonas Stienen committed
43 44
	}

45
	for( int i = 0; i < 2; i++ )
Jonas Stienen's avatar
Jonas Stienen committed
46 47 48 49 50
		m_vpHelper2x2.push_back( new ITAHDFTSpectra( m_oConfig.dSampleRate, 2, iDFTSize, true ) );

	t = new ITAHDFTSpectrum( m_oConfig.dSampleRate, iDFTSize, true );
	det = new ITAHDFTSpectrum( m_oConfig.dSampleRate, iDFTSize, true );

51 52 53
	int l = m_oConfig.iCTCFilterLength;
	m_fft.plan( ITAFFT::FFT_R2C, l, m_sfCTC_temp[ 0 ].data(), ( *m_vpHRTFs[ 0 ] )[ 0 ]->data() );
	m_ifft.plan( ITAFFT::IFFT_C2R, l, ( *m_vpHRTFs[ 0 ] )[ 0 ]->data(), m_sfCTC_temp[ 0 ].data() );
Jonas Stienen's avatar
Jonas Stienen committed
54 55 56 57 58

}

ITANCTC::~ITANCTC()
{
59 60
	for( int n = 0; n < GetN(); n++ )
		delete m_vpHRTFs[ n ];
Jonas Stienen's avatar
Jonas Stienen committed
61

62 63
	for( int i = 0; i < 2; i++ )
		delete m_vpHelper2x2[ i ];
Jonas Stienen's avatar
Jonas Stienen committed
64 65

	delete t, det;
66

67
	return;
Jonas Stienen's avatar
Jonas Stienen committed
68 69 70 71 72 73 74 75 76 77 78 79 80 81
}

const ITANCTC::Config& ITANCTC::GetConfig() const
{
	return m_oConfig;
}

int ITANCTC::GetN() const
{
	return m_oConfig.N;
}

void ITANCTC::UpdateHeadPose( const Pose& oHead )
{
82 83
	m_oHeadPose = oHead;

Jonas Stienen's avatar
Jonas Stienen committed
84 85 86
	return;
}

87
int ITANCTC::GetLoudspeakerSide( int iNumLoudspeaker )
88
{
89
	Pose oDest = GetLoudspeakerPose( iNumLoudspeaker );
90
	VistaVector3D vConn = oDest.vPos - m_oHeadPose.vPos;
91 92
	double dDistanceMeters = double( vConn.GetLength() );
	float fLS2HeadDelaySamples = ( float ) dDistanceMeters / m_oConfig.fSpeedOfSound * ( float ) m_oConfig.dSampleRate;
93 94 95

	// 1/r attenuation due to distance law
	// Note: Gain of HRIR dataset is normalized to 1 m according to convention
96
	float fDistanceGain = 1 / ( float ) dDistanceMeters;
97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116

	// @todo jst: mit VistaQuaternion lsen
	/*
	VistaVector3D oFrom = vConn;
	oFrom.Normalize();
	VistaQuaternion qOri( Vista::ViewVector, oFrom );
	VistaQuaternion qHead2LS = qOri * m_oHeadPose.qOrient;
	float fX, fY, fZ;
	qHead2LS.GetAngles( fX, fY, fZ );

	VistaVector3D v3View = qHead2LS.GetViewDir();

	float fPhi = 180.0f / PI_F * fY;
	float fTheta = 180.0f / PI_F * fX;
	*/

	double dPhiDeg, dThetaDeg;

	VistaVector3D vDir = oDest.vPos - m_oHeadPose.vPos;
	vDir.Normalize();
117 118 119 120 121 122
	VistaVector3D vViewMinusZ = m_oHeadPose.vView * ( -1.0f ); // local z axis
	const VistaVector3D vRight = vViewMinusZ.Cross( m_oHeadPose.vUp ); // local x axis
	const double dAzimuthAngleDeg = atan2( vDir.Dot( vRight ), vDir.Dot( m_oHeadPose.vView ) ) * 180.0f / ITAConstants::PI_D;
	dPhiDeg = ( ( dAzimuthAngleDeg < 0.0f ) ? ( dAzimuthAngleDeg + 360.0f ) : dAzimuthAngleDeg );
	dThetaDeg = asin( vDir.Dot( m_oHeadPose.vUp ) ) * 180.0f / ITAConstants::PI_D;
	if( dPhiDeg < 180 )
123 124 125 126 127 128 129
	{
		return Config::Loudspeaker::LEFT_SIDE;
	}
	else
	{
		return Config::Loudspeaker::RIGHT_SIDE;
	}
130

131 132
}

Jonas Stienen's avatar
Jonas Stienen committed
133 134 135 136 137 138 139 140
bool ITANCTC::AddHRIR( const Pose& oDest, ITASampleFrame& sfDestHRIR, bool& bOutOfRange, double dReflectionFactor/* = 1.0f */ ) const
{
	if( sfDestHRIR.channels() != 2 )
		ITA_EXCEPT1( INVALID_PARAMETER, "Two channel HRIR expected" );

	// Calculate sample delay of HRIR insertion position
	VistaVector3D vConn = oDest.vPos - m_oHeadPose.vPos;
	double dDistanceMeters = double( vConn.GetLength() );
141 142
	float fLS2HeadDelaySamples = ( float ) dDistanceMeters / m_oConfig.fSpeedOfSound * ( float ) m_oConfig.dSampleRate;

Jonas Stienen's avatar
Jonas Stienen committed
143 144
	// 1/r attenuation due to distance law
	// Note: Gain of HRIR dataset is normalized to 1 m according to convention
145
	float fDistanceGain = 1 / ( float ) dDistanceMeters;
Jonas Stienen's avatar
Jonas Stienen committed
146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165

	// @todo jst: mit VistaQuaternion lsen
	/*
	VistaVector3D oFrom = vConn;
	oFrom.Normalize();
	VistaQuaternion qOri( Vista::ViewVector, oFrom );
	VistaQuaternion qHead2LS = qOri * m_oHeadPose.qOrient;
	float fX, fY, fZ;
	qHead2LS.GetAngles( fX, fY, fZ );

	VistaVector3D v3View = qHead2LS.GetViewDir();

	float fPhi = 180.0f / PI_F * fY;
	float fTheta = 180.0f / PI_F * fX;
	*/

	double dPhiDeg, dThetaDeg;

	VistaVector3D vDir = oDest.vPos - m_oHeadPose.vPos;
	vDir.Normalize();
166
	VistaVector3D vViewMinusZ = m_oHeadPose.vView * ( -1.0f ); // local z axis
Jonas Stienen's avatar
Jonas Stienen committed
167 168 169 170
	const VistaVector3D vRight = vViewMinusZ.Cross( m_oHeadPose.vUp ); // local x axis
	const double dAzimuthAngleDeg = atan2( vDir.Dot( vRight ), vDir.Dot( m_oHeadPose.vView ) ) * 180.0f / ITAConstants::PI_D;
	dPhiDeg = ( ( dAzimuthAngleDeg < 0.0f ) ? ( dAzimuthAngleDeg + 360.0f ) : dAzimuthAngleDeg );
	dThetaDeg = asin( vDir.Dot( m_oHeadPose.vUp ) ) * 180.0f / ITAConstants::PI_D;
171

Jonas Stienen's avatar
Jonas Stienen committed
172 173
	int iHRIRPreOffset = m_pHRIR->getMinEffectiveFilterOffset();
	int iHRIRFilerTaps = m_pHRIR->getMaxEffectiveFilterLength();
174 175
	int iOffset = ( std::max )( 0, int( fLS2HeadDelaySamples - iHRIRPreOffset ) ); // remove starting zeros from HRIR

Jonas Stienen's avatar
Jonas Stienen committed
176 177 178 179 180 181 182 183
	iOffset = int( fLS2HeadDelaySamples ) + iHRIRPreOffset;

	// Check against buffer overrun, prevent if necessary (PROBLEM: LS too far away for target filter)
	if( iOffset + iHRIRFilerTaps > sfDestHRIR.length() )
		return false;

	int iRecordIndex;
	m_pHRIR->getNearestNeighbour( DAFF_OBJECT_VIEW, float( dPhiDeg ), float( dThetaDeg ), iRecordIndex, bOutOfRange );
184 185 186
	m_pHRIR->addFilterCoeffs( iRecordIndex, 0, sfDestHRIR[ 0 ].data() + iOffset, fDistanceGain * float( dReflectionFactor ) );
	m_pHRIR->addFilterCoeffs( iRecordIndex, 1, sfDestHRIR[ 1 ].data() + iOffset, fDistanceGain * float( dReflectionFactor ) );

Jonas Stienen's avatar
Jonas Stienen committed
187 188 189 190 191 192 193 194
	return true;
}

const ITANCTC::Pose& ITANCTC::GetLoudspeakerPose( int iLoudspeakerID ) const
{
	if( iLoudspeakerID > GetN() )
		ITA_EXCEPT1( INVALID_PARAMETER, "Loudspeaker ID (starting from 1) out of range." );

195
	return m_oConfig.voLoudspeaker[ iLoudspeakerID - 1 ].oPose;
Jonas Stienen's avatar
Jonas Stienen committed
196 197 198 199 200 201
}

bool ITANCTC::CalculateFilter( std::vector< ITAHDFTSpectra* >& vpCTCFilter )
{
	if( !m_pHRIR )
		return false;
202

Jonas Stienen's avatar
Jonas Stienen committed
203
	bool bOutOfRange = false;
204
	for( int n = 0; n < GetN(); n++ )
Jonas Stienen's avatar
Jonas Stienen committed
205 206 207 208 209
	{
		m_sfCTC_temp.zero();

		switch( m_iOptimization )
		{
210
			// Here comes more if ready ... see feature/room_compensation branch
211
		case Config::OPTIMIZATION_NONE:
212 213 214 215
		default:
			if( AddHRIR( GetLoudspeakerPose( n + 1 ), m_sfCTC_temp, bOutOfRange ) == false )
				return false;
			break;
Jonas Stienen's avatar
Jonas Stienen committed
216 217 218 219 220
		}

		if( bOutOfRange )
			return false;

221
		ITAHDFTSpectra* pHRTF( m_vpHRTFs[ n ] );
Jonas Stienen's avatar
Jonas Stienen committed
222 223

		// Convert HRIRs to HRTFs
224 225
		m_fft.execute( m_sfCTC_temp[ 0 ].data(), ( *pHRTF )[ 0 ]->data() );
		m_fft.execute( m_sfCTC_temp[ 1 ].data(), ( *pHRTF )[ 1 ]->data() );
Jonas Stienen's avatar
Jonas Stienen committed
226

227
#ifdef NCTC_EXPORT_FILTER_TO_HARDDRIVE
228
		ITAFFTUtils::Export(pHRTF, "HRIR_LS" + IntToString(n + 1) + "_RAW");
229
#endif // NCTC_EXPORT_FILTER_TO_HARDDRIVE
Jonas Stienen's avatar
Jonas Stienen committed
230
	}
231

Jonas Stienen's avatar
Jonas Stienen committed
232 233 234 235
	/* Matrix to be inverted (as two-by-two row vector)
	 * a  c
	 * b  d
	 */
236 237 238 239
	ITAHDFTSpectrum* a = ( *m_vpHelper2x2[ 0 ] )[ 0 ];
	ITAHDFTSpectrum* b = ( *m_vpHelper2x2[ 0 ] )[ 1 ];
	ITAHDFTSpectrum* c = ( *m_vpHelper2x2[ 1 ] )[ 0 ];
	ITAHDFTSpectrum* d = ( *m_vpHelper2x2[ 1 ] )[ 1 ];
Jonas Stienen's avatar
Jonas Stienen committed
240 241

	// Least-squares minimization: C = WH*(HWH*-\beta)^-1
242 243
	// using H* as the hermitian (complex conjugated) transpose of H

244 245 246 247 248 249 250
	/* Sweet spot optimization: modify input HRTFs
	 *   1. Crosstalk side will be lowered by CTC factor
	 *   2. HRTF will be flattened by WICK factor
	 */ 


	// Prepare HRTF spectra
Jonas Stienen's avatar
Jonas Stienen committed
251

252 253
	int iDFTSize = m_oConfig.iCTCFilterLength + 1;
	for( int n = 0; n < GetN(); n++ )
Jonas Stienen's avatar
Jonas Stienen committed
254
	{
255 256
		ITAHDFTSpectra* pHRTF( m_vpHRTFs[ n ] ); // two-channel

257

258 259 260 261 262
		//  --- WICK factor ---
		
		// First, store original energy of HRTF (left channel)
		float fEnergy = ( *pHRTF )[ 0 ]->getEnergy();
		
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
263
		// Apply WICK factor only on magnitudes (left channel)
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
264
		for( int i = 0; i < ( *pHRTF )[ 0 ]->getSize(); i++ )
265
		{
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
266
			float fMag = ( *pHRTF )[ 0 ]->calcMagnitude( i );
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
267
			( *pHRTF )[ 0 ]->setMagnitudePreservePhase( i, std::pow( fMag, m_fWaveIncidenceAngleCompensationFactor ) );
268
		}
269

270
		// Compensate initial HRTF energy when WICK is used (left channel)
271
		assert( fEnergy > 0 );
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
272
		float fEnergyCompensation = std::pow( fEnergy, ( 1 - m_fWaveIncidenceAngleCompensationFactor ) );
273 274 275
		( *pHRTF )[ 0 ]->mul( fEnergyCompensation );


276
		// First, store original energy of HRTF only on magnitudes (right channel)
277
		fEnergy = ( *pHRTF )[ 1 ]->getEnergy();
278

Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
279
		// Apply WICK factor only on magnitude (right channel)
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
280
		for( int i = 0; i < ( *pHRTF )[ 1 ]->getSize(); i++ )
281
		{
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
282
			float fMag = ( *pHRTF )[ 1 ]->calcMagnitude( i );
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
283
			( *pHRTF )[ 1 ]->setMagnitudePreservePhase( i, std::pow( fMag, m_fWaveIncidenceAngleCompensationFactor ) );
284
		}
285 286

		// Compensate initial HRTF energy when WICK is used (right channel)
287
		assert( fEnergy > 0 );
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
288
		fEnergyCompensation = std::pow( fEnergy, ( 1 - m_fWaveIncidenceAngleCompensationFactor ) );
289
		( *pHRTF )[ 1 ]->mul( fEnergyCompensation );
290 291 292


#ifdef NCTC_EXPORT_FILTER_TO_HARDDRIVE
293
		ITAFFTUtils::Export(pHRTF, "HRIR_LS" + IntToString(n + 1) + "_WICKed");
294
#endif // NCTC_EXPORT_FILTER_TO_HARDDRIVE
295

Michael Kohnen's avatar
Michael Kohnen committed
296
		//  --- CTC compensation factor ---
297
		// CTC compensation factors
Michael Kohnen's avatar
Michael Kohnen committed
298
		int iLSSide = GetLoudspeakerSide(n + 1);
299 300

		float fRightChannelCTC = 1.0f;
Michael Kohnen's avatar
Michael Kohnen committed
301
		if (iLSSide == Config::Loudspeaker::LEFT_SIDE)
302 303 304
			fRightChannelCTC *= m_fCrossTalkCancellationFactor; // only apply to left channel if LS is at left side

		float fLeftChannelCTC = 1.0f;
Michael Kohnen's avatar
Michael Kohnen committed
305
		if (iLSSide == Config::Loudspeaker::RIGHT_SIDE)
306
			fLeftChannelCTC *= m_fCrossTalkCancellationFactor; // only apply to right channel if LS is at right side
Michael Kohnen's avatar
Michael Kohnen committed
307 308 309 310 311
		
		(*pHRTF)[0]->mul(fLeftChannelCTC);
		(*pHRTF)[1]->mul(fRightChannelCTC);
		
#ifdef NCTC_EXPORT_FILTER_TO_HARDDRIVE
312
		ITAFFTUtils::Export(pHRTF, "HRIR_LS" + IntToString(n + 1) + "_WICKedPlusCTCFactor");
Michael Kohnen's avatar
Michael Kohnen committed
313 314 315 316 317
#endif // NCTC_EXPORT_FILTER_TO_HARDDRIVE
		
		//  --- Weighting ---
		float fWeight = float( m_vdWeights[ n ] ); // diag element
				
Jonas Stienen's avatar
Jonas Stienen committed
318
		// Element wise (a and d): HWH* -> 2x2
319
		t->copy( ( *pHRTF )[ 0 ] );
Michael Kohnen's avatar
Michael Kohnen committed
320
		t->mul( fWeight );
321 322

		t->mul_conj( ( *pHRTF )[ 0 ] );
Jonas Stienen's avatar
Jonas Stienen committed
323 324
		n == 0 ? a->copy( t ) : a->add( t );

325
		t->copy( ( *pHRTF )[ 1 ] );
Michael Kohnen's avatar
Michael Kohnen committed
326
		t->mul( fWeight );
327 328

		t->mul_conj( ( *pHRTF )[ 1 ] );
Jonas Stienen's avatar
Jonas Stienen committed
329
		n == 0 ? d->copy( t ) : d->add( t );
330

Jonas Stienen's avatar
Jonas Stienen committed
331
		// Cross elements (b and c): HWH*
332
		t->copy( ( *pHRTF )[ 1 ] );
Michael Kohnen's avatar
Michael Kohnen committed
333
		t->mul( fWeight );
334

335
		t->mul_conj( ( *pHRTF )[ 0 ] );
Jonas Stienen's avatar
Jonas Stienen committed
336 337
		n == 0 ? b->copy( t ) : b->add( t );

338
		t->copy( ( *pHRTF )[ 0 ] );
Michael Kohnen's avatar
Michael Kohnen committed
339
		t->mul( fWeight );
340 341

		t->mul_conj( ( *pHRTF )[ 1 ] );
Jonas Stienen's avatar
Jonas Stienen committed
342 343 344
		n == 0 ? c->copy( t ) : c->add( t );
	}

345

Jonas Stienen's avatar
Jonas Stienen committed
346
	// Regularize
347

Jonas Stienen's avatar
Jonas Stienen committed
348 349 350 351
	a->add( m_fBeta );
	d->add( m_fBeta );

	ITAHDFTSpectra abcd( m_oConfig.dSampleRate, 4, m_oConfig.iCTCFilterLength );
352 353 354 355
	abcd[ 0 ]->copyFrom( *a );
	abcd[ 1 ]->copyFrom( *b );
	abcd[ 2 ]->copyFrom( *c );
	abcd[ 3 ]->copyFrom( *d );
Jonas Stienen's avatar
Jonas Stienen committed
356

357 358

	// Invert via determinant (simple 2x2 matrix inversion)
Jonas Stienen's avatar
Jonas Stienen committed
359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380
	/*
	 * d/det  -b/det
	 * -c/det  a/det
	 */
	det->copy( a );
	det->mul( d );
	t->copy( b );
	t->mul( c );
	det->sub( t );

	t->copy( d );
	d->copy( a );
	a->copy( t );
	b->mul( -1.0f );
	c->mul( -1.0f );
	a->div( det );
	b->div( det );
	c->div( det );
	d->div( det );


	// Calculate CTC filter WH*Inv (Nx2)
381

Jonas Stienen's avatar
Jonas Stienen committed
382 383
	ITASampleFrame sfTargetData_shift( m_sfCTC_temp.channels(), m_sfCTC_temp.length(), true );

384
	for( int n = 0; n < GetN(); n++ )
Jonas Stienen's avatar
Jonas Stienen committed
385
	{
386 387 388
		ITAHDFTSpectra* pHRTF( m_vpHRTFs[ n ] ); // two-channel, already WICKed
		ITAHDFTSpectra* pCTCFilter( vpCTCFilter[ n ] ); // two-channel
		float fWeight = float( m_vdWeights[ n ] ); // diag element
Jonas Stienen's avatar
Jonas Stienen committed
389 390

		t->copy( a );
391
		t->mul_conj( ( *pHRTF )[ 0 ] );
Michael Kohnen's avatar
Michael Kohnen committed
392
		t->mul( fWeight );
393
		( *pCTCFilter )[ 0 ]->copy( t );
394

Jonas Stienen's avatar
Jonas Stienen committed
395
		t->copy( b );
396
		t->mul_conj( ( *pHRTF )[ 1 ] );
Michael Kohnen's avatar
Michael Kohnen committed
397
		t->mul( fWeight );
398
		( *pCTCFilter )[ 0 ]->add( t );
Jonas Stienen's avatar
Jonas Stienen committed
399 400

		t->copy( c );
401
		t->mul_conj( ( *pHRTF )[ 0 ] );
Michael Kohnen's avatar
Michael Kohnen committed
402
		t->mul( fWeight );
403
		( *pCTCFilter )[ 1 ]->copy( t );
Jonas Stienen's avatar
Jonas Stienen committed
404
		t->copy( d );
405
		t->mul_conj( ( *pHRTF )[ 1 ] );
Michael Kohnen's avatar
Michael Kohnen committed
406
		t->mul( fWeight );
407
		( *pCTCFilter )[ 1 ]->add( t );
Jonas Stienen's avatar
Jonas Stienen committed
408

409
#ifdef NCTC_EXPORT_FILTER_TO_HARDDRIVE
410
		ITAFFTUtils::Export(pCTCFilter, "CTCFilter_noshift_" + IntToString(n + 1));
411 412
#endif // NCTC_EXPORT_FILTER_TO_HARDDRIVE 

Jonas Stienen's avatar
Jonas Stienen committed
413 414

		// Time-shift
415 416
		m_ifft.execute( ( *pCTCFilter )[ 0 ]->data(), m_sfCTC_temp[ 0 ].data() );
		m_ifft.execute( ( *pCTCFilter )[ 1 ]->data(), m_sfCTC_temp[ 1 ].data() );
Jonas Stienen's avatar
Jonas Stienen committed
417 418 419 420 421

		// Normalize after IFFT
		m_sfCTC_temp.div_scalar( float( m_sfCTC_temp.length() ) );

		// Shift the CTC filter according to desired delay
422 423
		int iShiftSamples = int( m_oConfig.dSampleRate * m_vfDelayTime[ n ] );
		if( m_vfDelayTime[ n ] < 0.0f )
Jonas Stienen's avatar
Jonas Stienen committed
424 425
			iShiftSamples = m_sfCTC_temp.length() / 2; // if invalid, shift by half length
		sfTargetData_shift.cyclic_write( m_sfCTC_temp, m_sfCTC_temp.length(), 0, iShiftSamples );
426

427 428
		m_fft.execute( sfTargetData_shift[ 0 ].data(), ( *pCTCFilter )[ 0 ]->data() );
		m_fft.execute( sfTargetData_shift[ 1 ].data(), ( *pCTCFilter )[ 1 ]->data() );
Jonas Stienen's avatar
Jonas Stienen committed
429

430
#ifdef NCTC_EXPORT_FILTER_TO_HARDDRIVE
431
		ITAFFTUtils::Export(pCTCFilter, "CTCFilter_shift_" + IntToString(n + 1));
432
#endif // NCTC_EXPORT_FILTER_TO_HARDDRIVE
Jonas Stienen's avatar
Jonas Stienen committed
433 434 435 436 437 438 439 440 441 442 443 444 445 446 447
	}

	return true;
}

void ITANCTC::SetHRIR( const DAFFContentIR* pHRIR )
{
	// TODO: Set m_oConfig.iCTCFilterLength according to HRIR length and working area
	// TODO: Minimize loudspeaker delay

	if( m_pHRIR != pHRIR )
	{
		m_pHRIR = pHRIR;

		// Return if HRIR is reset (NULL)
448
		if( !m_pHRIR )
Jonas Stienen's avatar
Jonas Stienen committed
449
			return;
450 451 452 453

		if( m_pHRIR->getProperties()->getNumberOfChannels() != 2 )
			ITA_EXCEPT1( INVALID_PARAMETER, "HRIR dataset must have exactly two channels" );

Jonas Stienen's avatar
Jonas Stienen committed
454 455
		// Get HRIR filter length
		if( m_pHRIR->getFilterLength() > m_oConfig.iCTCFilterLength )
456 457 458
			ITA_EXCEPT1( INVALID_PARAMETER,
			std::string( "The filter length of the HRIR database \"" ) + m_pHRIR->getParent()->getFilename() +
			std::string( "\" is larger than the CTC filters. This leads to buffer overruns." ) );
Jonas Stienen's avatar
Jonas Stienen committed
459 460 461 462

		// Get HRIR delay samples
		const DAFFMetadata* pHRIRMetadata = m_pHRIR->getParent()->getMetadata();

463 464 465 466 467 468 469 470 471 472 473
		if( !pHRIRMetadata->hasKey( "DELAY_SAMPLES" ) )
			ITA_EXCEPT1( INVALID_PARAMETER,
			std::string( "HRIR database \"" ) + m_pHRIR->getParent()->getFilename() +
			std::string( "\" is missing a \"DELAY_SAMPLES\" metadata tag" ) );
		if( ( pHRIRMetadata->getKeyType( "DELAY_SAMPLES" ) != DAFFMetadata::DAFF_INT ) &&
			( pHRIRMetadata->getKeyType( "DELAY_SAMPLES" ) != DAFFMetadata::DAFF_FLOAT ) )
			ITA_EXCEPT1( INVALID_PARAMETER,
			std::string( "The metadata tag \"DELAY_SAMPLES\" in HRIR database \"" ) + m_pHRIR->getParent()->getFilename() +
			std::string( "\" is not numeric" ) );

		float fHRIRDelay = ( float ) pHRIRMetadata->getKeyFloat( "DELAY_SAMPLES" );
Jonas Stienen's avatar
Jonas Stienen committed
474
		if( fHRIRDelay < 0 )
475 476 477
			ITA_EXCEPT1( INVALID_PARAMETER,
			std::string( "The metadata tag \"DELAY_SAMPLES\" in HRIR database \"" ) + m_pHRIR->getParent()->getFilename() +
			std::string( "\" must not be negative" ) );
Jonas Stienen's avatar
Jonas Stienen committed
478 479 480 481 482 483 484 485 486


	}
}

void ITANCTC::SetBeta( float fBeta )
{
	m_fBeta = fBeta;
}
487

488 489 490 491 492
float ITANCTC::GetBeta()
{
	return m_fBeta;
}

493
void ITANCTC::SetCrossTalkCancellationFactor( float fFactor )
494
{
495
	m_fCrossTalkCancellationFactor = fFactor;
496 497
}

498
float ITANCTC::GetCrossTalkCancellationFactor()
499
{
500 501 502 503 504 505 506 507 508 509 510
	return m_fCrossTalkCancellationFactor;
}

void ITANCTC::SetWaveIncidenceAngleCompensationFactor( float fFactor )
{
	m_fWaveIncidenceAngleCompensationFactor = fFactor;
}

float ITANCTC::GetWaveIncidenceAngleCompensation()
{
	return m_fWaveIncidenceAngleCompensationFactor;
511 512
}

513
void ITANCTC::SetDelayTime( float fDelayTime )
Jonas Stienen's avatar
Jonas Stienen committed
514
{
515 516 517 518 519 520 521 522 523 524 525
	for( int n = 0; n < GetN(); n++ )
		m_vfDelayTime[ n ] = fDelayTime;
}

void ITANCTC::SetDelayTime( std::vector< float > vfDelayTime )
{
	if( int( vfDelayTime.size() ) != GetN() )
		ITA_EXCEPT1( INVALID_PARAMETER, "Provide as many delay values as channels for NCTC" );

	for( int n = 0; n < GetN(); n++ )
		m_vfDelayTime[ n ] = vfDelayTime[ n ];
Jonas Stienen's avatar
Jonas Stienen committed
526 527
}

528
std::vector< float > ITANCTC::GetDelayTime()
Jonas Stienen's avatar
Jonas Stienen committed
529
{
530 531 532 533
	std::vector< float > vfDelayTime( GetN() );
	for( int n = 0; n < GetN(); n++ )
		vfDelayTime[ n ] = m_vfDelayTime[ n ];
	return vfDelayTime;
Jonas Stienen's avatar
Jonas Stienen committed
534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559
}

void ITANCTC::SetOptimization( int iOptimization )
{
	m_iOptimization = iOptimization;
}

int ITANCTC::GetOptimization() const
{
	return  m_iOptimization;
}

bool ITANCTC::GetHRTF( std::vector< ITAHDFTSpectra* >& vpHRTF ) const
{
	if( m_vpHRTFs.empty() )
		return false;

	vpHRTF = m_vpHRTFs;
	return true;
}

ITANCTC::Pose ITANCTC::GetHeadPose() const
{
	return m_oHeadPose;
}

560

Jonas Stienen's avatar
Jonas Stienen committed
561 562 563
// --- Loudspeaker ---

ITANCTC::Config::Loudspeaker::Loudspeaker()
564 565
	: pDirectivity( NULL )
	, iSide( SIDE_NOT_SPECIFIED )
Jonas Stienen's avatar
Jonas Stienen committed
566 567 568 569 570
{
}

ITANCTC::Config::Loudspeaker::Loudspeaker( const Pose& oStaticPose )
	: pDirectivity( NULL )
571
	, iSide( SIDE_NOT_SPECIFIED )
Jonas Stienen's avatar
Jonas Stienen committed
572 573 574 575 576
{
	oPose = oStaticPose;
}


577
// --- Pose ---
Jonas Stienen's avatar
Jonas Stienen committed
578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594

ITANCTC::Pose& ITANCTC::Pose::operator=( const ITANCTC::Pose& oPoseRHS )
{
	vPos = oPoseRHS.vPos;
	//qOrient = oPoseRHS.qOrient;
	vView = oPoseRHS.vView;
	vUp = oPoseRHS.vUp;

	return *this;
}

void ITANCTC::Pose::SetOrientationYPRdeg( double fYaw, double fPitch, double fRoll )
{
	double yaw = grad2rad( fYaw );
	double pitch = grad2rad( fPitch );
	double roll = grad2rad( fRoll );

595 596 597
	double sy = sin( yaw ), cy = cos( yaw );
	double sp = sin( pitch ), cp = cos( pitch );
	double sr = sin( roll ), cr = cos( roll );
Jonas Stienen's avatar
Jonas Stienen committed
598 599 600 601 602 603

	vView.SetValues( -sy*cp, sp, -cy*cp );
	vUp.SetValues( cy*sr + sy*sp*cr, cp*cr, -sy*sr + cy*sp*cr );

	return;
}
604 605 606 607 608 609 610 611 612 613 614

ITANCTC::Config::Config()
{
	// Set some default values
	N = 0;
	iCTCFilterLength = 4096;
	fSpeedOfSound = 344.0f;
	iOptimization = OPTIMIZATION_NONE;
	fCrossTalkCancellationFactor = 1.0f;
	fWaveIncidenceAngleCompensationFactor = 1.0f;
}