VABinauralCluster.cpp 7.64 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
Lucas Moesch's avatar
Lucas Moesch committed
15
#include "../Config/VAConfig.h"
16
#include "../RelationMetrics/VARelationMetrics.h"
17 18
#include "../BinauralTimeOfArrivalEstimator/VABinauralTOAEstimator.h"

19 20
VABinauralCluster::VABinauralCluster()
{
Lucas Moesch's avatar
Lucas Moesch committed
21 22
	_output = new ITASampleFrame(2, VAConfig::blockLength, true);
	_tempHRIR = new ITASampleFrame(2, VAConfig::hrirLength, true);
23

Lucas Moesch's avatar
Lucas Moesch committed
24 25 26 27 28
	// initialize left channel convolver for each cluster
	_FIRConvolverChL = new ITAUPConvolution(VAConfig::blockLength, VAConfig::hrirLength);
	_FIRConvolverChL->SetFilterExchangeFadingFunction(ITABase::FadingFunction::COSINE_SQUARE);
	_FIRConvolverChL->SetFilterCrossfadeLength((std::min)(VAConfig::blockLength, 32));
	_FIRConvolverChL->SetGain(1.0f, true);
29

Lucas Moesch's avatar
Lucas Moesch committed
30 31 32
	ITAUPFilter* HRIRFilterChL = _FIRConvolverChL->RequestFilter();
	HRIRFilterChL->identity();
	_FIRConvolverChL->ExchangeFilter(HRIRFilterChL);
33

Lucas Moesch's avatar
Lucas Moesch committed
34
	// initialize right channel convolver for each cluster
35

Lucas Moesch's avatar
Lucas Moesch committed
36
	_FIRConvolverChR = new ITAUPConvolution(VAConfig::blockLength, VAConfig::hrirLength);
37

Lucas Moesch's avatar
Lucas Moesch committed
38 39 40
	_FIRConvolverChR->SetFilterExchangeFadingFunction(ITABase::FadingFunction::COSINE_SQUARE);
	_FIRConvolverChR->SetFilterCrossfadeLength((std::min)(VAConfig::blockLength, 32));
	_FIRConvolverChR->SetGain(1.0f, true);
41

Lucas Moesch's avatar
Lucas Moesch committed
42 43 44 45
	ITAUPFilter* HRIRFilterChR = _FIRConvolverChR->RequestFilter();
	HRIRFilterChR->identity();

	_FIRConvolverChR->ExchangeFilter(HRIRFilterChR);
46 47
}

Lucas Moesch's avatar
Lucas Moesch committed
48 49 50
VABinauralCluster::~VABinauralCluster()
{}

51
void
Lucas Moesch's avatar
Lucas Moesch committed
52
VABinauralCluster::init(int sourceID, VABinauralSoundSource* source, VABinauralListener* listener)
53
{
54 55
	_listener = listener;
	_listenerPos = listener->predPos;
56
	_clusterSourcePos = _clusterSourcePos + source->predPos;
57
	_clusterSourceToListenerPos = _clusterSourcePos - _listenerPos;
Lucas Moesch's avatar
Lucas Moesch committed
58 59 60 61
	
	int outputLength = listener->output->GetLength();
	
	if (_output->length() != outputLength) _output->init(2, outputLength, true);
62 63 64 65
	
	_tmpChL.Init(listener->output->GetLength(), true);
	_tmpChR.Init(listener->output->GetLength(), true);

66 67
	maxError = getDistError(source);

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

70 71 72
	// --
	source->AddReference();

Lucas Moesch's avatar
wip  
Lucas Moesch committed
73
	++numSources;
74 75
}

