dspop.cc

各种工程计算的库函数

/*

    DSP operations
    Copyright (C) 1998-2005 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>
#include <limits.h>
#ifdef USE_INTEL_MATH
    #include <mathimf.h>
#else
    #include <math.h>
#endif
#include <float.h>
#ifdef DSP_IPP
    #include <ipp.h>
#endif
#ifdef __INTEL_COMPILER
    #include <xmmintrin.h>
    #include <pmmintrin.h>
#endif

#include "Compilers.hh"
#include "dsp/DSPOp.hh"
#ifdef DSP_X86
#include "dsp/X86.h"
#endif
#if (defined(DSP_X86_64) && (!defined(DSP_X86)))
#include "dsp/X86-64.h"
#endif


#ifdef DSP_X86
bool bHave3DNow = false;
bool bHaveSSE = false;
#endif


extern "C"
{

INLINE int FloatCompare (const void *vpValue1, const void *vpValue2)
{
    if (*(float *)vpValue1 == *(float *)vpValue2)
    {
        return 0;
    }
    if (*(float *)vpValue1 < *(float *)vpValue2)
    {
        return -1;
    }
    else
    {
        return 1;
    }
}


INLINE int DoubleCompare (const void *vpValue1, const void *vpValue2)
{
    if (*(double *)vpValue1 == *(double *)vpValue2)
    {
        return 0;
    }
    if (*(double *)vpValue1 < *(double *)vpValue2)
    {
        return -1;
    }
    else
    {
        return 1;
    }
}


INLINE int LongCompare (const void *vpValue1, const void *vpValue2)
{
    if (*(long *)vpValue1 == *(long *)vpValue2)
    {
        return 0;
    }
    if (*(long *)vpValue1 < *(long *)vpValue2)
    {
        return -1;
    }
    else
    {
        return 1;
    }
}

}


signed long clDSPOp::Round (float fValue)
{
    signed long slTemp;

    // nearbyintf() seems to be buggy in some glibc versions
    #if defined(USE_INTEL_MATH)
    slTemp = lrintf(fValue);
    #elif defined(_ISOC9X_SOURCE)
    slTemp = (signed long) nearbyintf(fValue);
    #else
    slTemp = (fValue >= 0.0F) ?
        ((signed long) (fValue + 0.5F)) : ((signed long) (fValue - 0.5F));
    #endif
    return slTemp;
}


signed long clDSPOp::Round (double dValue)
{
    signed long slTemp;

    // nearbyint() seems to be buggy in some glibc versions
    #if defined(USE_INTEL_MATH)
    slTemp = lrint(dValue);
    #elif defined(_ISOC9X_SOURCE)
    slTemp = (signed long) nearbyint(dValue);
    #else
    slTemp = (dValue >= 0.0) ?
        ((signed long) (dValue + 0.5)) : ((signed long) (dValue - 0.5));
    #endif
    return slTemp;
}


INLINE void clDSPOp::Cart2Polar (float *fpMagn, float *fpPhase,
    float fReal, float fImag)
{
    #ifndef _ISOC9X_SOURCE
    *fpMagn = sqrtf(fReal * fReal + fImag * fImag);
    #else
    *fpMagn = hypotf(fReal, fImag);
    #endif
    *fpPhase = atan2f(fImag, fReal);
}


INLINE void clDSPOp::Cart2Polar (double *dpMagn, double *dpPhase,
    double dReal, double dImag)
{
    #ifndef _ISOC9X_SOURCE
    *dpMagn = sqrt(dReal * dReal + dImag * dImag);
    #else
    *dpMagn = hypot(dReal, dImag);
    #endif
    *dpPhase = atan2(dImag, dReal);
}


INLINE void clDSPOp::Cart2Polar (float *fpMagn, float *fpPhase,
    const stpSCplx spCplx)
{
    #ifndef _ISOC9X_SOURCE
    *fpMagn = sqrtf(spCplx->R * spCplx->R + spCplx->I * spCplx->I);
    #else
    *fpMagn = hypotf(spCplx->R, spCplx->I);
    #endif
    *fpPhase = atan2f(spCplx->I, spCplx->R);
}


INLINE void clDSPOp::Cart2Polar (double *dpMagn, double *dpPhase,
    const stpDCplx spCplx)
{
    #ifndef _ISOC9X_SOURCE
    *dpMagn = sqrt(spCplx->R * spCplx->R + spCplx->I * spCplx->I);
    #else
    *dpMagn = hypot(spCplx->R, spCplx->I);
    #endif
    *dpPhase = atan2(spCplx->I, spCplx->R);
}


INLINE void clDSPOp::Cart2Polar (stpSPolar spPolar, const stpSCplx spCplx)
{
    #ifndef _ISOC9X_SOURCE
    spPolar->M = sqrtf(spCplx->R * spCplx->R + spCplx->I * spCplx->I);
    #else
    spPolar->M = hypotf(spCplx->R, spCplx->I);
    #endif
    spPolar->P = atan2f(spCplx->I, spCplx->R);
}


INLINE void clDSPOp::Cart2Polar (stpDPolar spPolar, const stpDCplx spCplx)
{
    #ifndef _ISOC9X_SOURCE
    spPolar->M = sqrt(spCplx->R * spCplx->R + spCplx->I * spCplx->I);
    #else
    spPolar->M = hypot(spCplx->R, spCplx->I);
    #endif
    spPolar->P = atan2(spCplx->I, spCplx->R);
}


INLINE void clDSPOp::Cart2Polar (utpSCoord upCoord)
{
    #ifndef _ISOC9X_SOURCE
    upCoord->P.M = sqrtf(upCoord->C.R * upCoord->C.R +
        upCoord->C.I * upCoord->C.I);
    #else
    upCoord->P.M = hypotf(upCoord->C.R, upCoord->C.I);
    #endif
    upCoord->P.P = atan2f(upCoord->C.I, upCoord->C.R);
}


INLINE void clDSPOp::Cart2Polar (utpDCoord upCoord)
{
    #ifndef _ISOC9X_SOURCE
    upCoord->P.M = sqrt(upCoord->C.R * upCoord->C.R +
        upCoord->C.I * upCoord->C.I);
    #else
    upCoord->P.M = hypot(upCoord->C.R, upCoord->C.I);
    #endif
    upCoord->P.P = atan2(upCoord->C.I, upCoord->C.R);
}


INLINE void clDSPOp::Polar2Cart (float *fpReal, float *fpImag,
    float fMagn, float fPhase)
{
    #ifndef _GNU_SOURCE
    *fpReal = fMagn * cosf(fPhase);
    *fpImag = fMagn * sinf(fPhase);
    #else
    sincosf(fPhase, fpImag, fpReal);
    *fpReal *= fMagn;
    *fpImag *= fMagn;
    #endif
}


INLINE void clDSPOp::Polar2Cart (double *dpReal, double *dpImag,
    double dMagn, double dPhase)
{
    #ifndef _GNU_SOURCE
    *dpReal = dMagn * cos(dPhase);
    *dpImag = dMagn * sin(dPhase);
    #else
    sincos(dPhase, dpImag, dpReal);
    *dpReal *= dMagn;
    *dpImag *= dMagn;
    #endif
}


INLINE void clDSPOp::Polar2Cart (stpSCplx spCplx, float fMagn, float fPhase)
{
    #ifndef _GNU_SOURCE
    spCplx->R = fMagn * cosf(fPhase);
    spCplx->I = fMagn * sinf(fPhase);
    #else
    sincosf(fPhase, &spCplx->I, &spCplx->R);
    spCplx->R *= fMagn;
    spCplx->I *= fMagn;
    #endif
}


INLINE void clDSPOp::Polar2Cart (stpDCplx spCplx, double dMagn, double dPhase)
{
    #ifndef _GNU_SOURCE
    spCplx->R = dMagn * cos(dPhase);
    spCplx->I = dMagn * sin(dPhase);
    #else
    sincos(dPhase, &spCplx->I, &spCplx->R);
    spCplx->R *= dMagn;
    spCplx->I *= dMagn;
    #endif
}


INLINE void clDSPOp::Polar2Cart (stpSCplx spCplx, const stpSPolar spPolar)
{
    #ifndef _GNU_SOURCE
    spCplx->R = spPolar->M * cosf(spPolar->P);
    spCplx->I = spPolar->M * sinf(spPolar->P);
    #else
    sincosf(spPolar->P, &spCplx->I, &spCplx->R);
    spCplx->R *= spPolar->M;
    spCplx->I *= spPolar->M;
    #endif
}


INLINE void clDSPOp::Polar2Cart (stpDCplx spCplx, const stpDPolar spPolar)
{
    #ifndef _GNU_SOURCE
    spCplx->R = spPolar->M * cos(spPolar->P);
    spCplx->I = spPolar->M * sin(spPolar->P);
    #else
    sincos(spPolar->P, &spCplx->I, &spCplx->R);
    spCplx->R *= spPolar->M;
    spCplx->I *= spPolar->M;
    #endif
}


INLINE void clDSPOp::Polar2Cart (utpSCoord upCoord)
{
    #ifndef _GNU_SOURCE
    upCoord->C.R = upCoord->P.M * cosf(upCoord->P.P);
    upCoord->C.I = upCoord->P.M * sinf(upCoord->P.P);
    #else
    float fReal;
    float fImag;

    sincosf(upCoord->P.P, &fImag, &fReal);
    upCoord->C.R = upCoord->P.M * fReal;
    upCoord->C.I = upCoord->P.M * fImag;
    #endif
}


INLINE void clDSPOp::Polar2Cart (utpDCoord upCoord)
{
    #ifndef _GNU_SOURCE
    upCoord->C.R = upCoord->P.M * cos(upCoord->P.P);
    upCoord->C.I = upCoord->P.M * sin(upCoord->P.P);
    #else
    double dReal;
    double dImag;

    sincos(upCoord->P.P, &dImag, &dReal);
    upCoord->C.R = upCoord->P.M * dReal;
    upCoord->C.I = upCoord->P.M * dImag;
    #endif
}


INLINE void clDSPOp::CplxAdd (stpSCplx spCplxDest, const stpSCplx spCplxSrc)
{
    spCplxDest->R += spCplxSrc->R;
    spCplxDest->I += spCplxSrc->I;
}


INLINE void clDSPOp::CplxAdd (stpDCplx spCplxDest, const stpDCplx spCplxSrc)
{
    spCplxDest->R += spCplxSrc->R;
    spCplxDest->I += spCplxSrc->I;
}


INLINE void clDSPOp::CplxAdd (stpSCplx spCplxDest, const stpSCplx spCplxSrc1,
    const stpSCplx spCplxSrc2)
{
    spCplxDest->R = spCplxSrc1->R + spCplxSrc2->R;
    spCplxDest->I = spCplxSrc1->I + spCplxSrc2->I;
}


INLINE void clDSPOp::CplxAdd (stpDCplx spCplxDest, const stpDCplx spCplxSrc1,
    const stpDCplx spCplxSrc2)
{
    spCplxDest->R = spCplxSrc1->R + spCplxSrc2->R;
    spCplxDest->I = spCplxSrc1->I + spCplxSrc2->I;
}


INLINE void clDSPOp::CplxSub (stpSCplx spCplxDest, const stpSCplx spCplxSrc)
{
    spCplxDest->R -= spCplxSrc->R;
    spCplxDest->I -= spCplxSrc->I;
}


INLINE void clDSPOp::CplxSub (stpDCplx spCplxDest, const stpDCplx spCplxSrc)
{
    spCplxDest->R -= spCplxSrc->R;
    spCplxDest->I -= spCplxSrc->I;
}


INLINE void clDSPOp::CplxSub (stpSCplx spCplxDest, const stpSCplx spCplxSrc1,
    const stpSCplx spCplxSrc2)
{
    spCplxDest->R = spCplxSrc1->R - spCplxSrc2->R;
    spCplxDest->I = spCplxSrc1->I - spCplxSrc2->I;
}


INLINE void clDSPOp::CplxSub (stpDCplx spCplxDest, const stpDCplx spCplxSrc1,
    const stpDCplx spCplxSrc2)
{
    spCplxDest->R = spCplxSrc1->R - spCplxSrc2->R;
    spCplxDest->I = spCplxSrc1->I - spCplxSrc2->I;
}


INLINE void clDSPOp::CplxMul (stpSCplx spCplxDest, float fSrc)
{
    spCplxDest->R *= fSrc;
    spCplxDest->I *= fSrc;
}


INLINE void clDSPOp::CplxMul (stpDCplx spCplxDest, double dSrc)
{
    spCplxDest->R *= dSrc;
    spCplxDest->I *= dSrc;
}


INLINE void clDSPOp::CplxMul (stpSCplx spCplxDest, const stpSCplx spCplxSrc)
{
    float fReal;
    float fImag;

    fReal = spCplxDest->R * spCplxSrc->R - spCplxDest->I * spCplxSrc->I;
    fImag = spCplxDest->R * spCplxSrc->I + spCplxDest->I * spCplxSrc->R;
    spCplxDest->R = fReal;
    spCplxDest->I = fImag;
}


INLINE void clDSPOp::CplxMul (stpDCplx spCplxDest, const stpDCplx spCplxSrc)
{
    double dReal;
    double dImag;

    dReal = spCplxDest->R * spCplxSrc->R - spCplxDest->I * spCplxSrc->I;
    dImag = spCplxDest->R * spCplxSrc->I + spCplxDest->I * spCplxSrc->R;
    spCplxDest->R = dReal;
    spCplxDest->I = dImag;
}


INLINE void clDSPOp::CplxMul (stpSCplx spCplxDest, const stpSCplx spCplxSrc1,
    const stpSCplx spCplxSrc2)
{
    spCplxDest->R = 
        spCplxSrc1->R * spCplxSrc2->R - spCplxSrc1->I * spCplxSrc2->I;
    spCplxDest->I =
        spCplxSrc1->R * spCplxSrc2->I + spCplxSrc1->I * spCplxSrc2->R;
}


INLINE void clDSPOp::CplxMul (stpDCplx spCplxDest, const stpDCplx spCplxSrc1,
    const stpDCplx spCplxSrc2)
{
    spCplxDest->R =
        spCplxSrc1->R * spCplxSrc2->R - spCplxSrc1->I * spCplxSrc2->I;
    spCplxDest->I =
        spCplxSrc1->R * spCplxSrc2->I + spCplxSrc1->I * spCplxSrc2->R;
}


INLINE void clDSPOp::CplxMulC (stpSCplx spCplxDest, const stpSCplx spCplxSrc)
{
    float fReal;
    float fImag;

    fReal = spCplxDest->R * spCplxSrc->R - spCplxDest->I * (-spCplxSrc->I);
    fImag = spCplxDest->R * (-spCplxSrc->I) + spCplxDest->I * spCplxSrc->R;
    spCplxDest->R = fReal;
    spCplxDest->I = fImag;
}


INLINE void clDSPOp::CplxMulC (stpDCplx spCplxDest, const stpDCplx spCplxSrc)
{
    double dReal;
    double dImag;

    dReal = spCplxDest->R * spCplxSrc->R - spCplxDest->I * (-spCplxSrc->I);
    dImag = spCplxDest->R * (-spCplxSrc->I) + spCplxDest->I * spCplxSrc->R;
    spCplxDest->R = dReal;
    spCplxDest->I = dImag;
}


INLINE void clDSPOp::CplxMulC (stpSCplx spCplxDest, const stpSCplx spCplxSrc1,
    const stpSCplx spCplxSrc2)
{
    spCplxDest->R =
        spCplxSrc1->R * spCplxSrc2->R - spCplxSrc1->I * (-spCplxSrc2->I);
    spCplxDest->I =
        spCplxSrc1->R * (-spCplxSrc2->I) + spCplxSrc1->I * spCplxSrc2->R;
}


INLINE void clDSPOp::CplxMulC (stpDCplx spCplxDest, const stpDCplx spCplxSrc1,
    const stpDCplx spCplxSrc2)
{
    spCplxDest->R =
        spCplxSrc1->R * spCplxSrc2->R - spCplxSrc1->I * (-spCplxSrc2->I);
    spCplxDest->I =
        spCplxSrc1->R * (-spCplxSrc2->I) + spCplxSrc1->I * spCplxSrc2->R;
}


INLINE void clDSPOp::CplxDiv (stpSCplx spCplxDest, const stpSCplx spCplxSrc)
{
    float fReal;
    float fImag;

    fReal = (spCplxDest->R * spCplxSrc->R + spCplxDest->I * spCplxSrc->I) /
        (spCplxSrc->R * spCplxSrc->R + spCplxSrc->I * spCplxSrc->I);
    fImag = (spCplxDest->I * spCplxSrc->R - spCplxDest->R * spCplxSrc->I) /
        (spCplxSrc->R * spCplxSrc->R + spCplxSrc->I * spCplxSrc->I);
    spCplxDest->R = fReal;
    spCplxDest->I = fImag;
}


INLINE void clDSPOp::CplxDiv (stpDCplx spCplxDest, const stpDCplx spCplxSrc)
{
    double dReal;
    double dImag;

    dReal = (spCplxDest->R * spCplxSrc->R + spCplxDest->I * spCplxSrc->I) /
        (spCplxSrc->R * spCplxSrc->R + spCplxSrc->I * spCplxSrc->I);
    dImag = (spCplxDest->I * spCplxSrc->R - spCplxDest->R * spCplxSrc->I) /
        (spCplxSrc->R * spCplxSrc->R + spCplxSrc->I * spCplxSrc->I);
    spCplxDest->R = dReal;
    spCplxDest->I = dImag;
}


INLINE void clDSPOp::CplxDiv (stpSCplx spCplxDest, const stpSCplx spCplxSrc1,
    const stpSCplx spCplxSrc2)
{
    spCplxDest->R =
        (spCplxSrc1->R * spCplxSrc2->R + spCplxSrc1->I * spCplxSrc2->I) /
        (spCplxSrc2->R * spCplxSrc2->R + spCplxSrc2->I * spCplxSrc2->I);
    spCplxDest->I =
        (spCplxSrc1->I * spCplxSrc2->R - spCplxSrc1->R * spCplxSrc2->I) /
        (spCplxSrc2->R * spCplxSrc2->R + spCplxSrc2->I * spCplxSrc2->I);
}


INLINE void clDSPOp::CplxDiv (stpDCplx spCplxDest, const stpDCplx spCplxSrc1,
    const stpDCplx spCplxSrc2)
{
    spCplxDest->R =
        (spCplxSrc1->R * spCplxSrc2->R + spCplxSrc1->I * spCplxSrc2->I) /
        (spCplxSrc2->R * spCplxSrc2->R + spCplxSrc2->I * spCplxSrc2->I);
    spCplxDest->I =
        (spCplxSrc1->I * spCplxSrc2->R - spCplxSrc1->R * spCplxSrc2->I) /
        (spCplxSrc2->R * spCplxSrc2->R + spCplxSrc2->I * spCplxSrc2->I);
}


INLINE void clDSPOp::CplxExp (stpSCplx spCplxDest, const stpSCplx spCplxSrc)
{
    float fRealExp;

    fRealExp = expf(spCplxSrc->R);
    #ifndef _GNU_SOURCE
    spCplxDest->R = fRealExp * cosf(spCplxSrc->I);
    spCplxDest->I = fRealExp * sinf(spCplxSrc->I);
    #else
    sincosf(spCplxSrc->I, &spCplxDest->I, &spCplxDest->R);
    spCplxDest->R *= fRealExp;
    spCplxDest->I *= fRealExp;
    #endif
}


INLINE void clDSPOp::CplxExp (stpDCplx spCplxDest, const stpDCplx spCplxSrc)
{
    double dRealExp;