VABinauralCluster.cpp 6.72 KB
Newer Older
1
2
3
#define NOMINMAX

#include <math.h>
4
5
#include "VABinauralCluster.h"

Lucas Moesch's avatar
WIP    
Lucas Moesch committed
6
7
8
// VA includes
#include "../../../../../directivities/VADirectivityDAFFHRIR.h"

9
// ITA includes
10
11
#include <ITAUPFilter.h>
#include <ITAUPFilterPool.h>
12
13
#include <ITAConstants.h>

14
// Utils
15
#include "../RelationMetrics/VARelationMetrics.h"
16
17
#include "../BinauralTimeOfArrivalEstimator/VABinauralTOAEstimator.h"

18
19
20
21
22
23
VABinauralCluster::VABinauralCluster()
{
}

VABinauralCluster::VABinauralCluster(const VABinauralCluster& cluster) :
	maxError(cluster.maxError),
Lucas Moesch's avatar
wip    
Lucas Moesch committed
24
	numSources(cluster.numSources),
25
26
27
28
29
30
31
32
	_clusterSourcePos(cluster._clusterSourcePos)
{

}


VABinauralCluster::~VABinauralCluster()
{
33
34
35
36
37
38
39
40
41
42
43
	VABinauralSoundSource* source;
	std::map<int, VABinauralSoundSource*>::const_iterator it;

	// clear all references from this cluster
	for (it = _sources.begin(); it != _sources.end();)
	{
		source = it->second;
		it = _sources.erase(it);

		source->RemoveReference();
	}
44
45
46
}

void
47
VABinauralCluster::init(int sourceID, VABinauralSoundSource* source, VABinauralListener* listener, ITAUPConvolution* FIRConvolverChL, ITAUPConvolution* FIRConvolverChR)
48
{
49
50
	_FIRConvolverChL = FIRConvolverChL;
	_FIRConvolverChR = FIRConvolverChR;
51
52
	_listener = listener;
	_listenerPos = listener->predPos;
53
	_clusterSourcePos = _clusterSourcePos + source->predPos;
54
55
	_clusterSourceToListenerPos = _clusterSourcePos - _listenerPos;

56
	// TODO: do new in the constructor and reshape on init if nescessary
57
	_output = new ITASampleFrame(2, listener->output->GetLength(), true);
58
	_tempHRIR = new ITASampleFrame(2, 256, true);
59
60
61
62
	
	_tmpChL.Init(listener->output->GetLength(), true);
	_tmpChR.Init(listener->output->GetLength(), true);

63
64
	maxError = getDistError(source);

Lucas Moesch's avatar
WIP    
Lucas Moesch committed
65
66
	_sources.insert(std::pair<int, VABinauralSoundSource*>(sourceID, source));

67
68
69
	// --
	source->AddReference();

Lucas Moesch's avatar
wip    
Lucas Moesch committed
70
	++numSources;
71
72
}

Lucas Moesch's avatar
Lucas Moesch committed
73
74
75
void
VABinauralCluster::init(VABinauralCluster* cluster)
{
76
77
	_FIRConvolverChL = cluster->_FIRConvolverChL;
	_FIRConvolverChR = cluster->_FIRConvolverChR;
Lucas Moesch's avatar
Lucas Moesch committed
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
	_listener = cluster->_listener;
	_listenerPos = _listener->predPos;
	_clusterSourcePos = cluster->_clusterSourcePos;
	_clusterSourceToListenerPos = _clusterSourcePos - _listenerPos;

	_output = new ITASampleFrame(2, _listener->output->GetLength(), true);

	_tmpChL.Init(_listener->output->GetLength(), true);
	_tmpChR.Init(_listener->output->GetLength(), true);

	maxError = cluster->maxError;

	std::map< int, VABinauralSoundSource*>::const_iterator it;
	for (it = cluster->_sources.begin(); it != cluster->_sources.end(); ++it)
	{
		_sources.insert(std::pair<int, VABinauralSoundSource*>(it->first, it->second));
		it->second->AddReference();
	}

	numSources = cluster->numSources;
}

