VAAmbisonicsBinauralMixdownReproduction.cpp 14.6 KB
Newer Older
Jonas Stienen's avatar
Jonas Stienen committed
1
2
#include "VAAmbisonicsBinauralMixdownReproduction.h"

3
#if (VACORE_WITH_REPRODUCTION_AMBISONICS_BINAURAL_MIXDOWN==1 )
Jonas Stienen's avatar
Jonas Stienen committed
4

5
6
7
8
9
10
#include "../../Data/VAHRIRDataset.h"
#include "../../Scene/VAListenerState.h"
#include "../../Scene/VAMotionState.h"
#include "../../Scene/VASceneState.h"
#include "../../Utils/VAUtils.h"
#include "../../VAHardwareSetup.h"
Jonas Stienen's avatar
Jonas Stienen committed
11

12
13
14
15
#include <ITAUPConvolution.h>
#include <ITAUPFilterPool.h>
#include <ITAUPFilter.h>
#include <ITAUPTrigger.h>
16
#include <ITADataSourceRealization.h>
Jonas Stienen's avatar
Jonas Stienen committed
17
18
19
20
21
22
23
24
#include <ITANumericUtils.h>
#include <ITAStreamPatchbay.h>

#include <VistaBase/VistaQuaternion.h>
#include <VistaBase/VistaTransformMatrix.h>
#include <VistaBase/VistaVector3D.h>

#include <xutility>
25
#include <ITAFastMath.h>
Jonas Stienen's avatar
Jonas Stienen committed
26
27
28
29
30

class CMixdownStreamFilter : public ITADatasourceRealization
{
public:
	ITADatasource* pdsInput;
31
32
	ITAUPFilterPool* pFilterPool;
	ITAUPTrigger* pTrigger;
Jonas Stienen's avatar
Jonas Stienen committed
33

34
	std::vector< ITAUPConvolution* > vpConvolver;
Jonas Stienen's avatar
Jonas Stienen committed
35
36
37
38

	CMixdownStreamFilter( int iNumLoudspeaker, double dSampleRate, int iBlockLength, int iMaxFilterLength );
	~CMixdownStreamFilter();
	void SetIdentity();
39
	void SetFilter( const int iIndex, ITAUPFilter* pFilterChL, ITAUPFilter* pFilterChR );
Jonas Stienen's avatar
Jonas Stienen committed
40
41
42
43
44
45
46
47
48
49
50
	void ProcessStream( const ITAStreamInfo* pStreamInfo );
	void PostIncrementBlockPointer();
};

