00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifdef HAVE_CONFIG_H
00026 #include <config.h>
00027 #endif
00028
00029 #include <stdlib.h>
00030 #include <stdio.h>
00031 #include <string.h>
00032 #include <math.h>
00033
00034 #include "aflibConverter.h"
00035 #include "aflibConverterLargeFilter.h"
00036 #include "aflibConverterSmallFilter.h"
00037
00038 #include "aflibDebug.h"
00039
00040 #if (!defined(TRUE) || !defined(FALSE))
00041 # define TRUE 1
00042 # define FALSE 0
00043 #endif
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053 #define Nhc 8
00054 #define Na 7
00055 #define Np (Nhc+Na)
00056 #define Npc (1<<Nhc)
00057 #define Amask ((1<<Na)-1)
00058 #define Pmask ((1<<Np)-1)
00059 #define Nh 16
00060 #define Nb 16
00061 #define Nhxn 14
00062 #define Nhg (Nh-Nhxn)
00063 #define NLpScl 13
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114 aflibConverter::aflibConverter(
00115 bool high_quality,
00116 bool linear_interpolation,
00117 bool filter_interpolation)
00118 {
00119
00120
00121
00122 interpFilt = filter_interpolation;
00123 largeFilter = high_quality;
00124 linearInterp = linear_interpolation;
00125
00126 _II = NULL;
00127 _JJ = NULL;
00128 _vol = 1.0;
00129 }
00130
00131 aflibConverter::~aflibConverter()
00132 {
00133 deleteMemory();
00134 }
00135
00136
00137 void
00138 aflibConverter::deleteMemory()
00139 {
00140 int i;
00141
00142
00143 if (_II != NULL)
00144 {
00145 for (i = 0; i < _nChans; i++)
00146 {
00147 delete [] _II[i];
00148 _II[i] = NULL;
00149 delete [] _JJ[i];
00150 _JJ[i] = NULL;
00151 }
00152 delete [] _II;
00153 _II = NULL;
00154 delete [] _JJ;
00155 _JJ = NULL;
00156 }
00157 }
00158
00159 void
00160 aflibConverter::initialize(
00161 double fac,
00162 int channels,
00163 double volume)
00164 {
00165
00166
00167
00168
00169
00170
00171
00172 int i;
00173
00174
00175 deleteMemory();
00176
00177 _factor = fac;
00178 _nChans = channels;
00179 _initial = TRUE;
00180 _vol = volume;
00181
00182
00183 _II = new short * [_nChans];
00184 _JJ = new short * [_nChans];
00185
00186 for (i = 0; i < _nChans; i++)
00187 {
00188
00189 _II[i] = new short[IBUFFSIZE + 256];
00190 _JJ[i] = new short[(int)(((double)IBUFFSIZE)*_factor)];
00191 memset(_II[i], 0, sizeof(short) * (IBUFFSIZE + 256));
00192 }
00193 }
00194
00195 int
00196 aflibConverter::resample(
00197 int& inCount,
00198 int outCount,
00199 short inArray[],
00200 short outArray[])
00201 {
00202 int Ycount;
00203
00204
00205
00206 if (linearInterp == TRUE)
00207 Ycount = resampleFast(inCount,outCount,inArray,outArray);
00208
00209 else if (largeFilter == FALSE)
00210 Ycount = resampleWithFilter(inCount,outCount,inArray,outArray,
00211 SMALL_FILTER_IMP, SMALL_FILTER_IMPD,
00212 (unsigned short)(SMALL_FILTER_SCALE * _vol),
00213 SMALL_FILTER_NMULT, SMALL_FILTER_NWING);
00214
00215 else
00216 Ycount = resampleWithFilter(inCount,outCount,inArray,outArray,
00217 LARGE_FILTER_IMP, LARGE_FILTER_IMPD,
00218 (unsigned short)(LARGE_FILTER_SCALE * _vol),
00219 LARGE_FILTER_NMULT, LARGE_FILTER_NWING);
00220
00221 _initial = FALSE;
00222
00223 return (Ycount);
00224 }
00225
00226
00227
00228 int
00229 aflibConverter::err_ret(char *s)
00230 {
00231 aflib_debug("resample: %s \n\n",s);
00232 return -1;
00233 }
00234
00235 int
00236 aflibConverter::readData(
00237 int inCount,
00238 short inArray[],
00239 short *outPtr[],
00240 int dataArraySize,
00241 int Xoff,
00242 bool init_count)
00243 {
00244 int i, Nsamps, c;
00245 static unsigned int framecount;
00246 short *ptr;
00247
00248 if (init_count == TRUE)
00249 framecount = 0;
00250
00251 Nsamps = dataArraySize - Xoff;
00252
00253
00254 if (Nsamps > (inCount - (int)framecount))
00255 {
00256 Nsamps = inCount - framecount;
00257 }
00258
00259 for (c = 0; c < _nChans; c++)
00260 {
00261 ptr = outPtr[c];
00262 ptr += Xoff;
00263
00264 for (i = 0; i < Nsamps; i++)
00265 *ptr++ = (short) inArray[c * inCount + i + framecount];
00266 }
00267
00268 framecount += Nsamps;
00269
00270 if ((int)framecount >= inCount)
00271 return (((Nsamps - (framecount - inCount)) - 1) + Xoff);
00272 else
00273 return 0;
00274 }
00275
00276 int
00277 aflibConverter::SrcLinear(
00278 short X[],
00279 short Y[],
00280 double factor,
00281 unsigned int *Time,
00282 unsigned short& Nx,
00283 unsigned short Nout)
00284 {
00285 short iconst;
00286 short *Xp, *Ystart;
00287 int v,x1,x2;
00288
00289 double dt;
00290 unsigned int dtb;
00291
00292 unsigned int start_sample, end_sample;
00293
00294 dt = 1.0/factor;
00295 dtb = (unsigned int)(dt*(1<<Np) + 0.5);
00296
00297 start_sample = (*Time)>>Np;
00298 Ystart = Y;
00299
00300
00301
00302
00303
00304
00305 while (Y - Ystart != Nout)
00306
00307 {
00308 iconst = (*Time) & Pmask;
00309 Xp = &X[(*Time)>>Np];
00310 x1 = *Xp++;
00311 x2 = *Xp;
00312 x1 *= ((1<<Np)-iconst);
00313 x2 *= iconst;
00314 v = x1 + x2;
00315 *Y++ = WordToHword(v,Np);
00316 *Time += dtb;
00317 }
00318 end_sample = (*Time)>>Np;
00319 Nx = end_sample - start_sample;
00320 return (Y - Ystart);
00321 }
00322
00323
00324 int
00325 aflibConverter::SrcUp(
00326 short X[],
00327 short Y[],
00328 double factor,
00329 unsigned int *Time,
00330 unsigned short& Nx,
00331 unsigned short Nout,
00332 unsigned short Nwing,
00333 unsigned short LpScl,
00334 short Imp[],
00335 short ImpD[],
00336 bool Interp)
00337 {
00338 short *Xp, *Ystart;
00339 int v;
00340
00341 double dt;
00342 unsigned int dtb;
00343
00344 unsigned int start_sample, end_sample;
00345
00346 dt = 1.0/factor;
00347 dtb = (unsigned int)(dt*(1<<Np) + 0.5);
00348
00349 start_sample = (*Time)>>Np;
00350 Ystart = Y;
00351
00352
00353
00354
00355
00356
00357 while (Y - Ystart != Nout)
00358
00359 {
00360 Xp = &X[*Time>>Np];
00361
00362 v = FilterUp(Imp, ImpD, Nwing, Interp, Xp, (short)(*Time&Pmask),-1);
00363
00364 v += FilterUp(Imp, ImpD, Nwing, Interp, Xp+1,
00365 (short)((((*Time)^Pmask)+1)&Pmask), 1);
00366 v >>= Nhg;
00367 v *= LpScl;
00368 *Y++ = WordToHword(v,NLpScl);
00369 *Time += dtb;
00370 }
00371 end_sample = (*Time)>>Np;
00372 Nx = end_sample - start_sample;
00373 return (Y - Ystart);
00374 }
00375
00376
00377
00378 int
00379 aflibConverter::SrcUD(
00380 short X[],
00381 short Y[],
00382 double factor,
00383 unsigned int *Time,
00384 unsigned short& Nx,
00385 unsigned short Nout,
00386 unsigned short Nwing,
00387 unsigned short LpScl,
00388 short Imp[],
00389 short ImpD[],
00390 bool Interp)
00391 {
00392 short *Xp, *Ystart;
00393 int v;
00394
00395 double dh;
00396 double dt;
00397
00398 unsigned int dhb, dtb;
00399 unsigned int start_sample, end_sample;
00400
00401 dt = 1.0/factor;
00402 dtb = (unsigned int)(dt*(1<<Np) + 0.5);
00403
00404 dh = MIN(Npc, factor*Npc);
00405 dhb = (unsigned int)(dh*(1<<Na) + 0.5);
00406
00407 start_sample = (*Time)>>Np;
00408 Ystart = Y;
00409
00410
00411
00412
00413
00414
00415 while (Y - Ystart != Nout)
00416
00417 {
00418 Xp = &X[*Time>>Np];
00419 v = FilterUD(Imp, ImpD, Nwing, Interp, Xp, (short)(*Time&Pmask),
00420 -1, dhb);
00421 v += FilterUD(Imp, ImpD, Nwing, Interp, Xp+1,
00422 (short)((((*Time)^Pmask)+1)&Pmask), 1, dhb);
00423 v >>= Nhg;
00424 v *= LpScl;
00425 *Y++ = WordToHword(v,NLpScl);
00426 *Time += dtb;
00427 }
00428
00429 end_sample = (*Time)>>Np;
00430 Nx = end_sample - start_sample;
00431 return (Y - Ystart);
00432 }
00433
00434
00435 int
00436 aflibConverter::resampleFast(
00437 int& inCount,
00438 int outCount,
00439 short inArray[],
00440 short outArray[])
00441 {
00442 unsigned int Time2;
00443 #if 0
00444 unsigned short Ncreep;
00445 #endif
00446 unsigned short Xp, Xoff, Xread;
00447 int OBUFFSIZE = (int)(((double)IBUFFSIZE)*_factor);
00448 unsigned short Nout = 0, Nx, orig_Nx;
00449 unsigned short maxOutput;
00450 int total_inCount = 0;
00451 int c, i, Ycount, last;
00452 bool first_pass = TRUE;
00453
00454
00455 Xoff = 10;
00456
00457 Nx = IBUFFSIZE - 2*Xoff;
00458 last = 0;
00459 Ycount = 0;
00460
00461 Xp = Xoff;
00462 Xread = Xoff;
00463
00464 if (_initial == TRUE)
00465 _Time = (Xoff<<Np);
00466
00467 do {
00468 if (!last)
00469 {
00470 last = readData(inCount, inArray, _II,
00471 IBUFFSIZE, (int)Xread,first_pass);
00472 first_pass = FALSE;
00473 if (last && (last-Xoff<Nx)) {
00474 Nx = last-Xoff;
00475 if (Nx <= 0)
00476 break;
00477 }
00478 }
00479
00480 if ((outCount-Ycount) > (OBUFFSIZE - (2*Xoff*_factor)) )
00481 maxOutput = OBUFFSIZE - (unsigned short)(2*Xoff*_factor);
00482 else
00483 maxOutput = outCount-Ycount;
00484
00485 for (c = 0; c < _nChans; c++)
00486 {
00487 orig_Nx = Nx;
00488 Time2 = _Time;
00489
00490 Nout=SrcLinear(_II[c],_JJ[c],_factor,&Time2,orig_Nx,maxOutput);
00491 }
00492 Nx = orig_Nx;
00493 _Time = Time2;
00494
00495 _Time -= (Nx<<Np);
00496 Xp += Nx;
00497 #if 0
00498 Ncreep = (Time>>Np) - Xoff;
00499 if (Ncreep) {
00500 Time -= (Ncreep<<Np);
00501 Xp += Ncreep;
00502 }
00503 #endif
00504 for (c = 0; c < _nChans; c++)
00505 {
00506 for (i=0; i<IBUFFSIZE-Xp+Xoff; i++) {
00507 _II[c][i] = _II[c][i+Xp-Xoff];
00508 }
00509 }
00510 if (last) {
00511 last -= Xp;
00512 if (!last)
00513 last++;
00514 }
00515 Xread = IBUFFSIZE - Nx;
00516 Xp = Xoff;
00517
00518 Ycount += Nout;
00519 if (Ycount>outCount) {
00520 Nout -= (Ycount-outCount);
00521 Ycount = outCount;
00522 }
00523
00524 if (Nout > OBUFFSIZE)
00525 return err_ret("Output array overflow");
00526
00527 for (c = 0; c < _nChans; c++)
00528 for (i = 0; i < Nout; i++)
00529 outArray[c * outCount + i + Ycount - Nout] = _JJ[c][i];
00530
00531 total_inCount += Nx;
00532
00533 } while (Ycount < outCount);
00534
00535 inCount = total_inCount;
00536
00537 return(Ycount);
00538 }
00539
00540
00541 int
00542 aflibConverter::resampleWithFilter(
00543 int& inCount,
00544 int outCount,
00545 short inArray[],
00546 short outArray[],
00547 short Imp[], short ImpD[],
00548 unsigned short LpScl, unsigned short Nmult, unsigned short Nwing)
00549 {
00550 unsigned int Time2;
00551 #if 0
00552 unsigned short Ncreep;
00553 #endif
00554 unsigned short Xp, Xoff, Xread;
00555 int OBUFFSIZE = (int)(((double)IBUFFSIZE)*_factor);
00556 unsigned short Nout = 0, Nx, orig_Nx;
00557 unsigned short maxOutput;
00558 int total_inCount = 0;
00559 int c, i, Ycount, last;
00560 bool first_pass = TRUE;
00561
00562
00563
00564 if (_factor < 1)
00565 LpScl = (unsigned short)(LpScl*_factor + 0.5);
00566
00567
00568 Xoff = (unsigned short)(((Nmult+1)/2.0) * MAX(1.0,1.0/_factor) + 10);
00569
00570 if (IBUFFSIZE < 2*Xoff)
00571 return err_ret("IBUFFSIZE (or factor) is too small");
00572
00573 Nx = IBUFFSIZE - 2*Xoff;
00574
00575 last = 0;
00576 Ycount = 0;
00577 Xp = Xoff;
00578 Xread = Xoff;
00579
00580 if (_initial == TRUE)
00581 _Time = (Xoff<<Np);
00582
00583 do {
00584 if (!last)
00585 {
00586 last = readData(inCount, inArray, _II,
00587 IBUFFSIZE, (int)Xread,first_pass);
00588 first_pass = FALSE;
00589 if (last && (last-Xoff<Nx)) {
00590 Nx = last-Xoff;
00591 if (Nx <= 0)
00592 break;
00593 }
00594 }
00595
00596 if ( (outCount-Ycount) > (OBUFFSIZE - (2*Xoff*_factor)) )
00597 maxOutput = OBUFFSIZE - (unsigned short)(2*Xoff*_factor);
00598 else
00599 maxOutput = outCount-Ycount;
00600
00601 for (c = 0; c < _nChans; c++)
00602 {
00603 orig_Nx = Nx;
00604 Time2 = _Time;
00605
00606 if (_factor >= 1) {
00607 Nout=SrcUp(_II[c],_JJ[c],_factor,
00608 &Time2,Nx,maxOutput,Nwing,LpScl,Imp,ImpD,interpFilt);
00609 }
00610 else {
00611 Nout=SrcUD(_II[c],_JJ[c],_factor,
00612 &Time2,Nx,maxOutput,Nwing,LpScl,Imp,ImpD,interpFilt);
00613 }
00614 }
00615 _Time = Time2;
00616
00617 _Time -= (Nx<<Np);
00618 Xp += Nx;
00619 #if 0
00620 Ncreep = (Time>>Np) - Xoff;
00621 if (Ncreep) {
00622 Time -= (Ncreep<<Np);
00623 Xp += Ncreep;
00624 }
00625 #endif
00626 if (last) {
00627 last -= Xp;
00628 if (!last)
00629 last++;
00630 }
00631
00632 Ycount += Nout;
00633 if (Ycount > outCount) {
00634 Nout -= (Ycount - outCount);
00635 Ycount = outCount;
00636 }
00637
00638 if (Nout > OBUFFSIZE)
00639 return err_ret("Output array overflow");
00640
00641 for (c = 0; c < _nChans; c++)
00642 {
00643 for (i = 0; i < Nout; i++)
00644 {
00645 outArray[c * outCount + i + Ycount - Nout] = _JJ[c][i];
00646 }
00647 }
00648
00649 int act_incount = (int)Nx;
00650
00651 for (c = 0; c < _nChans; c++)
00652 {
00653 for (i=0; i<IBUFFSIZE-act_incount+Xoff; i++) {
00654 _II[c][i] = _II[c][i+act_incount];
00655 }
00656 }
00657 Xread = IBUFFSIZE - Nx;
00658 Xp = Xoff;
00659
00660 total_inCount += Nx;
00661
00662 } while (Ycount < outCount);
00663
00664 inCount = total_inCount;
00665
00666 return(Ycount);
00667 }
00668
00669 int
00670 aflibConverter::FilterUp(
00671 short Imp[],
00672 short ImpD[],
00673 unsigned short Nwing,
00674 bool Interp,
00675 short *Xp,
00676 short Ph,
00677 short Inc)
00678 {
00679 short *Hp, *Hdp = NULL, *End;
00680 short a = 0;
00681 int v, t;
00682
00683 v=0;
00684 Hp = &Imp[Ph>>Na];
00685 End = &Imp[Nwing];
00686
00687 if (Interp)
00688 {
00689 Hdp = &ImpD[Ph>>Na];
00690 a = Ph & Amask;
00691 }
00692
00693 if (Inc == 1)
00694 {
00695 End--;
00696 if (Ph == 0)
00697 {
00698 Hp += Npc;
00699 Hdp += Npc;
00700 }
00701 }
00702
00703 if (Interp)
00704 {
00705 while (Hp < End)
00706 {
00707 t = *Hp;
00708 t += (((int)*Hdp)*a)>>Na;
00709 Hdp += Npc;
00710 t *= *Xp;
00711 if (t & (1<<(Nhxn-1)))
00712 t += (1<<(Nhxn-1));
00713 t >>= Nhxn;
00714 v += t;
00715 Hp += Npc;
00716 Xp += Inc;
00717 }
00718 }
00719 else
00720 {
00721 while (Hp < End)
00722 {
00723 t = *Hp;
00724 t *= *Xp;
00725 if (t & (1<<(Nhxn-1)))
00726 t += (1<<(Nhxn-1));
00727 t >>= Nhxn;
00728 v += t;
00729 Hp += Npc;
00730 Xp += Inc;
00731 }
00732 }
00733 return(v);
00734 }
00735
00736
00737 int
00738 aflibConverter::FilterUD(
00739 short Imp[],
00740 short ImpD[],
00741 unsigned short Nwing,
00742 bool Interp,
00743 short *Xp,
00744 short Ph,
00745 short Inc,
00746 unsigned short dhb)
00747 {
00748 short a;
00749 short *Hp, *Hdp, *End;
00750 int v, t;
00751 unsigned int Ho;
00752
00753 v=0;
00754 Ho = (Ph*(unsigned int)dhb)>>Np;
00755 End = &Imp[Nwing];
00756 if (Inc == 1)
00757 {
00758 End--;
00759 if (Ph == 0)
00760 Ho += dhb;
00761 }
00762
00763
00764 if (Interp)
00765 {
00766 while ((Hp = &Imp[Ho>>Na]) < End)
00767 {
00768 t = *Hp;
00769 Hdp = &ImpD[Ho>>Na];
00770 a = Ho & Amask;
00771 t += (((int)*Hdp)*a)>>Na;
00772 t *= *Xp;
00773 if (t & 1<<(Nhxn-1))
00774 t += 1<<(Nhxn-1);
00775 t >>= Nhxn;
00776 v += t;
00777 Ho += dhb;
00778 Xp += Inc;
00779 }
00780 }
00781 else
00782 {
00783 while ((Hp = &Imp[Ho>>Na]) < End)
00784 {
00785 t = *Hp;
00786 t *= *Xp;
00787 if (t & 1<<(Nhxn-1))
00788 t += 1<<(Nhxn-1);
00789 t >>= Nhxn;
00790 v += t;
00791 Ho += dhb;
00792 Xp += Inc;
00793 }
00794 }
00795 return(v);
00796 }
00797