filter.cc

各种工程计算的库函数

/*

    FFT filter
    Copyright (C) 1999-2003 Jussi Laako

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

*/


#include <stdio.h>
#include <stdlib.h>
#ifdef USE_INTEL_MATH
    #include <mathimf.h>
#else
    #include <math.h>
#endif
#include <float.h>

#include "dsp/Filter.hh"


void clFilter::InitCoeffsS ()
{
    long lCoeffCntr;
    stpSCplx spSCCoeffs = CCoeffs;

    for (lCoeffCntr = 0; lCoeffCntr < lSpectPoints; lCoeffCntr++)
    {
        spSCCoeffs[lCoeffCntr].R = 1.0f;
        spSCCoeffs[lCoeffCntr].I = 0.0f;
    }
    ReadyFilterS();
}


void clFilter::InitCoeffsD ()
{
    long lCoeffCntr;
    stpDCplx spDCCoeffs = CCoeffs;

    for (lCoeffCntr = 0; lCoeffCntr < lSpectPoints; lCoeffCntr++)
    {
        spDCCoeffs[lCoeffCntr].R = 1.0;
        spDCCoeffs[lCoeffCntr].I = 0.0;
    }
    ReadyFilterD();
}


void clFilter::ReadyFilterS ()
{
    float *fpCoeffWin = CoeffWin;
    float *fpProc = Proc;
    stpSCplx spSCCoeffs = CCoeffs;
    
    fpCoeffWin = CoeffWin;
    IFFTo(fpProc, spSCCoeffs);
    Mul(fpProc, fpCoeffWin, lFFTSize);
    FFTi(spSCCoeffs, fpProc);
}


void clFilter::ReadyFilterD ()
{
    double *dpCoeffWin = CoeffWin;
    double *dpProc = Proc;
    stpDCplx spDCCoeffs = CCoeffs;

    dpCoeffWin = CoeffWin;
    IFFTo(dpProc, spDCCoeffs);
    Mul(dpProc, dpCoeffWin, lFFTSize);
    FFTi(spDCCoeffs, dpProc);
}


float clFilter::GetKaiserBeta (float fAlpha)
{
    if (fAlpha < 21.0f)
    {
        return 0.0f;
    }
    else if (fAlpha >= 21.0f && fAlpha <= 50.0f)
    {
        return (0.5842f * powf(fAlpha - 21.0f, 0.4f) + 
            0.07886f * (fAlpha - 21.0f));
    }
    else
    {
        return (0.1102f * (fAlpha - 8.7f));
    }
}


double clFilter::GetKaiserBeta (double dAlpha)
{
    if (dAlpha < 21.0)
    {
        return 0.0;
    }
    else if (dAlpha >= 21.0 && dAlpha <= 50.0)
    {
        return (0.5842 * pow(dAlpha - 21.0, 0.4) + 
            0.07886 * (dAlpha - 21.0));
    }
    else
    {
        return (0.1102 * (dAlpha - 8.7));
    }
}


clFilter::clFilter ()
{
    bInitialized = false;
}


clFilter::~clFilter ()
{
    if (bInitialized) Uninitialize();
}


bool clFilter::Initialize (long lWindowSize, const float *fpFiltCoeffs,
    float fOverlap, float fBeta, int iSmoothWindow)
{
    float *fpCoeffWin;
    float *fpTemp;
    double *dpTemp2;
    clDSPAlloc Temp;
    clDSPAlloc Temp2;

    if (bInitialized) Uninitialize();
    bInitialized = true;
    lFFTSize = lWindowSize;
    lHalfSize = lFFTSize / 2;
    lOldSize = (long) ((float) lWindowSize * fOverlap + 0.5f);
    lNewSize = lWindowSize - lOldSize;
    lSpectPoints = lFFTSize / 2 + 1;
    Prev.Size(lOldSize * sizeof(float));
    Proc.Size(lFFTSize * sizeof(float));
    CCoeffs.Size(lSpectPoints * sizeof(stSCplx));
    CProc.Size(lSpectPoints * sizeof(stSCplx));
    CoeffWin.Size(lFFTSize * sizeof(float));

    fpTemp = (float *) Temp.Size(lFFTSize * sizeof(float));
    dpTemp2 = (double *) Temp2.Size(lFFTSize * sizeof(double));
    fpCoeffWin = CoeffWin;
    switch (iSmoothWindow)
    {
        case FILTER_SMOOTH_NONE:
            Set(dpTemp2, 1.0, lFFTSize);
            break;
        case FILTER_SMOOTH_KAISER:
            WinKaiser(dpTemp2, (double) fBeta, lFFTSize);
            break;
        case FILTER_SMOOTH_KAISER_BESSEL:
            WinKaiserBessel(dpTemp2, 4.0, lFFTSize);
            break;
        case FILTER_SMOOTH_DOLPH_CHEBYSHEV:
            WinDolphChebyshev(dpTemp2, 1.0 / pow(2.0, 32.0), lFFTSize);
            break;
    }
    Convert(fpTemp, dpTemp2, lFFTSize);
    Copy(fpCoeffWin, &fpTemp[lHalfSize], lHalfSize);
    Copy(&fpCoeffWin[lHalfSize], fpTemp, lHalfSize);

    FFTInitialize(lFFTSize, true);
    if (fpFiltCoeffs != NULL)
    {
        SetCoeffs(fpFiltCoeffs);
    }
    else
    {
        InitCoeffsS();
    }
    Zero((float *) Prev, lOldSize);

    return true;
}


