michael@0: ////////////////////////////////////////////////////////////////////////////////
michael@0: ///
michael@0: /// FIR low-pass (anti-alias) filter with filter coefficient design routine and
michael@0: /// MMX optimization. 
michael@0: /// 
michael@0: /// Anti-alias filter is used to prevent folding of high frequencies when 
michael@0: /// transposing the sample rate with interpolation.
michael@0: ///
michael@0: /// Author        : Copyright (c) Olli Parviainen
michael@0: /// Author e-mail : oparviai 'at' iki.fi
michael@0: /// SoundTouch WWW: http://www.surina.net/soundtouch
michael@0: ///
michael@0: ////////////////////////////////////////////////////////////////////////////////
michael@0: //
michael@0: // Last changed  : $Date: 2014-01-05 15:40:22 -0600 (Sun, 05 Jan 2014) $
michael@0: // File revision : $Revision: 4 $
michael@0: //
michael@0: // $Id: AAFilter.cpp 177 2014-01-05 21:40:22Z oparviai $
michael@0: //
michael@0: ////////////////////////////////////////////////////////////////////////////////
michael@0: //
michael@0: // License :
michael@0: //
michael@0: //  SoundTouch audio processing library
michael@0: //  Copyright (c) Olli Parviainen
michael@0: //
michael@0: //  This library is free software; you can redistribute it and/or
michael@0: //  modify it under the terms of the GNU Lesser General Public
michael@0: //  License as published by the Free Software Foundation; either
michael@0: //  version 2.1 of the License, or (at your option) any later version.
michael@0: //
michael@0: //  This library is distributed in the hope that it will be useful,
michael@0: //  but WITHOUT ANY WARRANTY; without even the implied warranty of
michael@0: //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
michael@0: //  Lesser General Public License for more details.
michael@0: //
michael@0: //  You should have received a copy of the GNU Lesser General Public
michael@0: //  License along with this library; if not, write to the Free Software
michael@0: //  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
michael@0: //
michael@0: ////////////////////////////////////////////////////////////////////////////////
michael@0: 
michael@0: #include <memory.h>
michael@0: #include <assert.h>
michael@0: #include <math.h>
michael@0: #include <stdlib.h>
michael@0: #include "AAFilter.h"
michael@0: #include "FIRFilter.h"
michael@0: 
michael@0: using namespace soundtouch;
michael@0: 
michael@0: #define PI        3.141592655357989
michael@0: #define TWOPI    (2 * PI)
michael@0: 
michael@0: // define this to save AA filter coefficients to a file
michael@0: // #define _DEBUG_SAVE_AAFILTER_COEFFICIENTS   1
michael@0: 
michael@0: #ifdef _DEBUG_SAVE_AAFILTER_COEFFICIENTS
michael@0:     #include <stdio.h>
michael@0: 
michael@0:     static void _DEBUG_SAVE_AAFIR_COEFFS(SAMPLETYPE *coeffs, int len)
michael@0:     {
michael@0:         FILE *fptr = fopen("aa_filter_coeffs.txt", "wt");
michael@0:         if (fptr == NULL) return;
michael@0: 
michael@0:         for (int i = 0; i < len; i ++)
michael@0:         {
michael@0:             double temp = coeffs[i];
michael@0:             fprintf(fptr, "%lf\n", temp);
michael@0:         }
michael@0:         fclose(fptr);
michael@0:     }
michael@0: 
michael@0: #else
michael@0:     #define _DEBUG_SAVE_AAFIR_COEFFS(x, y)
michael@0: #endif
michael@0: 
michael@0: 
michael@0: /*****************************************************************************
michael@0:  *
michael@0:  * Implementation of the class 'AAFilter'
michael@0:  *
michael@0:  *****************************************************************************/
michael@0: 
michael@0: AAFilter::AAFilter(uint len)
michael@0: {
michael@0:     pFIR = FIRFilter::newInstance();
michael@0:     cutoffFreq = 0.5;
michael@0:     setLength(len);
michael@0: }
michael@0: 
michael@0: 
michael@0: 
michael@0: AAFilter::~AAFilter()
michael@0: {
michael@0:     delete pFIR;
michael@0: }
michael@0: 
michael@0: 
michael@0: 
michael@0: // Sets new anti-alias filter cut-off edge frequency, scaled to
michael@0: // sampling frequency (nyquist frequency = 0.5).
michael@0: // The filter will cut frequencies higher than the given frequency.
michael@0: void AAFilter::setCutoffFreq(double newCutoffFreq)
michael@0: {
michael@0:     cutoffFreq = newCutoffFreq;
michael@0:     calculateCoeffs();
michael@0: }
michael@0: 
michael@0: 
michael@0: 
michael@0: // Sets number of FIR filter taps
michael@0: void AAFilter::setLength(uint newLength)
michael@0: {
michael@0:     length = newLength;
michael@0:     calculateCoeffs();
michael@0: }
michael@0: 
michael@0: 
michael@0: 
michael@0: // Calculates coefficients for a low-pass FIR filter using Hamming window
michael@0: void AAFilter::calculateCoeffs()
michael@0: {
michael@0:     uint i;
michael@0:     double cntTemp, temp, tempCoeff,h, w;
michael@0:     double wc;
michael@0:     double scaleCoeff, sum;
michael@0:     double *work;
michael@0:     SAMPLETYPE *coeffs;
michael@0: 
michael@0:     assert(length >= 2);
michael@0:     assert(length % 4 == 0);
michael@0:     assert(cutoffFreq >= 0);
michael@0:     assert(cutoffFreq <= 0.5);
michael@0: 
michael@0:     work = new double[length];
michael@0:     coeffs = new SAMPLETYPE[length];
michael@0: 
michael@0:     wc = 2.0 * PI * cutoffFreq;
michael@0:     tempCoeff = TWOPI / (double)length;
michael@0: 
michael@0:     sum = 0;
michael@0:     for (i = 0; i < length; i ++) 
michael@0:     {
michael@0:         cntTemp = (double)i - (double)(length / 2);
michael@0: 
michael@0:         temp = cntTemp * wc;
michael@0:         if (temp != 0) 
michael@0:         {
michael@0:             h = sin(temp) / temp;                     // sinc function
michael@0:         } 
michael@0:         else 
michael@0:         {
michael@0:             h = 1.0;
michael@0:         }
michael@0:         w = 0.54 + 0.46 * cos(tempCoeff * cntTemp);       // hamming window
michael@0: 
michael@0:         temp = w * h;
michael@0:         work[i] = temp;
michael@0: 
michael@0:         // calc net sum of coefficients 
michael@0:         sum += temp;
michael@0:     }
michael@0: 
michael@0:     // ensure the sum of coefficients is larger than zero
michael@0:     assert(sum > 0);
michael@0: 
michael@0:     // ensure we've really designed a lowpass filter...
michael@0:     assert(work[length/2] > 0);
michael@0:     assert(work[length/2 + 1] > -1e-6);
michael@0:     assert(work[length/2 - 1] > -1e-6);
michael@0: 
michael@0:     // Calculate a scaling coefficient in such a way that the result can be
michael@0:     // divided by 16384
michael@0:     scaleCoeff = 16384.0f / sum;
michael@0: 
michael@0:     for (i = 0; i < length; i ++) 
michael@0:     {
michael@0:         temp = work[i] * scaleCoeff;
michael@0: //#if SOUNDTOUCH_INTEGER_SAMPLES
michael@0:         // scale & round to nearest integer
michael@0:         temp += (temp >= 0) ? 0.5 : -0.5;
michael@0:         // ensure no overfloods
michael@0:         assert(temp >= -32768 && temp <= 32767);
michael@0: //#endif
michael@0:         coeffs[i] = (SAMPLETYPE)temp;
michael@0:     }
michael@0: 
michael@0:     // Set coefficients. Use divide factor 14 => divide result by 2^14 = 16384
michael@0:     pFIR->setCoefficients(coeffs, length, 14);
michael@0: 
michael@0:     _DEBUG_SAVE_AAFIR_COEFFS(coeffs, length);
michael@0: 
michael@0:     delete[] work;
michael@0:     delete[] coeffs;
michael@0: }
michael@0: 
michael@0: 
michael@0: // Applies the filter to the given sequence of samples. 
michael@0: // Note : The amount of outputted samples is by value of 'filter length' 
michael@0: // smaller than the amount of input samples.
michael@0: uint AAFilter::evaluate(SAMPLETYPE *dest, const SAMPLETYPE *src, uint numSamples, uint numChannels) const
michael@0: {
michael@0:     return pFIR->evaluate(dest, src, numSamples, numChannels);
michael@0: }
michael@0: 
michael@0: 
michael@0: /// Applies the filter to the given src & dest pipes, so that processed amount of
michael@0: /// samples get removed from src, and produced amount added to dest 
michael@0: /// Note : The amount of outputted samples is by value of 'filter length' 
michael@0: /// smaller than the amount of input samples.
michael@0: uint AAFilter::evaluate(FIFOSampleBuffer &dest, FIFOSampleBuffer &src) const
michael@0: {
michael@0:     SAMPLETYPE *pdest;
michael@0:     const SAMPLETYPE *psrc;
michael@0:     uint numSrcSamples;
michael@0:     uint result;
michael@0:     int numChannels = src.getChannels();
michael@0: 
michael@0:     assert(numChannels == dest.getChannels());
michael@0: 
michael@0:     numSrcSamples = src.numSamples();
michael@0:     psrc = src.ptrBegin();
michael@0:     pdest = dest.ptrEnd(numSrcSamples);
michael@0:     result = pFIR->evaluate(pdest, psrc, numSrcSamples, numChannels);
michael@0:     src.receiveSamples(result);
michael@0:     dest.putSamples(result);
michael@0: 
michael@0:     return result;
michael@0: }
michael@0: 
michael@0: 
michael@0: uint AAFilter::getLength() const
michael@0: {
michael@0:     return pFIR->getLength();
michael@0: }