CVAAmbisonicsBinauralMixdownReproduction::CVAAmbisonicsBinauralMixdownReproduction( const CVAAudioReproductionModuleInitParams& oParams )
: m_oParams( oParams )
, m_pdsStreamFilter( NULL )
, m_iListenerID( -1 )
{
	CVAConfigInterpreter conf( *(m_oParams.pConfig) );
	
51
	conf.ReqInteger( "TruncationOrder", m_iAmbisonicsTruncationOrder ); // Sollte aus der Zahl der Eingangskanle berechnet werden
Jonas Stienen's avatar
Jonas Stienen committed
52
53
54
	conf.OptInteger( "HRIRFilterLength", m_iHRIRFilterLength, 128 );	
	conf.OptInteger( "TrackedListenerID", m_iListenerID, 1 );

55
	double dSampleRate = m_oParams.pCore->GetCoreConfig()->oAudioDriverConfig.dSampleRate;
Jonas Stienen's avatar
Jonas Stienen committed
56
57
	int iBlockLength = oParams.pCore->GetCoreConfig()->oAudioDriverConfig.iBuffersize;

58
	m_sfHRIRTemp.init( 2, m_iHRIRFilterLength, true );
Jonas Stienen's avatar
Jonas Stienen committed
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85

	// Binaural output
	for( size_t i=0; i<m_oParams.vpOutputs.size(); i++ )
	{
		const CVAHardwareOutput* pTargetOutput = m_oParams.vpOutputs[i];
		if( pTargetOutput == nullptr )
			VA_EXCEPT2( INVALID_PARAMETER, "Unrecognized output '"+pTargetOutput->sIdentifier+"' for binaural mixdown reproduction" );
		if( pTargetOutput->GetPhysicalOutputChannels().size() != 2 )
			VA_EXCEPT2( INVALID_PARAMETER, "Expecting two channels for binaural downmix target, problem with output '"+pTargetOutput->sIdentifier+"' detected" );
		m_vpTargetOutputs.push_back( pTargetOutput );
	}

	// Virtual loudspeaker output
	std::string sVirtualOutput;
	conf.ReqString( "VirtualOutput", sVirtualOutput );
	m_pVirtualOutput = m_oParams.pCore->GetCoreConfig()->oHardwareSetup.GetOutput( sVirtualOutput );
	if( m_pVirtualOutput == nullptr )
		VA_EXCEPT2( INVALID_PARAMETER, "Unrecognized virtual output '"+sVirtualOutput+"' for binaural mixdown reproduction" );

	// Decoder input (B-Format with implicit channels)
	m_pDecoderMatrixPatchBay = new ITAStreamPatchbay( dSampleRate, iBlockLength );
	m_pDecoderMatrixPatchBay->AddInput( GetNumInputChannels() );

	// Decoder output (N-channel loudspeaker)
	m_pDecoderMatrixPatchBay->AddOutput( GetNumVirtualLoudspeaker() );
	m_pdsStreamFilter = new CMixdownStreamFilter( GetNumVirtualLoudspeaker(), dSampleRate, iBlockLength, m_iHRIRFilterLength );
	m_pdsStreamFilter->pdsInput = m_pDecoderMatrixPatchBay->GetOutputDatasource( 0 );
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
	
	double dYInv[9][25] =
	{
		{ 0.141479, 0.141920, 0.141646, 0.141830, 0.142275, 0.141479, 0.141526, 0.141477, 0.141646, 0.141477, 0.141994, 0.142275, 0.142275, 0.141994, 0.141526, 0.141646, 0.141830, 0.141526, 0.141994, 0.141479, 0.141477, 0.141920, 0.142466, 0.141830, 0.141920 },
		{ 0.005941, 0.000089, 0.086866, -0.289171, -0.218760, 0.132279, -0.048748, 0.071201, -0.185809, -0.181762, 0.034672, -0.097372, -0.227303, 0.268540, -0.043764, -0.167073, -0.026226, 0.231909, 0.143544, -0.214299, 0.160602, 0.253240, 0.208520, -0.056470, 0.159354 },
		{ 0.290269, -0.120293, 0.057773, 0.017307, 0.127976, -0.149033, -0.246478, -0.271526, 0.204262, 0.037630, 0.059254, 0.035368, -0.077298, 0.084189, 0.205663, -0.219915, 0.224294, 0.018744, -0.214181, -0.129190, 0.225973, -0.132183, -0.033016, -0.182721, 0.187134 },
		{ -0.010309, 0.259113, -0.259466, 0.048463, -0.121716, -0.211398, 0.104138, -0.083419, 0.044248, 0.226498, 0.275812, -0.261374, -0.146311, 0.039797, 0.172511, -0.043928, -0.187809, -0.140852, 0.119610, 0.147593, -0.094330, -0.002682, 0.203135, -0.222917, 0.145594 },
		{ 0.003282, -0.006696, -0.269060, -0.131720, 0.296452, -0.310102, -0.063386, -0.049882, -0.103034, -0.428107, 0.094691, 0.284039, 0.372584, 0.114170, -0.092901, 0.073076, 0.056890, -0.381878, 0.184781, -0.325510, -0.157803, -0.006838, 0.444594, 0.142130, 0.260228 },
		{ 0.006189, -0.003088, 0.059091, -0.061678, -0.317678, -0.218363, 0.141425, -0.209619, -0.439036, -0.085531, 0.030327, -0.038565, 0.201335, 0.253543, -0.105321, 0.428546, -0.068044, 0.051366, -0.347849, 0.314950, 0.398489, -0.373260, -0.072262, 0.118783, 0.336251 },
		{ 0.519905, -0.128956, -0.222463, -0.253086, -0.095566, -0.071242, 0.363030, 0.423903, 0.166847, -0.253144, -0.231166, -0.242859, -0.197860, -0.179531, 0.179474, 0.223870, 0.180918, -0.239684, 0.189199, -0.092858, 0.186994, -0.092435, -0.250168, 0.034298, 0.082579 },
		{ -0.009086, -0.352444, -0.165782, 0.007717, -0.177053, 0.316863, -0.292659, 0.226190, 0.099659, 0.096699, 0.185697, -0.105388, 0.131534, 0.030299, 0.408122, 0.113469, -0.438948, -0.030252, -0.278323, -0.207655, -0.222219, 0.016649, -0.070396, 0.420575, 0.296734 },
		{ -0.012261, 0.367459, 0.351366, -0.428608, -0.189546, 0.145810, 0.047338, -0.002382, -0.187556, 0.115177, 0.413844, 0.336094, -0.171487, -0.398253, 0.158751, -0.155985, 0.188937, -0.192006, -0.025891, -0.117003, -0.096159, -0.357402, -0.011634, 0.237909, -0.016513 },
	};
			
			
			//if (m_iAmbisonicsTruncationOrder == 3)
			//{
							/*	{	double dYInv[16][12] =
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
		{
			{ 0.1935, 0.1935, 0.1935, 0.1935, 0.2222, 0.2222, 0.2222, 0.2222, 0.2386, 0.2386, 0.2386, 0.2386 },
			{ 0.5226, -0.5226, -0.5226, 0.5226, -0.0000, -0.6052, 0.0000, 0.6052, -0.0000, -0.6203, 0.0000, 0.6203 },
			{ -0.0083, -0.0083, -0.0083, -0.0083, 0.6265, 0.6265, 0.6265, 0.6265, -0.5964, -0.5964, -0.5964, -0.5964 },
			{ 0.5226, 0.5226, -0.5226, -0.5226, 0.6052, -0.0000, -0.6052, -0.0000, 0.6203, 0.0000, -0.6203, -0.0000 },
			{ 1.3985, -1.3985, 1.3985, -1.3985, 0.0000, -0.0000, -0.0000, 0.0000, -0.0000, -0.0000, -0.0000, -0.0000 },
			{ 0.0072, -0.0072, -0.0072, 0.0072, -0.0000, -2.0462, 0.0000, 2.0462, 0.0000, 2.0034, 0.0000, -2.0034 },
			{ -0.4886, -0.4886, -0.4886, -0.4886, -0.1783, -0.1783, -0.1783, -0.1783, -0.1914, -0.1914, -0.1914, -0.1914 },
			{ 0.0072, 0.0072, -0.0072, -0.0072, 2.0462, 0.0000, -2.0462, 0.0000, -2.0034, 0.0000, 2.0034, -0.0000 },
			{ -0.0000, 0.0000, 0.0000, -0.0000, 0.7820, -0.7820, 0.7820, -0.7820, 0.8396, -0.8396, 0.8396, -0.8396 },
			{ 1.5412, -1.5412, -1.5412, 1.5412, -0.0000, 1.1505, 0.0000, -1.1505, -0.0000, 1.3099, 0.0000, -1.3099 },
			{ 0.0000, 0.0000, 0.0000, -0.0000, -0.0000, 0.0000, -0.0000, 0.0000, -0.0000, -0.0000, 0.0000, 0.0000 },
			{ -0.5516, 0.5516, 0.5516, -0.5516, 0.0000, -0.3022, 0.0000, 0.3022, -0.0000, -0.1409, -0.0000, 0.1409 },
			{ 0.0004, 0.0004, 0.0004, 0.0004, -1.1248, -1.1248, -1.1248, -1.1248, 1.0876, 1.0876, 1.0876, 1.0876 },
			{ -0.5516, -0.5516, 0.5516, 0.5516, 0.3022, 0.0000, -0.3022, 0.0000, 0.1409, -0.0000, -0.1409, 0.0000 },
			{ -0.0000, 0.0000, 0.0000, 0.0000, 1.4519, -1.4519, 1.4519, -1.4519, -1.4043, 1.4043, -1.4043, 1.4043 },
			{ -1.5412, -1.5412, 1.5412, 1.5412, 1.1505, 0.0000, -1.1505, -0.0000, 1.3099, -0.0000, -1.3099, 0.0000 },
		};
122
	}
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
	else if (m_iAmbisonicsTruncationOrder == 2)
	{
		double dYInv[9][12] =
		{
			{ 0.1935, 0.1935, 0.1935, 0.1935, 0.2287, 0.2287, 0.2287, 0.2287, 0.2323, 0.2323, 0.2323, 0.2323 },
			{ 0.5348, -0.5348, -0.5348, 0.5348, 0.0000, -0.6703, -0.0000, 0.6703, -0.0000, -0.7317, 0.0000, 0.7317 },
			{ -0.0087, -0.0087, -0.0087, -0.0087, 0.5053, 0.5053, 0.5053, 0.5053, -0.4788, -0.4788, -0.4788, -0.4788 },
			{ 0.5348, 0.5348, -0.5348, -0.5348, 0.6703, 0.0000, -0.6703, -0.0000, 0.7317, -0.0000, -0.7317, -0.0000 },
			{ 1.6712, -1.6712, 1.6712, -1.6712, -0.0000, 0.0000, -0.0000, 0.0000, 0.0000, -0.0000, -0.0000, -0.0000 },
			{ -0.0414, 0.0414, 0.0414, -0.0414, 0.0000, -2.4757, 0.0000, 2.4757, 0.0000, 2.3722, 0.0000, -2.3722 },
			{ -0.5839, -0.5839, -0.5839, -0.5839, -0.1921, -0.1921, -0.1921, -0.1921, -0.2490, -0.2490, -0.2490, -0.2490 },
			{ -0.0414, -0.0414, 0.0414, 0.0414, 2.4757, -0.0000, -2.4757, 0.0000, -2.3722, -0.0000, 2.3722, -0.0000 },
			{ 0.0000, -0.0000, 0.0000, -0.0000, 0.9531, -0.9531, 0.9531, -0.9531, 0.9854, -0.9854, 0.9854, -0.9854 },
		};
	}
	else if (m_iAmbisonicsTruncationOrder == 1)
Jonas Stienen's avatar
Jonas Stienen committed
139
	{
140
141
142
143
144
145
146
		double dYInv[4][12] =
		{
			{ 0.2533, 0.2533, 0.2533, 0.2533, 0.2484, 0.2484, 0.2484, 0.2484, 0.2578, 0.2578, 0.2578, 0.2578 },
			{ 0.5940, -0.5940, -0.5940, 0.5940, -0.0000, -0.7734, 0.0000, 0.7734, -0.0000, -0.7864, 0.0000, 0.7864 },
			{ -0.0163, -0.0163, -0.0163, -0.0163, 0.5596, 0.5596, 0.5596, 0.5596, -0.5351, -0.5351, -0.5351, -0.5351 },
			{ 0.5940, 0.5940, -0.5940, -0.5940, 0.7734, 0.0000, -0.7734, 0.0000, 0.7864, -0.0000, -0.7864, -0.0000 },
		};
Jonas Stienen's avatar
Jonas Stienen committed
147

148
149
150
151
152
	}
	else
	{
		VA_EXCEPT1("Ambisonics truncation order unknown - ATM only 1,2,3 is supported");
	} */
Jonas Stienen's avatar
Jonas Stienen committed
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
	// Matrix
	for( int i=0; i < GetNumInputChannels(); i++ )
	{
		for( int j=0; j < GetNumVirtualLoudspeaker(); j++ )
		{
			double dGain = dYInv[i][j]; // todo calculate from LS setup @mko
			m_pDecoderMatrixPatchBay->ConnectChannels( 0, i, 0, j, dGain );
		}
	}

	m_viLastHRIRIndex.resize( GetNumVirtualLoudspeaker() );

	return;
}