bool clFilter::Initialize (long lWindowSize, const double *dpFiltCoeffs,
    double dOverlap, double dBeta, int iSmoothWindow)
{
    double *dpTemp;
    double *dpCoeffWin;
    clDSPAlloc Temp;

    if (bInitialized) Uninitialize();
    bInitialized = true;
    lFFTSize = lWindowSize;
    lHalfSize = lFFTSize / 2;
    lOldSize = (long) ((double) lWindowSize * dOverlap + 0.5);
    lNewSize = lWindowSize - lOldSize;
    lSpectPoints = lFFTSize / 2 + 1;
    Prev.Size(lOldSize * sizeof(double));
    Proc.Size(lFFTSize * sizeof(double));
    CCoeffs.Size(lSpectPoints * sizeof(stDCplx));
    CProc.Size(lSpectPoints * sizeof(stDCplx));
    CoeffWin.Size(lFFTSize * sizeof(double));

    dpTemp = (double *) Temp.Size(lFFTSize * sizeof(double));
    dpCoeffWin = CoeffWin;
    switch (iSmoothWindow)
    {
        case FILTER_SMOOTH_NONE:
            Set(dpTemp, 1.0, lFFTSize);
            break;
        case FILTER_SMOOTH_KAISER:
            WinKaiser(dpTemp, dBeta, lFFTSize);
            break;
        case FILTER_SMOOTH_KAISER_BESSEL:
            WinKaiserBessel(dpTemp, 4.0, lFFTSize);
            break;
        case FILTER_SMOOTH_DOLPH_CHEBYSHEV:
            WinDolphChebyshev(dpTemp, 1.0 / pow(2.0, 32.0), lFFTSize);
            break;
    }
    Copy(dpCoeffWin, &dpTemp[lHalfSize], lHalfSize);
    Copy(&dpCoeffWin[lHalfSize], dpTemp, lHalfSize);

    FFTInitialize(lFFTSize, true);
    if (dpFiltCoeffs != NULL)
    {
        SetCoeffs(dpFiltCoeffs);
    }
    else
    {
        InitCoeffsD();
    }
    Zero((double *) Prev, lOldSize);

    return true;
}


bool clFilter::InitFloat (long lWindowSize)
{
    float *fpNoCoeffs = NULL;

    return Initialize(lWindowSize, fpNoCoeffs);
}


bool clFilter::InitDouble (long lWindowSize)
{
    double *dpNoCoeffs = NULL;

    return Initialize(lWindowSize, dpNoCoeffs);
}


bool clFilter::InitializeLP (float fPassBand, float fStopBand,
    float fRippleRatio, float fOverlap)
{
    long lWindowSize;
    long lTwosExp;
    float fDeltaOmega;
    float fBeta;
    float fCorner;
    float *fpNullPtr = NULL;
    
    if (fPassBand >= fStopBand) return false;

    fCorner = (fPassBand + fStopBand) * 0.5f;
    fDeltaOmega = fStopBand - fPassBand;
    lTwosExp = (long)
        (logf((fRippleRatio - 8.0f) / (2.285f * fDeltaOmega)) / 
        logf(2.0f) + 0.5f);
    lWindowSize = (long) powf(2.0f, lTwosExp);
    fBeta = GetKaiserBeta(fRippleRatio);
    
    if (!Initialize(lWindowSize, fpNullPtr, fOverlap, fBeta, 
        FILTER_SMOOTH_KAISER)) return false;
    DesignLP(&fCorner);

    return true;
}


