diff --git a/assignment-client/src/audio/AudioMixer.cpp b/assignment-client/src/audio/AudioMixer.cpp index 7ba9242306..08e6e029a0 100644 --- a/assignment-client/src/audio/AudioMixer.cpp +++ b/assignment-client/src/audio/AudioMixer.cpp @@ -193,8 +193,12 @@ void AudioMixer::addStreamToMixForListeningNodeWithStream(AudioMixerClientData& // check if this is a server echo of a source back to itself bool isEcho = (&streamToAdd == &listeningNodeStream); - // figure out the gain for this source at the listener glm::vec3 relativePosition = streamToAdd.getPosition() - listeningNodeStream.getPosition(); + + // figure out the distance between source and listener + float distance = glm::max(glm::length(relativePosition), EPSILON); + + // figure out the gain for this source at the listener float gain = gainForSource(streamToAdd, listeningNodeStream, relativePosition, isEcho); // figure out the azimuth to this source at the listener @@ -240,7 +244,7 @@ void AudioMixer::addStreamToMixForListeningNodeWithStream(AudioMixerClientData& // this is not done for stereo streams since they do not go through the HRTF static int16_t silentMonoBlock[AudioConstants::NETWORK_FRAME_SAMPLES_PER_CHANNEL] = {}; - hrtf.renderSilent(silentMonoBlock, _mixedSamples, HRTF_DATASET_INDEX, azimuth, gain, + hrtf.renderSilent(silentMonoBlock, _mixedSamples, HRTF_DATASET_INDEX, azimuth, distance, gain, AudioConstants::NETWORK_FRAME_SAMPLES_PER_CHANNEL); ++_hrtfSilentRenders;; @@ -287,7 +291,7 @@ void AudioMixer::addStreamToMixForListeningNodeWithStream(AudioMixerClientData& // silent frame from source // we still need to call renderSilent via the HRTF for mono source - hrtf.renderSilent(streamBlock, _mixedSamples, HRTF_DATASET_INDEX, azimuth, gain, + hrtf.renderSilent(streamBlock, _mixedSamples, HRTF_DATASET_INDEX, azimuth, distance, gain, AudioConstants::NETWORK_FRAME_SAMPLES_PER_CHANNEL); ++_hrtfSilentRenders; @@ -300,7 +304,7 @@ void AudioMixer::addStreamToMixForListeningNodeWithStream(AudioMixerClientData& // the mixer is struggling so we're going to drop off some streams // we call renderSilent via the HRTF with the actual frame data and a gain of 0.0 - hrtf.renderSilent(streamBlock, _mixedSamples, HRTF_DATASET_INDEX, azimuth, 0.0f, + hrtf.renderSilent(streamBlock, _mixedSamples, HRTF_DATASET_INDEX, azimuth, distance, 0.0f, AudioConstants::NETWORK_FRAME_SAMPLES_PER_CHANNEL); ++_hrtfStruggleRenders; @@ -311,7 +315,7 @@ void AudioMixer::addStreamToMixForListeningNodeWithStream(AudioMixerClientData& ++_hrtfRenders; // mono stream, call the HRTF with our block and calculated azimuth and gain - hrtf.render(streamBlock, _mixedSamples, HRTF_DATASET_INDEX, azimuth, gain, + hrtf.render(streamBlock, _mixedSamples, HRTF_DATASET_INDEX, azimuth, distance, gain, AudioConstants::NETWORK_FRAME_SAMPLES_PER_CHANNEL); } diff --git a/libraries/audio-client/src/AudioClient.cpp b/libraries/audio-client/src/AudioClient.cpp index bec30edb4e..7a24832ea0 100644 --- a/libraries/audio-client/src/AudioClient.cpp +++ b/libraries/audio-client/src/AudioClient.cpp @@ -882,13 +882,13 @@ void AudioClient::mixLocalAudioInjectors(int16_t* inputBuffer) { } else { - // calculate gain and azimuth for hrtf + // calculate distance, gain and azimuth for hrtf glm::vec3 relativePosition = injector->getPosition() - _positionGetter(); - float gain = gainForSource(relativePosition, injector->getVolume()); - float azimuth = azimuthForSource(relativePosition); + float distance = glm::max(glm::length(relativePosition), EPSILON); + float gain = gainForSource(distance, injector->getVolume()); + float azimuth = azimuthForSource(relativePosition); - - injector->getLocalHRTF().render(_scratchBuffer, _hrtfBuffer, 1, azimuth, gain, AudioConstants::NETWORK_FRAME_SAMPLES_PER_CHANNEL); + injector->getLocalHRTF().render(_scratchBuffer, _hrtfBuffer, 1, azimuth, distance, gain, AudioConstants::NETWORK_FRAME_SAMPLES_PER_CHANNEL); } } else { @@ -1298,37 +1298,19 @@ float AudioClient::azimuthForSource(const glm::vec3& relativePosition) { } } -float AudioClient::gainForSource(const glm::vec3& relativePosition, float volume) { - // TODO: put these in a place where we can share with AudioMixer! - const float DEFAULT_ATTENUATION_PER_DOUBLING_IN_DISTANCE = 0.18f; +float AudioClient::gainForSource(float distance, float volume) { + const float ATTENUATION_BEGINS_AT_DISTANCE = 1.0f; - - //qDebug() << "initial gain is " << volume; - // I'm assuming that the AudioMixer's getting of the stream's attenuation // factor is basically same as getting volume float gain = volume; - float distanceBetween = glm::length(relativePosition); - if (distanceBetween < EPSILON ) { - distanceBetween = EPSILON; + + // attenuate based on distance + if (distance >= ATTENUATION_BEGINS_AT_DISTANCE) { + gain /= distance; // attenuation = -6dB * log2(distance) } - // audio mixer has notion of zones. Unsure how to map that across here... - - // attenuate based on distance now - if (distanceBetween >= ATTENUATION_BEGINS_AT_DISTANCE) { - float distanceCoefficient = 1.0f - (logf(distanceBetween/ATTENUATION_BEGINS_AT_DISTANCE) / logf(2.0f) - * DEFAULT_ATTENUATION_PER_DOUBLING_IN_DISTANCE); - if (distanceCoefficient < 0.0f) { - distanceCoefficient = 0.0f; - } - - gain *= distanceCoefficient; - } - - //qDebug() << "calculated gain as " << gain; - return gain; } diff --git a/libraries/audio-client/src/AudioClient.h b/libraries/audio-client/src/AudioClient.h index df7b10ab04..3e4aa931a6 100644 --- a/libraries/audio-client/src/AudioClient.h +++ b/libraries/audio-client/src/AudioClient.h @@ -217,7 +217,7 @@ private: void outputFormatChanged(); void mixLocalAudioInjectors(int16_t* inputBuffer); float azimuthForSource(const glm::vec3& relativePosition); - float gainForSource(const glm::vec3& relativePosition, float volume); + float gainForSource(float distance, float volume); QByteArray firstInputFrame; QAudioInput* _audioInput; diff --git a/libraries/audio/src/AudioHRTF.cpp b/libraries/audio/src/AudioHRTF.cpp index e7cf4436c6..378e2154c1 100644 --- a/libraries/audio/src/AudioHRTF.cpp +++ b/libraries/audio/src/AudioHRTF.cpp @@ -16,6 +16,13 @@ #include "AudioHRTF.h" #include "AudioHRTFData.h" +#ifndef MAX +#define MAX(a,b) (((a) > (b)) ? (a) : (b)) +#endif +#ifndef MIN +#define MIN(a,b) (((a) < (b)) ? (a) : (b)) +#endif + // // Equal-gain crossfade // @@ -58,6 +65,103 @@ static const float crossfadeTable[HRTF_BLOCK] = { 0.0024846123f, 0.0019026510f, 0.0013981014f, 0.0009710421f, 0.0006215394f, 0.0003496476f, 0.0001554090f, 0.0000388538f, }; +// +// Model the frequency-dependent attenuation of sound propogation in air. +// +// Fit using linear regression to a log-log model of lowpass cutoff frequency vs distance, +// loosely based on data from Handbook of Acoustics. Only the onset of significant +// attenuation is modelled, not the filter slope. +// +// 1m -> -3dB @ 55kHz +// 10m -> -3dB @ 12kHz +// 100m -> -3dB @ 2.5kHz +// 1km -> -3dB @ 0.6kHz +// 10km -> -3dB @ 0.1kHz +// +static const int NLOWPASS = 64; +static const float lowpassTable[NLOWPASS][5] = { // { b0, b1, b2, a1, a2 } + // distance = 1 + { 0.999772371f, 1.399489756f, 0.454495527f, 1.399458985f, 0.454298669f }, + { 0.999631480f, 1.357609808f, 0.425210203f, 1.357549905f, 0.424901586f }, + { 0.999405154f, 1.311503050f, 0.394349994f, 1.311386830f, 0.393871368f }, + { 0.999042876f, 1.260674595f, 0.361869089f, 1.260450057f, 0.361136504f }, + // distance = 2 + { 0.998465222f, 1.204646525f, 0.327757118f, 1.204214978f, 0.326653886f }, + { 0.997548106f, 1.143019308f, 0.292064663f, 1.142195387f, 0.290436690f }, + { 0.996099269f, 1.075569152f, 0.254941286f, 1.074009405f, 0.252600301f }, + { 0.993824292f, 1.002389610f, 0.216688640f, 0.999469185f, 0.213433357f }, + // distance = 4 + { 0.990280170f, 0.924075266f, 0.177827150f, 0.918684864f, 0.173497723f }, + { 0.984818279f, 0.841917936f, 0.139164195f, 0.832151968f, 0.133748443f }, + { 0.976528670f, 0.758036513f, 0.101832398f, 0.740761682f, 0.095635899f }, + { 0.964216485f, 0.675305244f, 0.067243474f, 0.645654855f, 0.061110348f }, + // distance = 8 + { 0.946463038f, 0.596943020f, 0.036899688f, 0.547879974f, 0.032425772f }, + { 0.921823868f, 0.525770189f, 0.012060451f, 0.447952111f, 0.011702396f }, + { 0.890470015f, 0.463334299f, -0.001227816f, 0.347276405f, 0.005300092f }, + { 0.851335343f, 0.407521164f, -0.009353968f, 0.241900234f, 0.007602305f }, + // distance = 16 + { 0.804237360f, 0.358139558f, -0.014293332f, 0.130934213f, 0.017149373f }, + { 0.750073259f, 0.314581568f, -0.016625381f, 0.014505388f, 0.033524057f }, + { 0.690412072f, 0.275936128f, -0.017054561f, -0.106682490f, 0.055976129f }, + { 0.627245545f, 0.241342015f, -0.016246850f, -0.231302564f, 0.083643275f }, + // distance = 32 + { 0.562700627f, 0.210158533f, -0.014740899f, -0.357562697f, 0.115680957f }, + { 0.498787849f, 0.181982455f, -0.012925406f, -0.483461730f, 0.151306628f }, + { 0.437224055f, 0.156585449f, -0.011055180f, -0.607042210f, 0.189796534f }, + { 0.379336998f, 0.133834032f, -0.009281617f, -0.726580065f, 0.230469477f }, + // distance = 64 + { 0.326040627f, 0.113624970f, -0.007683443f, -0.840693542f, 0.272675696f }, + { 0.277861727f, 0.095845793f, -0.006291936f, -0.948380091f, 0.315795676f }, + { 0.234997480f, 0.080357656f, -0.005109519f, -1.049001190f, 0.359246807f }, + { 0.197386484f, 0.066993521f, -0.004122547f, -1.142236313f, 0.402493771f }, + // distance = 128 + { 0.164780457f, 0.055564709f, -0.003309645f, -1.228023442f, 0.445058962f }, + { 0.136808677f, 0.045870650f, -0.002646850f, -1.306498037f, 0.486530514f }, + { 0.113031290f, 0.037708627f, -0.002110591f, -1.377937457f, 0.526566783f }, + { 0.092980475f, 0.030881892f, -0.001679255f, -1.442713983f, 0.564897095f }, + // distance = 256 + { 0.076190239f, 0.025205585f, -0.001333863f, -1.501257246f, 0.601319206f }, + { 0.062216509f, 0.020510496f, -0.001058229f, -1.554025452f, 0.635694228f }, + { 0.050649464f, 0.016644994f, -0.000838826f, -1.601484205f, 0.667939837f }, + { 0.041120009f, 0.013475547f, -0.000664513f, -1.644091518f, 0.698022561f }, + // distance = 512 + { 0.033302044f, 0.010886252f, -0.000526217f, -1.682287704f, 0.725949783f }, + { 0.026911868f, 0.008777712f, -0.000416605f, -1.716488979f, 0.751761953f }, + { 0.021705773f, 0.007065551f, -0.000329788f, -1.747083800f, 0.775525335f }, + { 0.017476603f, 0.005678758f, -0.000261057f, -1.774431204f, 0.797325509f }, + // distance = 1024 + { 0.014049828f, 0.004558012f, -0.000206658f, -1.798860530f, 0.817261711f }, + { 0.011279504f, 0.003654067f, -0.000163610f, -1.820672082f, 0.835442043f }, + { 0.009044384f, 0.002926264f, -0.000129544f, -1.840138412f, 0.851979516f }, + { 0.007244289f, 0.002341194f, -0.000102586f, -1.857505967f, 0.866988864f }, + // distance = 2048 + { 0.005796846f, 0.001871515f, -0.000081250f, -1.872996926f, 0.880584038f }, + { 0.004634607f, 0.001494933f, -0.000064362f, -1.886811124f, 0.892876302f }, + { 0.003702543f, 0.001193324f, -0.000050993f, -1.899127955f, 0.903972829f }, + { 0.002955900f, 0.000951996f, -0.000040407f, -1.910108223f, 0.913975712f }, + // distance = 4096 + { 0.002358382f, 0.000759068f, -0.000032024f, -1.919895894f, 0.922981321f }, + { 0.001880626f, 0.000604950f, -0.000025383f, -1.928619738f, 0.931079931f }, + { 0.001498926f, 0.000481920f, -0.000020123f, -1.936394836f, 0.938355560f }, + { 0.001194182f, 0.000383767f, -0.000015954f, -1.943323983f, 0.944885977f }, + // distance = 8192 + { 0.000951028f, 0.000305502f, -0.000012651f, -1.949498943f, 0.950742822f }, + { 0.000757125f, 0.000243126f, -0.000010033f, -1.955001608f, 0.955991826f }, + { 0.000602572f, 0.000193434f, -0.000007957f, -1.959905036f, 0.960693085f }, + { 0.000479438f, 0.000153861f, -0.000006312f, -1.964274383f, 0.964901371f }, + // distance = 16384 + { 0.000381374f, 0.000122359f, -0.000005007f, -1.968167752f, 0.968666478f }, + { 0.000303302f, 0.000097288f, -0.000003972f, -1.971636944f, 0.972033562f }, + { 0.000241166f, 0.000077342f, -0.000003151f, -1.974728138f, 0.975043493f }, + { 0.000191726f, 0.000061475f, -0.000002500f, -1.977482493f, 0.977733194f }, + // distance = 32768 + { 0.000152399f, 0.000048857f, -0.000001984f, -1.979936697f, 0.980135969f }, + { 0.000121122f, 0.000038825f, -0.000001574f, -1.982123446f, 0.982281818f }, + { 0.000096252f, 0.000030849f, -0.000001249f, -1.984071877f, 0.984197728f }, + { 0.000076480f, 0.000024509f, -0.000000991f, -1.985807957f, 0.985907955f }, +}; + static const float TWOPI = 6.283185307f; // @@ -162,40 +266,68 @@ static void interleave_4x4(float* src0, float* src1, float* src2, float* src3, f } } -// 4 channels (interleaved) -static void biquad_4x4(float* src, float* dst, float coef[5][4], float state[2][4], int numFrames) { +// process 2 cascaded biquads on 4 channels (interleaved) +// biquads computed in parallel, by adding one sample of delay +static void biquad2_4x4(float* src, float* dst, float coef[5][8], float state[3][8], int numFrames) { // enable flush-to-zero mode to prevent denormals unsigned int ftz = _MM_GET_FLUSH_ZERO_MODE(); _MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON); - __m128 w1 = _mm_loadu_ps(state[0]); - __m128 w2 = _mm_loadu_ps(state[1]); + // restore state + __m128 y00 = _mm_loadu_ps(&state[0][0]); + __m128 w10 = _mm_loadu_ps(&state[1][0]); + __m128 w20 = _mm_loadu_ps(&state[2][0]); - __m128 b0 = _mm_loadu_ps(coef[0]); - __m128 b1 = _mm_loadu_ps(coef[1]); - __m128 b2 = _mm_loadu_ps(coef[2]); - __m128 a1 = _mm_loadu_ps(coef[3]); - __m128 a2 = _mm_loadu_ps(coef[4]); + __m128 y01; + __m128 w11 = _mm_loadu_ps(&state[1][4]); + __m128 w21 = _mm_loadu_ps(&state[2][4]); + + // first biquad coefs + __m128 b00 = _mm_loadu_ps(&coef[0][0]); + __m128 b10 = _mm_loadu_ps(&coef[1][0]); + __m128 b20 = _mm_loadu_ps(&coef[2][0]); + __m128 a10 = _mm_loadu_ps(&coef[3][0]); + __m128 a20 = _mm_loadu_ps(&coef[4][0]); + + // second biquad coefs + __m128 b01 = _mm_loadu_ps(&coef[0][4]); + __m128 b11 = _mm_loadu_ps(&coef[1][4]); + __m128 b21 = _mm_loadu_ps(&coef[2][4]); + __m128 a11 = _mm_loadu_ps(&coef[3][4]); + __m128 a21 = _mm_loadu_ps(&coef[4][4]); for (int i = 0; i < numFrames; i++) { + __m128 x00 = _mm_loadu_ps(&src[4*i]); + __m128 x01 = y00; // first biquad output + // transposed Direct Form II - __m128 x0 = _mm_loadu_ps(&src[4*i]); - __m128 y0; + y00 = _mm_add_ps(w10, _mm_mul_ps(x00, b00)); + y01 = _mm_add_ps(w11, _mm_mul_ps(x01, b01)); - y0 = _mm_add_ps(w1, _mm_mul_ps(x0, b0)); - w1 = _mm_add_ps(w2, _mm_mul_ps(x0, b1)); - w2 = _mm_mul_ps(x0, b2); - w1 = _mm_sub_ps(w1, _mm_mul_ps(y0, a1)); - w2 = _mm_sub_ps(w2, _mm_mul_ps(y0, a2)); + w10 = _mm_add_ps(w20, _mm_mul_ps(x00, b10)); + w11 = _mm_add_ps(w21, _mm_mul_ps(x01, b11)); - _mm_storeu_ps(&dst[4*i], y0); + w20 = _mm_mul_ps(x00, b20); + w21 = _mm_mul_ps(x01, b21); + + w10 = _mm_sub_ps(w10, _mm_mul_ps(y00, a10)); + w11 = _mm_sub_ps(w11, _mm_mul_ps(y01, a11)); + + w20 = _mm_sub_ps(w20, _mm_mul_ps(y00, a20)); + w21 = _mm_sub_ps(w21, _mm_mul_ps(y01, a21)); + + _mm_storeu_ps(&dst[4*i], y01); // second biquad output } // save state - _mm_storeu_ps(state[0], w1); - _mm_storeu_ps(state[1], w2); + _mm_storeu_ps(&state[0][0], y00); + _mm_storeu_ps(&state[1][0], w10); + _mm_storeu_ps(&state[2][0], w20); + + _mm_storeu_ps(&state[1][4], w11); + _mm_storeu_ps(&state[2][4], w21); _MM_SET_FLUSH_ZERO_MODE(ftz); } @@ -345,56 +477,105 @@ static void interleave_4x4(float* src0, float* src1, float* src2, float* src3, f } } -// 4 channels (interleaved) -static void biquad_4x4(float* src, float* dst, float coef[5][4], float state[2][4], int numFrames) { +// process 2 cascaded biquads on 4 channels (interleaved) +// biquads are computed in parallel, by adding one sample of delay +static void biquad2_4x4(float* src, float* dst, float coef[5][8], float state[3][8], int numFrames) { - // channel 0 - float w10 = state[0][0]; - float w20 = state[1][0]; + // restore state + float y00 = state[0][0]; + float w10 = state[1][0]; + float w20 = state[2][0]; + float y01 = state[0][1]; + float w11 = state[1][1]; + float w21 = state[2][1]; + + float y02 = state[0][2]; + float w12 = state[1][2]; + float w22 = state[2][2]; + + float y03 = state[0][3]; + float w13 = state[1][3]; + float w23 = state[2][3]; + + float y04; + float w14 = state[1][4]; + float w24 = state[2][4]; + + float y05; + float w15 = state[1][5]; + float w25 = state[2][5]; + + float y06; + float w16 = state[1][6]; + float w26 = state[2][6]; + + float y07; + float w17 = state[1][7]; + float w27 = state[2][7]; + + // first biquad coefs float b00 = coef[0][0]; float b10 = coef[1][0]; float b20 = coef[2][0]; float a10 = coef[3][0]; float a20 = coef[4][0]; - // channel 1 - float w11 = state[0][1]; - float w21 = state[1][1]; - float b01 = coef[0][1]; float b11 = coef[1][1]; float b21 = coef[2][1]; float a11 = coef[3][1]; float a21 = coef[4][1]; - // channel 2 - float w12 = state[0][2]; - float w22 = state[1][2]; - float b02 = coef[0][2]; float b12 = coef[1][2]; float b22 = coef[2][2]; float a12 = coef[3][2]; float a22 = coef[4][2]; - // channel 3 - float w13 = state[0][3]; - float w23 = state[1][3]; - float b03 = coef[0][3]; float b13 = coef[1][3]; float b23 = coef[2][3]; float a13 = coef[3][3]; float a23 = coef[4][3]; + // second biquad coefs + float b04 = coef[0][4]; + float b14 = coef[1][4]; + float b24 = coef[2][4]; + float a14 = coef[3][4]; + float a24 = coef[4][4]; + + float b05 = coef[0][5]; + float b15 = coef[1][5]; + float b25 = coef[2][5]; + float a15 = coef[3][5]; + float a25 = coef[4][5]; + + float b06 = coef[0][6]; + float b16 = coef[1][6]; + float b26 = coef[2][6]; + float a16 = coef[3][6]; + float a26 = coef[4][6]; + + float b07 = coef[0][7]; + float b17 = coef[1][7]; + float b27 = coef[2][7]; + float a17 = coef[3][7]; + float a27 = coef[4][7]; + for (int i = 0; i < numFrames; i++) { + // first biquad input float x00 = src[4*i+0] + 1.0e-20f; // prevent denormals float x01 = src[4*i+1] + 1.0e-20f; float x02 = src[4*i+2] + 1.0e-20f; float x03 = src[4*i+3] + 1.0e-20f; - float y00, y01, y02, y03; + // second biquad input is previous output + float x04 = y00; + float x05 = y01; + float x06 = y02; + float x07 = y03; // transposed Direct Form II y00 = b00 * x00 + w10; @@ -413,24 +594,57 @@ static void biquad_4x4(float* src, float* dst, float coef[5][4], float state[2][ w13 = b13 * x03 - a13 * y03 + w23; w23 = b23 * x03 - a23 * y03; - dst[4*i+0] = y00; - dst[4*i+1] = y01; - dst[4*i+2] = y02; - dst[4*i+3] = y03; + // transposed Direct Form II + y04 = b04 * x04 + w14; + w14 = b14 * x04 - a14 * y04 + w24; + w24 = b24 * x04 - a24 * y04; + + y05 = b05 * x05 + w15; + w15 = b15 * x05 - a15 * y05 + w25; + w25 = b25 * x05 - a25 * y05; + + y06 = b06 * x06 + w16; + w16 = b16 * x06 - a16 * y06 + w26; + w26 = b26 * x06 - a26 * y06; + + y07 = b07 * x07 + w17; + w17 = b17 * x07 - a17 * y07 + w27; + w27 = b27 * x07 - a27 * y07; + + dst[4*i+0] = y04; // second biquad output + dst[4*i+1] = y05; + dst[4*i+2] = y06; + dst[4*i+3] = y07; } // save state - state[0][0] = w10; - state[1][0] = w20; + state[0][0] = y00; + state[1][0] = w10; + state[2][0] = w20; - state[0][1] = w11; - state[1][1] = w21; + state[0][1] = y01; + state[1][1] = w11; + state[2][1] = w21; - state[0][2] = w12; - state[1][2] = w22; + state[0][2] = y02; + state[1][2] = w12; + state[2][2] = w22; - state[0][3] = w13; - state[1][3] = w23; + state[0][3] = y03; + state[1][3] = w13; + state[2][3] = w23; + + state[1][4] = w14; + state[2][4] = w24; + + state[1][5] = w15; + state[2][5] = w25; + + state[1][6] = w16; + state[2][6] = w26; + + state[1][7] = w17; + state[2][7] = w27; } // crossfade 4 inputs into 2 outputs with accumulation (interleaved) @@ -468,9 +682,63 @@ static void ThiranBiquad(float f, float& b0, float& b1, float& b2, float& a1, fl b2 = 1.0f; } -// compute new filters for a given azimuth and gain -static void setAzimuthAndGain(float firCoef[4][HRTF_TAPS], float bqCoef[5][4], int delay[4], - int index, float azimuth, float gain, int channel) { +// split x into exponent and fraction (0.0f to 1.0f) +static void splitf(float x, int& expn, float& frac) { + + union { float f; int i; } mant, bits = { x }; + const int IEEE754_MANT_BITS = 23; + const int IEEE754_EXPN_BIAS = 127; + + mant.i = bits.i & ((1 << IEEE754_MANT_BITS) - 1); + mant.i |= (IEEE754_EXPN_BIAS << IEEE754_MANT_BITS); + + frac = mant.f - 1.0f; + expn = (bits.i >> IEEE754_MANT_BITS) - IEEE754_EXPN_BIAS; +} + +static void distanceBiquad(float distance, float& b0, float& b1, float& b2, float& a1, float& a2) { + + // + // Computed from a lookup table quantized to distance = 2^(N/4) + // and reconstructed by piecewise linear interpolation. + // Approximation error < 0.25dB + // + + float x = distance; + x = MIN(MAX(x, 1.0f), 1<<30); + x *= x; + x *= x; // x = distance^4 + + // split x into e and frac, such that x = 2^(e+0) + frac * (2^(e+1) - 2^(e+0)) + int e; + float frac; + splitf(x, e, frac); + + // clamp to table limits + if (e < 0) { + e = 0; + frac = 0.0f; + } + if (e > NLOWPASS-2) { + e = NLOWPASS-2; + frac = 1.0f; + } + assert(frac >= 0.0f); + assert(frac <= 1.0f); + assert(e+0 >= 0); + assert(e+1 < NLOWPASS); + + // piecewise linear interpolation + b0 = lowpassTable[e+0][0] + frac * (lowpassTable[e+1][0] - lowpassTable[e+0][0]); + b1 = lowpassTable[e+0][1] + frac * (lowpassTable[e+1][1] - lowpassTable[e+0][1]); + b2 = lowpassTable[e+0][2] + frac * (lowpassTable[e+1][2] - lowpassTable[e+0][2]); + a1 = lowpassTable[e+0][3] + frac * (lowpassTable[e+1][3] - lowpassTable[e+0][3]); + a2 = lowpassTable[e+0][4] + frac * (lowpassTable[e+1][4] - lowpassTable[e+0][4]); +} + +// compute new filters for a given azimuth, distance and gain +static void setFilters(float firCoef[4][HRTF_TAPS], float bqCoef[5][8], int delay[4], + int index, float azimuth, float distance, float gain, int channel) { // convert from radians to table units azimuth *= HRTF_AZIMUTHS / TWOPI; @@ -551,9 +819,26 @@ static void setAzimuthAndGain(float firCoef[4][HRTF_TAPS], float bqCoef[5][4], i bqCoef[4][channel+1] = a2; delay[channel+1] = itdi; } + + // + // Second biquad implements the distance filter. + // + distanceBiquad(distance, b0, b1, b2, a1, a2); + + bqCoef[0][channel+4] = b0; + bqCoef[1][channel+4] = b1; + bqCoef[2][channel+4] = b2; + bqCoef[3][channel+4] = a1; + bqCoef[4][channel+4] = a2; + + bqCoef[0][channel+5] = b0; + bqCoef[1][channel+5] = b1; + bqCoef[2][channel+5] = b2; + bqCoef[3][channel+5] = a1; + bqCoef[4][channel+5] = a2; } -void AudioHRTF::render(int16_t* input, float* output, int index, float azimuth, float gain, int numFrames) { +void AudioHRTF::render(int16_t* input, float* output, int index, float azimuth, float distance, float gain, int numFrames) { assert(index >= 0); assert(index < HRTF_TABLES); @@ -562,18 +847,19 @@ void AudioHRTF::render(int16_t* input, float* output, int index, float azimuth, float in[HRTF_TAPS + HRTF_BLOCK]; // mono float firCoef[4][HRTF_TAPS]; // 4-channel float firBuffer[4][HRTF_DELAY + HRTF_BLOCK]; // 4-channel - float bqCoef[5][4]; // 4-channel (interleaved) + float bqCoef[5][8]; // 4-channel (interleaved) float bqBuffer[4 * HRTF_BLOCK]; // 4-channel (interleaved) int delay[4]; // 4-channel (interleaved) // to avoid polluting the cache, old filters are recomputed instead of stored - setAzimuthAndGain(firCoef, bqCoef, delay, index, _azimuthState, _gainState, L0); + setFilters(firCoef, bqCoef, delay, index, _azimuthState, _distanceState, _gainState, L0); // compute new filters - setAzimuthAndGain(firCoef, bqCoef, delay, index, azimuth, gain, L1); + setFilters(firCoef, bqCoef, delay, index, azimuth, distance, gain, L1); // new parameters become old _azimuthState = azimuth; + _distanceState = distance; _gainState = gain; // convert mono input to float @@ -611,14 +897,25 @@ void AudioHRTF::render(int16_t* input, float* output, int index, float azimuth, &firBuffer[R1][HRTF_DELAY] - delay[R1], bqBuffer, HRTF_BLOCK); - // process old/new fractional delay - biquad_4x4(bqBuffer, bqBuffer, bqCoef, _bqState, HRTF_BLOCK); + // process old/new biquads + biquad2_4x4(bqBuffer, bqBuffer, bqCoef, _bqState, HRTF_BLOCK); // new state becomes old _bqState[0][L0] = _bqState[0][L1]; _bqState[1][L0] = _bqState[1][L1]; + _bqState[2][L0] = _bqState[2][L1]; + _bqState[0][R0] = _bqState[0][R1]; _bqState[1][R0] = _bqState[1][R1]; + _bqState[2][R0] = _bqState[2][R1]; + + _bqState[0][L2] = _bqState[0][L3]; + _bqState[1][L2] = _bqState[1][L3]; + _bqState[2][L2] = _bqState[2][L3]; + + _bqState[0][R2] = _bqState[0][R3]; + _bqState[1][R2] = _bqState[1][R3]; + _bqState[2][R2] = _bqState[2][R3]; // crossfade old/new output and accumulate crossfade_4x2(bqBuffer, output, crossfadeTable, HRTF_BLOCK); @@ -626,15 +923,16 @@ void AudioHRTF::render(int16_t* input, float* output, int index, float azimuth, _silentState = false; } -void AudioHRTF::renderSilent(int16_t* input, float* output, int index, float azimuth, float gain, int numFrames) { +void AudioHRTF::renderSilent(int16_t* input, float* output, int index, float azimuth, float distance, float gain, int numFrames) { // process the first silent block, to flush internal state if (!_silentState) { - render(input, output, index, azimuth, gain, numFrames); + render(input, output, index, azimuth, distance, gain, numFrames); } // new parameters become old _azimuthState = azimuth; + _distanceState = distance; _gainState = gain; _silentState = true; diff --git a/libraries/audio/src/AudioHRTF.h b/libraries/audio/src/AudioHRTF.h index d3b6237d0c..63d1712980 100644 --- a/libraries/audio/src/AudioHRTF.h +++ b/libraries/audio/src/AudioHRTF.h @@ -21,7 +21,7 @@ static const int HRTF_TABLES = 25; // number of HRTF subjects static const int HRTF_DELAY = 24; // max ITD in samples (1.0ms at 24KHz) static const int HRTF_BLOCK = 256; // block processing size -static const float HRTF_GAIN = 0.5f; // HRTF global gain adjustment +static const float HRTF_GAIN = 1.0f; // HRTF global gain adjustment class AudioHRTF { @@ -33,15 +33,16 @@ public: // output: interleaved stereo mix buffer (accumulates into existing output) // index: HRTF subject index // azimuth: clockwise panning angle in radians + // distance: source distance in meters // gain: gain factor for distance attenuation // numFrames: must be HRTF_BLOCK in this version // - void render(int16_t* input, float* output, int index, float azimuth, float gain, int numFrames); + void render(int16_t* input, float* output, int index, float azimuth, float distance, float gain, int numFrames); // // Fast path when input is known to be silent // - void renderSilent(int16_t* input, float* output, int index, float azimuth, float gain, int numFrames); + void renderSilent(int16_t* input, float* output, int index, float azimuth, float distance, float gain, int numFrames); private: AudioHRTF(const AudioHRTF&) = delete; @@ -49,10 +50,10 @@ private: // SIMD channel assignmentS enum Channel { - L0, - R0, - L1, - R1 + L0, R0, + L1, R1, + L2, R2, + L3, R3 }; // For best cache utilization when processing thousands of instances, only @@ -64,11 +65,12 @@ private: // integer delay history float _delayState[4][HRTF_DELAY] = {}; - // fractional delay history - float _bqState[2][4] = {}; + // biquad history + float _bqState[3][8] = {}; // parameter history float _azimuthState = 0.0f; + float _distanceState = 0.0f; float _gainState = 0.0f; bool _silentState = false;