00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include <stdio.h>
00024 #ifdef USE_INTEL_MATH
00025 #include <mathimf.h>
00026 #else
00027 #include <math.h>
00028 #endif
00029 #include <float.h>
00030
00031 #include "dsp/RecInterpolator.hh"
00032
00033
00034 void clRecInterpolator::InitHalves (double dCenter)
00035 {
00036 bool bInverted = false;
00037 double dBandCenter = 0.5;
00038 double dBandWidth = 0.5;
00039 long lInitCntr;
00040
00041 for (lInitCntr = 0; lInitCntr < lSubRounds; lInitCntr++)
00042 {
00043 dBandWidth *= 0.5;
00044 if (dCenter <= dBandCenter)
00045 {
00046 bpHalves[lInitCntr] = (!bInverted) ? false : true;
00047 dBandCenter = dBandCenter - dBandWidth;
00048 }
00049 else
00050 {
00051 bpHalves[lInitCntr] = (!bInverted) ? true : false;
00052 dBandCenter = dBandCenter + dBandWidth;
00053 }
00054 if (bpHalves[lInitCntr]) bInverted = !bInverted;
00055 }
00056 }
00057
00058
00059 clRecInterpolator::clRecInterpolator ()
00060 {
00061 bInitialized = false;
00062 iType = FILTER_TYPE_FFT;
00063 lSubRounds = 0;
00064 }
00065
00066
00067 clRecInterpolator::~clRecInterpolator ()
00068 {
00069 if (bInitialized) Uninitialize();
00070 }
00071
00072
00073 bool clRecInterpolator::Initialize (long lIntFact, long lFiltSize,
00074 const float *fpNullPtr, float fBandCenter, int iFilterType)
00075 {
00076 long lInitCntr;
00077
00078 if (bInitialized) Uninitialize();
00079 iType = iFilterType;
00080 lFilterSize = abs((int) lFiltSize);
00081 lFactor = lIntFact;
00082 lIntSize = (long) (lFilterSize * (1.0f - DSP_FILT_DEF_OVERLAPF));
00083 lSubRounds = (long) (log(lFactor) / log(2) + 0.5);
00084 if (lSubRounds > RECINT_MAX_SUB_ROUNDS) return false;
00085 InitHalves(fBandCenter);
00086 for (lInitCntr = 0; lInitCntr < lSubRounds; lInitCntr++)
00087 {
00088 switch (iType)
00089 {
00090 case FILTER_TYPE_FFT:
00091 FFTIntBank[lInitCntr].Initialize(2, lFiltSize, fpNullPtr,
00092 bpHalves[lInitCntr]);
00093 break;
00094 case FILTER_TYPE_FIR:
00095 FIRIntBank[lInitCntr].Initialize(2, fpNullPtr,
00096 bpHalves[lInitCntr]);
00097 break;
00098 case FILTER_TYPE_IIR:
00099 IIRIntBank[lInitCntr].Initialize(2, fpNullPtr,
00100 bpHalves[lInitCntr]);
00101 break;
00102 }
00103 }
00104 IntBuf.Size(lIntSize * sizeof(float));
00105 bInitialized = true;
00106 return true;
00107 }
00108
00109
00110 bool clRecInterpolator::Initialize (long lIntFact, long lFiltSize,
00111 const double *dpNullPtr, double dBandCenter, int iFilterType)
00112 {
00113 long lInitCntr;
00114
00115 if (bInitialized) Uninitialize();
00116 iType = iFilterType;
00117 lFilterSize = lFiltSize;
00118 lFactor = lIntFact;
00119 lIntSize = (long) (lFilterSize * (1.0 - DSP_FILT_DEF_OVERLAP));
00120 lSubRounds = (long) (log(lFactor) / log(2) + 0.5);
00121 if (lSubRounds > RECINT_MAX_SUB_ROUNDS) return false;
00122 InitHalves(dBandCenter);
00123 for (lInitCntr = 0; lInitCntr < lSubRounds; lInitCntr++)
00124 {
00125 switch (iType)
00126 {
00127 case FILTER_TYPE_FFT:
00128 FFTIntBank[lInitCntr].Initialize(2, lFiltSize, dpNullPtr,
00129 bpHalves[lInitCntr]);
00130 break;
00131 case FILTER_TYPE_FIR:
00132 FIRIntBank[lInitCntr].Initialize(2, dpNullPtr,
00133 bpHalves[lInitCntr]);
00134 break;
00135 case FILTER_TYPE_IIR:
00136 IIRIntBank[lInitCntr].Initialize(2, dpNullPtr,
00137 bpHalves[lInitCntr]);
00138 break;
00139 }
00140 }
00141 IntBuf.Size(lIntSize * sizeof(double));
00142 bInitialized = true;
00143 return true;
00144 }
00145
00146
00147 bool clRecInterpolator::Initialize (long lIntFact, long lFiltSize,
00148 const float *fpNullPtr, float fBandCenter, bool bUseFIR)
00149 {
00150 if (!bUseFIR)
00151 return Initialize(lIntFact, lFiltSize, fpNullPtr, fBandCenter,
00152 FILTER_TYPE_FFT);
00153 else
00154 return Initialize(lIntFact, lFiltSize, fpNullPtr, fBandCenter,
00155 FILTER_TYPE_FIR);
00156 }
00157
00158
00159 bool clRecInterpolator::Initialize (long lIntFact, long lFiltSize,
00160 const double *dpNullPtr, double dBandCenter, bool bUseFIR)
00161 {
00162 if (!bUseFIR)
00163 return Initialize(lIntFact, lFiltSize, dpNullPtr, dBandCenter,
00164 FILTER_TYPE_FFT);
00165 else
00166 return Initialize(lIntFact, lFiltSize, dpNullPtr, dBandCenter,
00167 FILTER_TYPE_FIR);
00168 }
00169
00170
00171 void clRecInterpolator::Uninitialize ()
00172 {
00173 long lUninitCntr;
00174
00175 IntBuf.Free();
00176 for (lUninitCntr = 0; lUninitCntr < lSubRounds; lUninitCntr++)
00177 {
00178 switch (iType)
00179 {
00180 case FILTER_TYPE_FFT:
00181 FFTIntBank[lUninitCntr].Uninitialize();
00182 break;
00183 case FILTER_TYPE_FIR:
00184 FIRIntBank[lUninitCntr].Uninitialize();
00185 break;
00186 case FILTER_TYPE_IIR:
00187 IIRIntBank[lUninitCntr].Uninitialize();
00188 break;
00189 }
00190 }
00191 bInitialized = false;
00192 }
00193
00194
00195 void clRecInterpolator::Put (const float *fpSrcData, long lSrcCount)
00196 {
00197 long lRecCntr;
00198 long lSubCount;
00199 float *fpIntBuf;
00200
00201 fpIntBuf = IntBuf;
00202 switch (iType)
00203 {
00204 case FILTER_TYPE_FFT:
00205 FFTIntBank[0].Put(fpSrcData, lSrcCount);
00206 for (lRecCntr = 0; lRecCntr < (lSubRounds - 1); lRecCntr++)
00207 {
00208 while (FFTIntBank[lRecCntr].Get(fpIntBuf, lIntSize))
00209 FFTIntBank[lRecCntr + 1].Put(fpIntBuf, lIntSize);
00210 }
00211 break;
00212 case FILTER_TYPE_FIR:
00213 FIRIntBank[0].Put(fpSrcData, lSrcCount);
00214 for (lRecCntr = 0; lRecCntr < (lSubRounds - 1); lRecCntr++)
00215 {
00216 lSubCount = (lIntSize << lRecCntr);
00217 while (FIRIntBank[lRecCntr].Get(fpIntBuf, lSubCount))
00218 FIRIntBank[lRecCntr + 1].Put(fpIntBuf, lSubCount);
00219 }
00220 break;
00221 case FILTER_TYPE_IIR:
00222 IIRIntBank[0].Put(fpSrcData, lSrcCount);
00223 for (lRecCntr = 0; lRecCntr < (lSubRounds - 1); lRecCntr++)
00224 {
00225 lSubCount = (lIntSize << lRecCntr);
00226 while (IIRIntBank[lRecCntr].Get(fpIntBuf, lSubCount))
00227 IIRIntBank[lRecCntr + 1].Put(fpIntBuf, lSubCount);
00228 }
00229 break;
00230 }
00231 }
00232
00233
00234 void clRecInterpolator::Put (const double *dpSrcData, long lSrcCount)
00235 {
00236 long lRecCntr;
00237 long lSubCount;
00238 double *dpIntBuf;
00239
00240 dpIntBuf = IntBuf;
00241 switch (iType)
00242 {
00243 case FILTER_TYPE_FFT:
00244 FFTIntBank[0].Put(dpSrcData, lSrcCount);
00245 for (lRecCntr = 0; lRecCntr < (lSubRounds - 1); lRecCntr++)
00246 {
00247 while (FFTIntBank[lRecCntr].Get(dpIntBuf, lIntSize))
00248 FFTIntBank[lRecCntr + 1].Put(dpIntBuf, lIntSize);
00249 }
00250 break;
00251 case FILTER_TYPE_FIR:
00252 FIRIntBank[0].Put(dpSrcData, lSrcCount);
00253 for (lRecCntr = 0; lRecCntr < (lSubRounds - 1); lRecCntr++)
00254 {
00255 lSubCount = (lIntSize << lRecCntr);
00256 while (FIRIntBank[lRecCntr].Get(dpIntBuf, lSubCount))
00257 FIRIntBank[lRecCntr + 1].Put(dpIntBuf, lSubCount);
00258 }
00259 break;
00260 case FILTER_TYPE_IIR:
00261 IIRIntBank[0].Put(dpSrcData, lSrcCount);
00262 for (lRecCntr = 0; lRecCntr < (lSubRounds - 1); lRecCntr++)
00263 {
00264 lSubCount = (lIntSize << lRecCntr);
00265 while (IIRIntBank[lRecCntr].Get(dpIntBuf, lSubCount))
00266 IIRIntBank[lRecCntr + 1].Put(dpIntBuf, lSubCount);
00267 }
00268 break;
00269 }
00270 }
00271
00272
00273 bool clRecInterpolator::Get (float *fpDestData, long lDestCount)
00274 {
00275 switch (iType)
00276 {
00277 case FILTER_TYPE_FFT:
00278 return FFTIntBank[lSubRounds - 1].Get(fpDestData, lDestCount);
00279 case FILTER_TYPE_FIR:
00280 return FIRIntBank[lSubRounds - 1].Get(fpDestData, lDestCount);
00281 case FILTER_TYPE_IIR:
00282 return IIRIntBank[lSubRounds - 1].Get(fpDestData, lDestCount);
00283 }
00284 return false;
00285 }
00286
00287
00288 bool clRecInterpolator::Get (double *dpDestData, long lDestCount)
00289 {
00290 switch (iType)
00291 {
00292 case FILTER_TYPE_FFT:
00293 return FFTIntBank[lSubRounds - 1].Get(dpDestData, lDestCount);
00294 case FILTER_TYPE_FIR:
00295 return FIRIntBank[lSubRounds - 1].Get(dpDestData, lDestCount);
00296 case FILTER_TYPE_IIR:
00297 return IIRIntBank[lSubRounds - 1].Get(dpDestData, lDestCount);
00298 }
00299 return false;
00300 }