michael@0: /* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
michael@0: /* vim:set ts=4 sw=4 sts=4 et cindent: */
michael@0: /*
michael@0:  * Copyright (C) 2010 Google Inc. All rights reserved.
michael@0:  *
michael@0:  * Redistribution and use in source and binary forms, with or without
michael@0:  * modification, are permitted provided that the following conditions
michael@0:  * are met:
michael@0:  *
michael@0:  * 1.  Redistributions of source code must retain the above copyright
michael@0:  *     notice, this list of conditions and the following disclaimer.
michael@0:  * 2.  Redistributions in binary form must reproduce the above copyright
michael@0:  *     notice, this list of conditions and the following disclaimer in the
michael@0:  *     documentation and/or other materials provided with the distribution.
michael@0:  * 3.  Neither the name of Apple Computer, Inc. ("Apple") nor the names of
michael@0:  *     its contributors may be used to endorse or promote products derived
michael@0:  *     from this software without specific prior written permission.
michael@0:  *
michael@0:  * THIS SOFTWARE IS PROVIDED BY APPLE AND ITS CONTRIBUTORS "AS IS" AND ANY
michael@0:  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
michael@0:  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
michael@0:  * DISCLAIMED. IN NO EVENT SHALL APPLE OR ITS CONTRIBUTORS BE LIABLE FOR ANY
michael@0:  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
michael@0:  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
michael@0:  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
michael@0:  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
michael@0:  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
michael@0:  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
michael@0:  */
michael@0: 
michael@0: #include "FFTBlock.h"
michael@0: 
michael@0: #include <complex>
michael@0: 
michael@0: namespace mozilla {
michael@0: 
michael@0: typedef std::complex<double> Complex;
michael@0: 
michael@0: FFTBlock* FFTBlock::CreateInterpolatedBlock(const FFTBlock& block0, const FFTBlock& block1, double interp)
michael@0: {
michael@0:     FFTBlock* newBlock = new FFTBlock(block0.FFTSize());
michael@0: 
michael@0:     newBlock->InterpolateFrequencyComponents(block0, block1, interp);
michael@0: 
michael@0:     // In the time-domain, the 2nd half of the response must be zero, to avoid circular convolution aliasing...
michael@0:     int fftSize = newBlock->FFTSize();
michael@0:     nsTArray<float> buffer;
michael@0:     buffer.SetLength(fftSize);
michael@0:     newBlock->GetInverseWithoutScaling(buffer.Elements());
michael@0:     AudioBufferInPlaceScale(buffer.Elements(), 1.0f / fftSize, fftSize / 2);
michael@0:     PodZero(buffer.Elements() + fftSize / 2, fftSize / 2);
michael@0: 
michael@0:     // Put back into frequency domain.
michael@0:     newBlock->PerformFFT(buffer.Elements());
michael@0: 
michael@0:     return newBlock;
michael@0: }
michael@0: 
michael@0: void FFTBlock::InterpolateFrequencyComponents(const FFTBlock& block0, const FFTBlock& block1, double interp)
michael@0: {
michael@0:     // FIXME : with some work, this method could be optimized
michael@0: 
michael@0:     kiss_fft_cpx* dft = mOutputBuffer.Elements();
michael@0: 
michael@0:     const kiss_fft_cpx* dft1 = block0.mOutputBuffer.Elements();
michael@0:     const kiss_fft_cpx* dft2 = block1.mOutputBuffer.Elements();
michael@0: 
michael@0:     MOZ_ASSERT(mFFTSize == block0.FFTSize());
michael@0:     MOZ_ASSERT(mFFTSize == block1.FFTSize());
michael@0:     double s1base = (1.0 - interp);
michael@0:     double s2base = interp;
michael@0: 
michael@0:     double phaseAccum = 0.0;
michael@0:     double lastPhase1 = 0.0;
michael@0:     double lastPhase2 = 0.0;
michael@0: 
michael@0:     int n = mFFTSize / 2;
michael@0: 
michael@0:     dft[0].r = static_cast<float>(s1base * dft1[0].r + s2base * dft2[0].r);
michael@0:     dft[n].r = static_cast<float>(s1base * dft1[n].r + s2base * dft2[n].r);
michael@0: 
michael@0:     for (int i = 1; i < n; ++i) {
michael@0:         Complex c1(dft1[i].r, dft1[i].i);
michael@0:         Complex c2(dft2[i].r, dft2[i].i);
michael@0: 
michael@0:         double mag1 = abs(c1);
michael@0:         double mag2 = abs(c2);
michael@0: 
michael@0:         // Interpolate magnitudes in decibels
michael@0:         double mag1db = 20.0 * log10(mag1);
michael@0:         double mag2db = 20.0 * log10(mag2);
michael@0: 
michael@0:         double s1 = s1base;
michael@0:         double s2 = s2base;
michael@0: 
michael@0:         double magdbdiff = mag1db - mag2db;
michael@0: 
michael@0:         // Empirical tweak to retain higher-frequency zeroes
michael@0:         double threshold =  (i > 16) ? 5.0 : 2.0;
michael@0: 
michael@0:         if (magdbdiff < -threshold && mag1db < 0.0) {
michael@0:             s1 = pow(s1, 0.75);
michael@0:             s2 = 1.0 - s1;
michael@0:         } else if (magdbdiff > threshold && mag2db < 0.0) {
michael@0:             s2 = pow(s2, 0.75);
michael@0:             s1 = 1.0 - s2;
michael@0:         }
michael@0: 
michael@0:         // Average magnitude by decibels instead of linearly
michael@0:         double magdb = s1 * mag1db + s2 * mag2db;
michael@0:         double mag = pow(10.0, 0.05 * magdb);
michael@0: 
michael@0:         // Now, deal with phase
michael@0:         double phase1 = arg(c1);
michael@0:         double phase2 = arg(c2);
michael@0: 
michael@0:         double deltaPhase1 = phase1 - lastPhase1;
michael@0:         double deltaPhase2 = phase2 - lastPhase2;
michael@0:         lastPhase1 = phase1;
michael@0:         lastPhase2 = phase2;
michael@0: 
michael@0:         // Unwrap phase deltas
michael@0:         if (deltaPhase1 > M_PI)
michael@0:             deltaPhase1 -= 2.0 * M_PI;
michael@0:         if (deltaPhase1 < -M_PI)
michael@0:             deltaPhase1 += 2.0 * M_PI;
michael@0:         if (deltaPhase2 > M_PI)
michael@0:             deltaPhase2 -= 2.0 * M_PI;
michael@0:         if (deltaPhase2 < -M_PI)
michael@0:             deltaPhase2 += 2.0 * M_PI;
michael@0: 
michael@0:         // Blend group-delays
michael@0:         double deltaPhaseBlend;
michael@0: 
michael@0:         if (deltaPhase1 - deltaPhase2 > M_PI)
michael@0:             deltaPhaseBlend = s1 * deltaPhase1 + s2 * (2.0 * M_PI + deltaPhase2);
michael@0:         else if (deltaPhase2 - deltaPhase1 > M_PI)
michael@0:             deltaPhaseBlend = s1 * (2.0 * M_PI + deltaPhase1) + s2 * deltaPhase2;
michael@0:         else
michael@0:             deltaPhaseBlend = s1 * deltaPhase1 + s2 * deltaPhase2;
michael@0: 
michael@0:         phaseAccum += deltaPhaseBlend;
michael@0: 
michael@0:         // Unwrap
michael@0:         if (phaseAccum > M_PI)
michael@0:             phaseAccum -= 2.0 * M_PI;
michael@0:         if (phaseAccum < -M_PI)
michael@0:             phaseAccum += 2.0 * M_PI;
michael@0: 
michael@0:         dft[i].r = static_cast<float>(mag * cos(phaseAccum));
michael@0:         dft[i].i = static_cast<float>(mag * sin(phaseAccum));
michael@0:     }
michael@0: }
michael@0: 
michael@0: double FFTBlock::ExtractAverageGroupDelay()
michael@0: {
michael@0:     kiss_fft_cpx* dft = mOutputBuffer.Elements();
michael@0: 
michael@0:     double aveSum = 0.0;
michael@0:     double weightSum = 0.0;
michael@0:     double lastPhase = 0.0;
michael@0: 
michael@0:     int halfSize = FFTSize() / 2;
michael@0: 
michael@0:     const double kSamplePhaseDelay = (2.0 * M_PI) / double(FFTSize());
michael@0: 
michael@0:     // Remove DC offset
michael@0:     dft[0].r = 0.0f;
michael@0: 
michael@0:     // Calculate weighted average group delay
michael@0:     for (int i = 1; i < halfSize; i++) {
michael@0:         Complex c(dft[i].r, dft[i].i);
michael@0:         double mag = abs(c);
michael@0:         double phase = arg(c);
michael@0: 
michael@0:         double deltaPhase = phase - lastPhase;
michael@0:         lastPhase = phase;
michael@0: 
michael@0:         // Unwrap
michael@0:         if (deltaPhase < -M_PI)
michael@0:             deltaPhase += 2.0 * M_PI;
michael@0:         if (deltaPhase > M_PI)
michael@0:             deltaPhase -= 2.0 * M_PI;
michael@0: 
michael@0:         aveSum += mag * deltaPhase;
michael@0:         weightSum += mag;
michael@0:     }
michael@0: 
michael@0:     // Note how we invert the phase delta wrt frequency since this is how group delay is defined
michael@0:     double ave = aveSum / weightSum;
michael@0:     double aveSampleDelay = -ave / kSamplePhaseDelay;
michael@0: 
michael@0:     // Leave 20 sample headroom (for leading edge of impulse)
michael@0:     aveSampleDelay -= 20.0;
michael@0:     if (aveSampleDelay <= 0.0)
michael@0:         return 0.0;
michael@0: 
michael@0:     // Remove average group delay (minus 20 samples for headroom)
michael@0:     AddConstantGroupDelay(-aveSampleDelay);
michael@0: 
michael@0:     return aveSampleDelay;
michael@0: }
michael@0: 
michael@0: void FFTBlock::AddConstantGroupDelay(double sampleFrameDelay)
michael@0: {
michael@0:     int halfSize = FFTSize() / 2;
michael@0: 
michael@0:     kiss_fft_cpx* dft = mOutputBuffer.Elements();
michael@0: 
michael@0:     const double kSamplePhaseDelay = (2.0 * M_PI) / double(FFTSize());
michael@0: 
michael@0:     double phaseAdj = -sampleFrameDelay * kSamplePhaseDelay;
michael@0: 
michael@0:     // Add constant group delay
michael@0:     for (int i = 1; i < halfSize; i++) {
michael@0:         Complex c(dft[i].r, dft[i].i);
michael@0:         double mag = abs(c);
michael@0:         double phase = arg(c);
michael@0: 
michael@0:         phase += i * phaseAdj;
michael@0: 
michael@0:         dft[i].r = static_cast<float>(mag * cos(phase));
michael@0:         dft[i].i = static_cast<float>(mag * sin(phase));
michael@0:     }
michael@0: }
michael@0: 
michael@0: } // namespace mozilla