CVAAmbisonicsBinauralMixdownReproduction::~CVAAmbisonicsBinauralMixdownReproduction()
{
	delete m_pdsStreamFilter;
	m_pdsStreamFilter = NULL;

	delete m_pDecoderMatrixPatchBay;
	m_pDecoderMatrixPatchBay = NULL;
}

void CVAAmbisonicsBinauralMixdownReproduction::SetInputDatasource( ITADatasource* p )
{
	m_pDecoderMatrixPatchBay->SetInputDatasource( 0, p );
}

ITADatasource* CVAAmbisonicsBinauralMixdownReproduction::GetOutputDatasource()
{
	return m_pdsStreamFilter;
}

int CVAAmbisonicsBinauralMixdownReproduction::GetNumVirtualLoudspeaker() const
{
	size_t iSize = m_pVirtualOutput->vpDevices.size();
	return int( iSize );
}

int CVAAmbisonicsBinauralMixdownReproduction::GetNumInputChannels() const
{
	return ( ( m_iAmbisonicsTruncationOrder + 1 ) * ( m_iAmbisonicsTruncationOrder + 1 ) );
}

void CVAAmbisonicsBinauralMixdownReproduction::SetTrackedListener( const int iID )
{
	m_iListenerID = iID;
}

void CVAAmbisonicsBinauralMixdownReproduction::UpdateScene( CVASceneState* pNewState )
{
	if( m_iListenerID == -1 ) 
		return;

	CVAListenerState* pLIstenerState( pNewState->GetListenerState( m_iListenerID ) );
	if( pLIstenerState == nullptr )
		return;

	IVAHRIRDataset* pHRIRSet = pLIstenerState->GetHRIRDataset();

	if( pHRIRSet == nullptr )
		return;

	double x,y,z, vx, vy, vz, ux, uy, uz;
	m_oParams.pCore->GetListenerRealWorldHeadPositionOrientationVU( m_iListenerID, x,y,z, vx,vy,vz, ux,uy,uz );
	VAVec3 vListenerPos( x, y, z );
	VAVec3 vListenerView( vx, vy, vz );
	VAVec3 vListenerUp( ux, uy, uz );

	for( int i=0; i<GetNumVirtualLoudspeaker(); i++ )
	{
		int iIndexLeft = 2*i+0;
		int iIndexRight = 2*i+1;

		const CVAHardwareDevice* pDevice( m_pVirtualOutput->vpDevices[i] );

		double dAzimuth = GetAzimuthOnTarget_DEG( vListenerPos, vListenerView, vListenerUp, pDevice->vPos );
		double dElevation = GetElevationOnTarget_DEG( vListenerPos, vListenerView, vListenerUp, pDevice->vPos );

		double dDistance = ( pDevice->vPos - vListenerPos ).Length();
		
		// Re-init temp buffer to match with current HRIR length
		int iHRIRFilterLength = pHRIRSet->GetProperties()->iFilterLength;
		if( m_sfHRIRTemp.length() != iHRIRFilterLength )
238
			m_sfHRIRTemp.init( 2, iHRIRFilterLength, false );
239

Jonas Stienen's avatar
Jonas Stienen committed
240
241
242
243
244
245
246
247
248
249
		int iIndex;
		pHRIRSet->GetNearestNeighbour( float( dAzimuth ), float( dElevation ), &iIndex );

		// Skip if still same HRIR
		if( iIndex == m_viLastHRIRIndex[i] )
			continue;

		pHRIRSet->GetHRIRByIndex( &m_sfHRIRTemp, iIndex, 0 );
		m_viLastHRIRIndex[i] = iIndex;

250
251
		ITAUPFilter* pFilterChL = m_pdsStreamFilter->pFilterPool->RequestFilter();
		ITAUPFilter* pFilterChR = m_pdsStreamFilter->pFilterPool->RequestFilter();
Jonas Stienen's avatar
Jonas Stienen committed
252
253
				
		int iLoadFilter = (std::min)( m_iHRIRFilterLength, iHRIRFilterLength );
Michael Kohnen's avatar
Michael Kohnen committed
254
255
256
257
		pFilterChL->Zeros();
		pFilterChL->Load( m_sfHRIRTemp[0].data(), iLoadFilter );
		pFilterChR->Zeros();
		pFilterChR->Load( m_sfHRIRTemp[1].data(), iLoadFilter );
Jonas Stienen's avatar
Jonas Stienen committed
258
259
		
		// Update
260
261
		m_pdsStreamFilter->vpConvolver[iIndexLeft]->ExchangeFilter( pFilterChL );
		m_pdsStreamFilter->vpConvolver[iIndexRight]->ExchangeFilter( pFilterChR );
Jonas Stienen's avatar
Jonas Stienen committed
262

Michael Kohnen's avatar
Michael Kohnen committed
263
264
		pFilterChL->Release();
		pFilterChR->Release();
Jonas Stienen's avatar
Jonas Stienen committed
265
266
267
268
269
270
271
272
273
274
275
276
277
278
	}

	m_pdsStreamFilter->pTrigger->trigger();

	return;
}