Lucas Moesch's avatar
Lucas Moesch committed
76 77 78 79 80 81 82 83
void
VABinauralCluster::init(VABinauralCluster* cluster)
{
	_listener = cluster->_listener;
	_listenerPos = _listener->predPos;
	_clusterSourcePos = cluster->_clusterSourcePos;
	_clusterSourceToListenerPos = _clusterSourcePos - _listenerPos;

Lucas Moesch's avatar
Lucas Moesch committed
84 85 86
	int outputLength = _listener->output->GetLength();

	if (_output->length() != outputLength) _output->init(2, outputLength, true);
Lucas Moesch's avatar
Lucas Moesch committed
87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102

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

103 104 105 106 107 108
ITASampleFrame*
VABinauralCluster::getOutput()
{
	// reset output Buffer
	_output->zero();

109 110 111 112 113 114 115
	// 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);

116 117 118 119 120 121 122
	// 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;

123 124
		VARelationMetrics sourceMetrics;
		sourceMetrics.calc(_listener->predPos, _listener->predView, _listener->predUp, source->predPos);
125

126 127 128
		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);
129

130 131
		float gain = float(1/sourceMetrics.dist * source->state->GetVolume(VAConfig::amplitudeCalibration));

132 133
		source->vdlChL->SetDelayTime(toaDistance + toaSourceChL - toaHRTFChL);
		source->vdlChR->SetDelayTime(toaDistance + toSourceaChR - toaHRTFChR);
Lucas Moesch's avatar
WIP  
Lucas Moesch committed
134

135 136
		source->vdlChL->Process(input, &(_tmpChL));
		source->vdlChR->Process(input, &(_tmpChR));
137 138 139
		
		_tmpChL.mul_scalar(gain);
		_tmpChR.mul_scalar(gain);
140 141 142 143 144

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

145
	// convolve here!
Lucas Moesch's avatar
WIP  
Lucas Moesch committed
146 147
	CVADirectivityDAFFHRIR* HRIR = (CVADirectivityDAFFHRIR *)_listener->directivity;

148 149 150
	ITAUPFilter* HRIRFilterChL = _FIRConvolverChL->GetFilterPool()->RequestFilter();
	ITAUPFilter* HRIRFilterChR = _FIRConvolverChR->GetFilterPool()->RequestFilter();

Lucas Moesch's avatar
WIP  
Lucas Moesch committed
151 152
	if (HRIR){
		int index = -1;
153 154 155 156 157 158 159 160 161 162 163 164 165
		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);
166

167 168 169 170
		_FIRConvolverChL->ExchangeFilter(HRIRFilterChL);
		_FIRConvolverChR->ExchangeFilter(HRIRFilterChR);
		_FIRConvolverChL->ReleaseFilter(HRIRFilterChL);
		_FIRConvolverChR->ReleaseFilter(HRIRFilterChR);
Lucas Moesch's avatar
WIP  
Lucas Moesch committed
171 172 173 174

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

176 177 178
	return _output;
}

179 180 181
double
VABinauralCluster::getDistError(VABinauralSoundSource* source)
{
182 183
	VAVec3 sourceToListenerPos = source->predPos - _listenerPos;
	double dotp = _clusterSourceToListenerPos.Dot(sourceToListenerPos);
184

185
	return (dotp * dotp) / (_clusterSourcePos.Dot(_clusterSourcePos) * sourceToListenerPos.Dot(sourceToListenerPos));
186 187 188
}

void 
Lucas Moesch's avatar
WIP  
Lucas Moesch committed
189
VABinauralCluster::addSource(int sourceID, VABinauralSoundSource* source)
190 191 192
{
	double err = getDistError(source);

Lucas Moesch's avatar
wip  
Lucas Moesch committed
193
	_clusterSourcePos = (source->predPos + _clusterSourcePos * numSources) / (numSources + 1);
194 195
	_clusterSourceToListenerPos = _clusterSourcePos - _listenerPos;

196 197
	maxError = std::max(err, maxError);

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

200 201 202
	// --
	source->AddReference();

Lucas Moesch's avatar
wip  
Lucas Moesch committed
203
	++numSources;
204 205 206
}

void
207
VABinauralCluster::addSource(int sourceID, VABinauralSoundSource* source, double error)
208
{
Lucas Moesch's avatar
wip  
Lucas Moesch committed
209
	_clusterSourcePos = (source->predPos + _clusterSourcePos * numSources) / (numSources + 1);
210 211
	_clusterSourceToListenerPos = _clusterSourcePos - _listenerPos;

212 213
	maxError = std::max(error, maxError);

214 215 216 217
	_sources.insert(std::pair<int, VABinauralSoundSource*>(sourceID, source));
	// --
	source->AddReference();

Lucas Moesch's avatar
wip  
Lucas Moesch committed
218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235
	++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;

236 237
}

Lucas Moesch's avatar
Lucas Moesch committed
238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253
void
VABinauralCluster::PreRelease()
{
	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();
	}
}