171 lines
4.5 KiB
C++
171 lines
4.5 KiB
C++
/******************************************************************************
|
|
* resample.cpp *
|
|
*--------------*
|
|
*
|
|
*------------------------------------------------------------------------------
|
|
* Copyright (C) 1999 Entropic, Inc
|
|
* Copyright (C) 2000 Microsoft Corporation Date: 03/02/00
|
|
* All Rights Reserved
|
|
*
|
|
********************************************************************* PACOG ***/
|
|
|
|
#include "sigprocInt.h"
|
|
|
|
#define FILTER_LEN_2 .005 //(sec)
|
|
|
|
static double* WindowedLowPass (double dCutOff, double dGain, int iHalfLen);
|
|
|
|
static void FindResampleFactors (int iInSampFreq, int iOutSampFreq,
|
|
int* piUpFactor, int* piDownFactor);
|
|
|
|
|
|
|
|
/*****************************************************************************
|
|
* Resample *
|
|
*----------*
|
|
* Description:
|
|
*
|
|
******************************************************************* PACOG ***/
|
|
|
|
int Resample (double* pdOriginal, int iNumOrig, int iInSampFreq, int iOutSampFreq,
|
|
double** ppdResampled, int* piNumResamp)
|
|
{
|
|
int iUpFactor;
|
|
int iDownFactor;
|
|
int iLimitFactor;
|
|
double* pdFilterCoef;
|
|
int iHalfLen;
|
|
int iFilterLen;
|
|
double dAcum;
|
|
int iPhase;
|
|
int i;
|
|
int j;
|
|
int n;
|
|
|
|
assert (pdOriginal);
|
|
assert (iNumOrig >0);
|
|
assert (ppdResampled);
|
|
assert (piNumResamp);
|
|
assert (iInSampFreq >0);
|
|
assert (iOutSampFreq >0);
|
|
|
|
FindResampleFactors (iInSampFreq, iOutSampFreq, &iUpFactor, &iDownFactor);
|
|
iLimitFactor = (iUpFactor>iDownFactor)? iUpFactor: iDownFactor;
|
|
|
|
iHalfLen = (int)(iInSampFreq * iLimitFactor * FILTER_LEN_2);
|
|
iFilterLen = 2 * iHalfLen + 1;
|
|
|
|
if (! (pdFilterCoef = WindowedLowPass(.5/iLimitFactor, iUpFactor, iHalfLen)))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
*piNumResamp = (iNumOrig * iUpFactor) / iDownFactor;
|
|
*ppdResampled = new double[*piNumResamp];
|
|
if (*ppdResampled == 0)
|
|
{
|
|
fprintf (stderr, "Memory error\n");
|
|
return false;
|
|
}
|
|
|
|
for (i=0; i<*piNumResamp; i++)
|
|
{
|
|
dAcum = 0.0;
|
|
|
|
n = (int )((i * iDownFactor - iHalfLen) / (double)iUpFactor);
|
|
iPhase = (i*iDownFactor) - (n*iUpFactor + iHalfLen);
|
|
|
|
for (j=0; j<iFilterLen/iUpFactor; j++)
|
|
{
|
|
if ( (n+j >=0) && (n+j < iNumOrig) && (iUpFactor*j > iPhase))
|
|
{
|
|
dAcum += pdOriginal[n + j] * pdFilterCoef[iUpFactor * j - iPhase];
|
|
}
|
|
}
|
|
(*ppdResampled)[i] = dAcum;
|
|
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*****************************************************************************
|
|
* WindowedLowPass *
|
|
*-----------------*
|
|
* Description:
|
|
* Creates a low pass filter using the windowing method.
|
|
* CutOff is spec. in normalized frequency
|
|
******************************************************************* PACOG ***/
|
|
|
|
double* WindowedLowPass (double dCutOff, double dGain, int iHalfLen)
|
|
{
|
|
double* pdCoeffs;
|
|
double* pdWindow;
|
|
double dArg;
|
|
double dSinc;
|
|
int iFilterLen = iHalfLen*2 +1;
|
|
int i;
|
|
|
|
assert (dCutOff>0.0 && dCutOff<0.5);
|
|
|
|
pdWindow = ComputeWindow(WINDOW_BLACK, iFilterLen, true);
|
|
if (!pdWindow)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
pdCoeffs = new double[iFilterLen];
|
|
|
|
if (pdCoeffs)
|
|
{
|
|
dArg = 2.0 * M_PI * dCutOff;
|
|
pdCoeffs[iHalfLen] = (double)(dGain * 2.0 * dCutOff);
|
|
|
|
for (i=1; i<=iHalfLen; i++)
|
|
{
|
|
dSinc = dGain * sin(dArg*i)/(M_PI*i) * pdWindow[iHalfLen- i];
|
|
pdCoeffs[iHalfLen+i] = (double)dSinc;
|
|
pdCoeffs[iHalfLen-i] = (double)dSinc;
|
|
}
|
|
}
|
|
|
|
delete[] pdWindow;
|
|
|
|
return pdCoeffs;
|
|
}
|
|
/*****************************************************************************
|
|
* FindResampleFactors *
|
|
*---------------------*
|
|
* Description:
|
|
*
|
|
******************************************************************* PACOG ***/
|
|
|
|
void FindResampleFactors (int iInSampFreq, int iOutSampFreq, int* piUpFactor, int* piDownFactor)
|
|
{
|
|
static int aiPrimes[] = {2,3,5,7,11,13,17,19,23,29,31,37};
|
|
static int iNumPrimes = sizeof (aiPrimes) / sizeof(aiPrimes[0]);
|
|
int iDiv = 1;
|
|
int i;
|
|
|
|
assert (piUpFactor);
|
|
assert (piDownFactor);
|
|
|
|
while (iDiv)
|
|
{
|
|
iDiv = 0;
|
|
for (i=0; i<iNumPrimes;i++)
|
|
{
|
|
if ( (iInSampFreq % aiPrimes[i]) == 0 && (iOutSampFreq % aiPrimes[i]) == 0 )
|
|
{
|
|
iInSampFreq/=aiPrimes[i];
|
|
iOutSampFreq/=aiPrimes[i];
|
|
iDiv = 1;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
*piUpFactor = iOutSampFreq;
|
|
*piDownFactor = iInSampFreq;
|
|
}
|