// --= Stream filter =--

CMixdownStreamFilter::CMixdownStreamFilter( int iNumLoudspeaker, double dSampleRate, int iBlockLength, int iMaxFilterLength ) : ITADatasourceRealization( 2, dSampleRate, iBlockLength )
	, pdsInput( NULL )
	, pFilterPool( NULL )
	, pTrigger( NULL )
{
279
	pFilterPool = new ITAUPFilterPool( (int) m_uiBlocklength, iMaxFilterLength, 24 );
280
	ITAUPFilter* pDirac = pFilterPool->RequestFilter();
Jonas Stienen's avatar
Jonas Stienen committed
281
282
	pDirac->identity();

283
	pTrigger = new ITAUPTrigger;
Jonas Stienen's avatar
Jonas Stienen committed
284
285
286

	for( int i=0; i< 2 * iNumLoudspeaker; i++ )
	{
287
		ITAUPConvolution* pConvolver = new ITAUPConvolution( (int) GetBlocklength(), iMaxFilterLength, pFilterPool );
Michael Kohnen's avatar
Michael Kohnen committed
288
289
290
		pConvolver->SetFilterExchangeTrigger( pTrigger );
		pConvolver->SetFilterExchangeMode( ITAUPConvolution::CROSSFADE_COSINE_SQUARE );
		pConvolver->SetFilterCrossfadeLength( (int) GetBlocklength() );
291
		pConvolver->ExchangeFilter( pDirac );
Jonas Stienen's avatar
Jonas Stienen committed
292
293
294
295

		vpConvolver.push_back( pConvolver );
	}

Michael Kohnen's avatar
Michael Kohnen committed
296
	pDirac->Release();
Jonas Stienen's avatar
Jonas Stienen committed
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320

	pTrigger->trigger();
}

