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 15 16 17 18 19 20 21 22
// $Id: ITANCTC.cpp 2395 2012-04-20 06:58:52Z stienen $

#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>

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

24 25 26
	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
27
	m_iOptimization = m_oConfig.iOptimization;
28 29
	m_fCrossTalkCancellationFactor = m_oConfig.fCrossTalkCancellationFactor;
	m_fWaveIncidenceAngleCompensationFactor = m_oConfig.fWaveIncidenceAngleCompensationFactor;
Jonas Stienen's avatar
Jonas Stienen committed
30 31 32

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

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

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

46
	for( int i = 0; i < 2; i++ )
Jonas Stienen's avatar
Jonas Stienen committed
47 48 49 50 51
		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 );

52 53 54
	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
55 56 57 58 59

}

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

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

	delete t, det;
67

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

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

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

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

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

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

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

	// @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();
118 119 120 121 122 123
	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 )
124 125 126 127 128 129 130
	{
		return Config::Loudspeaker::LEFT_SIDE;
	}
	else
	{
		return Config::Loudspeaker::RIGHT_SIDE;
	}
131

132 133
}

Jonas Stienen's avatar
Jonas Stienen committed
134 135 136 137 138 139 140 141
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() );
142 143
	float fLS2HeadDelaySamples = ( float ) dDistanceMeters / m_oConfig.fSpeedOfSound * ( float ) m_oConfig.dSampleRate;

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

	// @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();
167
	VistaVector3D vViewMinusZ = m_oHeadPose.vView * ( -1.0f ); // local z axis
Jonas Stienen's avatar
Jonas Stienen committed
168 169 170 171
	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;
172

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

Jonas Stienen's avatar
Jonas Stienen committed
177 178 179 180 181 182 183 184
	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 );
185 186 187
	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
188 189 190 191 192 193 194 195
	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." );

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

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

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

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

		if( bOutOfRange )
			return false;

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

		// Convert HRIRs to HRTFs
225 226
		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
227

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

Jonas Stienen's avatar
Jonas Stienen committed
233 234 235 236
	/* Matrix to be inverted (as two-by-two row vector)
	 * a  c
	 * b  d
	 */
237 238 239 240
	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
241 242

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

245 246 247 248 249 250 251
	/* 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
252

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

258

259 260 261 262 263
		//  --- 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
264
		// Apply WICK factor only on magnitudes (left channel)
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
265
		for( int i = 0; i < ( *pHRTF )[ 0 ]->getSize(); i++ )
266
		{
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
267 268
			float fMag = ( *pHRTF )[ 0 ]->calcMagnitude( i );
			( *pHRTF )[ 0 ]->setMagnitudePreservePhase( i, std::powf( fMag, m_fWaveIncidenceAngleCompensationFactor ) );
269
		}
270

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


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

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

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


#ifdef NCTC_EXPORT_FILTER_TO_HARDDRIVE
		pHRTF->Export( "HRIR_LS" + IntToString( n + 1 ) + "_WICKed" );
#endif // NCTC_EXPORT_FILTER_TO_HARDDRIVE
296

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

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

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

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

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

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

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

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

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

346

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

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

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

358 359

	// Invert via determinant (simple 2x2 matrix inversion)
Jonas Stienen's avatar
Jonas Stienen committed
360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381
	/*
	 * 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)
382

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

385
	for( int n = 0; n < GetN(); n++ )
Jonas Stienen's avatar
Jonas Stienen committed
386
	{
387 388 389
		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
390 391

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

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

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

410 411 412 413
#ifdef NCTC_EXPORT_FILTER_TO_HARDDRIVE
		pCTCFilter->Export( "CTCFilter_noshift_" + IntToString( n+1 ) );
#endif // NCTC_EXPORT_FILTER_TO_HARDDRIVE 

Jonas Stienen's avatar
Jonas Stienen committed
414 415

		// Time-shift
416 417
		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
418 419 420 421 422

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

		// Shift the CTC filter according to desired delay
423 424
		int iShiftSamples = int( m_oConfig.dSampleRate * m_vfDelayTime[ n ] );
		if( m_vfDelayTime[ n ] < 0.0f )
Jonas Stienen's avatar
Jonas Stienen committed
425 426
			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 );
427

428 429
		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
430

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

	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)
449
		if( !m_pHRIR )
Jonas Stienen's avatar
Jonas Stienen committed
450
			return;
451 452 453 454

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

Jonas Stienen's avatar
Jonas Stienen committed
455 456
		// Get HRIR filter length
		if( m_pHRIR->getFilterLength() > m_oConfig.iCTCFilterLength )
457 458 459
			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
460 461 462 463

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

464 465 466 467 468 469 470 471 472 473 474
		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
475
		if( fHRIRDelay < 0 )
476 477 478
			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
479 480 481 482 483 484 485 486 487


	}
}

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

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

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

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

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

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

514
void ITANCTC::SetDelayTime( float fDelayTime )
Jonas Stienen's avatar
Jonas Stienen committed
515
{
516 517 518 519 520 521 522 523 524 525 526
	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
527 528
}

529
std::vector< float > ITANCTC::GetDelayTime()
Jonas Stienen's avatar
Jonas Stienen committed
530
{
531 532 533 534
	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
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 560
}

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;
}

561

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

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

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


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

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 );

596 597 598
	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
599 600 601 602 603 604

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

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

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