bool clFilter::InitializeLP (double dPassBand, double dStopBand,
    double dRippleRatio, double dOverlap)
{
    long lWindowSize;
    long lTwosExp;
    double dDeltaOmega;
    double dBeta;
    double dCorner;
    double *dpNullPtr = NULL;
    
    if (dPassBand >= dStopBand) return false;

    dCorner = (dPassBand + dStopBand) * 0.5;
    dDeltaOmega = dStopBand - dPassBand;
    lTwosExp = (long)
        (log((dRippleRatio - 8.0) / (2.285 * dDeltaOmega)) / 
        log(2.0) + 0.5);
    lWindowSize = (long) pow(2.0, lTwosExp);
    dBeta = GetKaiserBeta(dRippleRatio);
    
    if (!Initialize(lWindowSize, dpNullPtr, dOverlap, dBeta, 
        FILTER_SMOOTH_KAISER)) return false;
    DesignLP(&dCorner);

    return true;
}


bool clFilter::InitializeHP (float fPassBand, float fStopBand,
    float fRippleRatio, float fOverlap)
{
    long lWindowSize;
    long lTwosExp;
    float fDeltaOmega;
    float fBeta;
    float fCorner;
    float *fpNullPtr = NULL;
    
    if (fPassBand <= fStopBand) return false;

    fCorner = (fPassBand + fStopBand) * 0.5f;
    fDeltaOmega = fPassBand - fStopBand;
    lTwosExp = (long)
        (logf((fRippleRatio - 8.0f) / (2.285f * fDeltaOmega)) / 
        logf(2.0f) + 0.5f);
    lWindowSize = (long) powf(2.0f, lTwosExp);
    fBeta = GetKaiserBeta(fRippleRatio);
    
    if (!Initialize(lWindowSize, fpNullPtr, fOverlap, fBeta, 
        FILTER_SMOOTH_KAISER)) return false;
    DesignHP(&fCorner);

    return true;
}


bool clFilter::InitializeHP (double dPassBand, double dStopBand,
    double dRippleRatio, double dOverlap)
{
    long lWindowSize;
    long lTwosExp;
    double dDeltaOmega;
    double dBeta;
    double dCorner;
    double *dpNullPtr = NULL;
    
    if (dPassBand <= dStopBand) return false;

    dCorner = (dPassBand + dStopBand) * 0.5;
    dDeltaOmega = dPassBand - dStopBand;
    lTwosExp = (long)
        (log((dRippleRatio - 8.0) / (2.285 * dDeltaOmega)) / 
        log(2.0) + 0.5);
    lWindowSize = (long) pow(2.0, lTwosExp);
    dBeta = GetKaiserBeta(dRippleRatio);
    
    if (!Initialize(lWindowSize, dpNullPtr, dOverlap, dBeta, 
        FILTER_SMOOTH_KAISER)) return false;
    DesignHP(&dCorner);

    return true;
}


void clFilter::Uninitialize ()
{
    FFTUninitialize();
    CoeffWin.Free();
    Prev.Free();
    Proc.Free();
    CCoeffs.Free();
    CProc.Free();
    InBuf.Clear();
    OutBuf.Clear();
}


void clFilter::SetCoeffs (const float *fpFiltCoeffs)
{
    long lCoeffCntr;
    stpSCplx spSCCoeffs = CCoeffs;

    for (lCoeffCntr = 0; lCoeffCntr < lSpectPoints; lCoeffCntr++)
    {
        spSCCoeffs[lCoeffCntr].R = fpFiltCoeffs[lCoeffCntr];
        spSCCoeffs[lCoeffCntr].I = 0.0f;
    }
    ReadyFilterS();
}


void clFilter::SetCoeffs (const double *dpFiltCoeffs)
{
    long lCoeffCntr;
    stpDCplx spDCCoeffs = CCoeffs;

    for (lCoeffCntr = 0; lCoeffCntr < lSpectPoints; lCoeffCntr++)
    {
        spDCCoeffs[lCoeffCntr].R = dpFiltCoeffs[lCoeffCntr];
        spDCCoeffs[lCoeffCntr].I = 0.0;