CMixdownStreamFilter::~CMixdownStreamFilter()
{
	for( size_t i=0; i<vpConvolver.size(); i++ )
		delete vpConvolver[i];

	delete pFilterPool;
	delete pTrigger;
}

void CMixdownStreamFilter::ProcessStream( const ITAStreamInfo* pStreamInfo )
{
	float* pfBinauralOutputDataChL = GetWritePointer( 0 );
	float* pfBinauralOutputDataChR = GetWritePointer( 1 );
	
	// Iterate over loudspeaker channels
	for( unsigned int i=0; i<pdsInput->GetNumberOfChannels(); i++ )
	{
		int iIndexLeft = 2*i+0;
		int iIndexRight = 2*i+1;
		const float* pfInputData = pdsInput->GetBlockPointer( i, pStreamInfo );
321
		int iWriteMode = ( i == 0 ) ? ITAUPConvolution::OUTPUT_OVERWRITE : ITAUPConvolution::OUTPUT_MIX;
Michael Kohnen's avatar
Michael Kohnen committed
322
323
		vpConvolver[iIndexLeft]->Process( pfInputData, pfBinauralOutputDataChL, iWriteMode );
		vpConvolver[iIndexRight]->Process( pfInputData, pfBinauralOutputDataChR, iWriteMode );
Jonas Stienen's avatar
Jonas Stienen committed
324
325
326
327
328
329
330
331
332
333
	}

	IncrementWritePointer();
}

void CMixdownStreamFilter::PostIncrementBlockPointer()
{
	pdsInput->IncrementBlockPointer();
}

334
#endif // VACORE_WITH_REPRODUCTION_AMBISONICS_BINAURAL_MIXDOWN