100
101
102
103
104
105
ITASampleFrame*
VABinauralCluster::getOutput()
{
	// reset output Buffer
	_output->zero();

106
107
108
109
110
111
112
	// get cluster HRTF TOA
	VARelationMetrics clusterSourceMetrics;
	clusterSourceMetrics.calc(_listener->predPos, _listener->predView, _listener->predUp, _clusterSourcePos);

	double toaHRTFChL = _listener->toaEstimator->getTOALeft(clusterSourceMetrics.phi, clusterSourceMetrics.theta);
	double toaHRTFChR = _listener->toaEstimator->getTOARight(clusterSourceMetrics.phi, clusterSourceMetrics.theta);

113
114
115
116
117
118
119
	// set VDL Values
	std::map<int, VABinauralSoundSource*>::const_iterator it;
	for (it = _sources.begin(); it != _sources.end(); ++it)
	{
		VABinauralSoundSource* source = it->second;
		const ITASampleBuffer* input = source->data->pSignalSourceInputBuf;

120
121
		VARelationMetrics sourceMetrics;
		sourceMetrics.calc(_listener->predPos, _listener->predView, _listener->predUp, source->predPos);
122

123
124
125
126
		double toaDistance = sourceMetrics.dist / 343; // TODO: Medium Propagation....
	
		double toaSourceChL = _listener->toaEstimator->getTOALeft(sourceMetrics.phi, sourceMetrics.theta);
		double toSourceaChR = _listener->toaEstimator->getTOARight(sourceMetrics.phi, sourceMetrics.theta);
127

128
129
		source->vdlChL->SetDelayTime(toaDistance + toaSourceChL - toaHRTFChL);
		source->vdlChR->SetDelayTime(toaDistance + toSourceaChR - toaHRTFChR);
Lucas Moesch's avatar
WIP    
Lucas Moesch committed
130

131
132
133
134
135
136
137
		source->vdlChL->Process(input, &(_tmpChL));
		source->vdlChR->Process(input, &(_tmpChR));

		(*_output)[0] += _tmpChL;
		(*_output)[1] += _tmpChR;
	}

138
	// convolve here!
Lucas Moesch's avatar
WIP    
Lucas Moesch committed
139
140
	CVADirectivityDAFFHRIR* HRIR = (CVADirectivityDAFFHRIR *)_listener->directivity;

141
142
143
	ITAUPFilter* HRIRFilterChL = _FIRConvolverChL->GetFilterPool()->RequestFilter();
	ITAUPFilter* HRIRFilterChR = _FIRConvolverChR->GetFilterPool()->RequestFilter();

Lucas Moesch's avatar
WIP    
Lucas Moesch committed
144
145
	if (HRIR){
		int index = -1;
146
147
148
149
150
151
152
153
154
155
156
157
158
		int filterLength = 0;

		filterLength = HRIR->GetProperties()->iFilterLength;

		if (_tempHRIR->length() != filterLength)
		{
			_tempHRIR->init(2, filterLength, false);
		}

		HRIR->GetHRIR(_tempHRIR, clusterSourceMetrics.phi * 180 / ITAConstants::PI_F, clusterSourceMetrics.theta * 180 / ITAConstants::PI_F, clusterSourceMetrics.dist);

		HRIRFilterChL->Load((*_tempHRIR)[0].data(), filterLength);
		HRIRFilterChR->Load((*_tempHRIR)[1].data(), filterLength);
159

160
161
162
163
		_FIRConvolverChL->ExchangeFilter(HRIRFilterChL);
		_FIRConvolverChR->ExchangeFilter(HRIRFilterChR);
		_FIRConvolverChL->ReleaseFilter(HRIRFilterChL);
		_FIRConvolverChR->ReleaseFilter(HRIRFilterChR);
Lucas Moesch's avatar
WIP    
Lucas Moesch committed
164
165
166
167

		_FIRConvolverChL->Process((*_output)[0].data(), (*_output)[0].data(), ITABase::MixingMethod::OVERWRITE);
		_FIRConvolverChR->Process((*_output)[1].data(), (*_output)[1].data(), ITABase::MixingMethod::OVERWRITE);
	}
168

169
170
171
	return _output;
}

172
173
174
double
VABinauralCluster::getDistError(VABinauralSoundSource* source)
{
175
176
	VAVec3 sourceToListenerPos = source->predPos - _listenerPos;
	double dotp = _clusterSourceToListenerPos.Dot(sourceToListenerPos);
177

178
	return (dotp * dotp) / (_clusterSourcePos.Dot(_clusterSourcePos) * sourceToListenerPos.Dot(sourceToListenerPos));
179
180
181
}

void 
Lucas Moesch's avatar
WIP    
Lucas Moesch committed
182
VABinauralCluster::addSource(int sourceID, VABinauralSoundSource* source)
183
184
185
{
	double err = getDistError(source);

Lucas Moesch's avatar
wip    
Lucas Moesch committed
186
	_clusterSourcePos = (source->predPos + _clusterSourcePos * numSources) / (numSources + 1);
187
188
	_clusterSourceToListenerPos = _clusterSourcePos - _listenerPos;

189
190
	maxError = std::max(err, maxError);

Lucas Moesch's avatar
WIP    
Lucas Moesch committed
191
192
	_sources.insert(std::pair<int, VABinauralSoundSource*>(sourceID, source));

193
194
195
	// --
	source->AddReference();

Lucas Moesch's avatar
wip    
Lucas Moesch committed
196
	++numSources;
197
198
199
}

void
200
VABinauralCluster::addSource(int sourceID, VABinauralSoundSource* source, double error)
201
{
Lucas Moesch's avatar
wip    
Lucas Moesch committed
202
	_clusterSourcePos = (source->predPos + _clusterSourcePos * numSources) / (numSources + 1);
203
204
	_clusterSourceToListenerPos = _clusterSourcePos - _listenerPos;

205
206
	maxError = std::max(error, maxError);

207
208
209
210
	_sources.insert(std::pair<int, VABinauralSoundSource*>(sourceID, source));
	// --
	source->AddReference();

Lucas Moesch's avatar
wip    
Lucas Moesch committed
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
	++numSources;
}

void
VABinauralCluster::removeSource(int sourceID)
{
	std::map<int, VABinauralSoundSource*>::const_iterator it = _sources.find(sourceID);
	VABinauralSoundSource* source = it->second;

	_sources.erase(it);

	source->RemoveReference();

	_clusterSourcePos = (_clusterSourcePos * numSources - source->predPos) / (numSources - 1);

	//TODO: MaxError
	--numSources;

229
230
}