WDL/resample.cpp

   1 /*
   2 ** Copyright (C) 2010 Cockos Incorporated
   3 ** LGPL
   4 **/
   5 #include "resample.h"
   6 #include <math.h>
   7
   8 #include "denormal.h"
   9
  10 #ifndef min
  11 #define min(x,y) ((x)<(y)?(x):(y))
  12 #endif
  13
  14 #ifndef PI
  15 #define PI 3.1415926535897932384626433832795
  16 #endif
  17
  18 class WDL_Resampler::WDL_Resampler_IIRFilter
  19 {
  20 public:
  21   WDL_Resampler_IIRFilter()
  22   {
  23     m_fpos=-1;
  24     Reset();
  25   }
  26   ~WDL_Resampler_IIRFilter()
  27   {
  28   }
  29
  30   void Reset()
  31   {
  32     memset(m_hist,0,sizeof(m_hist));
  33   }
  34
  35   void setParms(double fpos, double Q)
  36   {
  37     if (fabs(fpos-m_fpos)<0.000001) return;
  38     m_fpos=fpos;
  39
  40     double pos = fpos * PI;
  41     double cpos=cos(pos);
  42     double spos=sin(pos);
  43
  44     double alpha=spos/(2.0*Q);
  45
  46     double sc=1.0/( 1 + alpha);
  47     m_b1 = (1-cpos) * sc;
  48     m_b2 = m_b0 = m_b1*0.5;
  49     m_a1 =  -2 * cpos * sc;
  50     m_a2 = (1-alpha)*sc;
  51
  52   }
  53
  54   void Apply(WDL_ResampleSample *in1, WDL_ResampleSample *out1, int ns, int span, int w)
  55   {
  56     double b0=m_b0,b1=m_b1,b2=m_b2,a1=m_a1,a2=m_a2;
  57     double *hist=m_hist[w];
  58     while (ns--)
  59     {
  60       double in=*in1;
  61       in1+=span;
  62       double out = (double) ( in*b0 + hist[0]*b1 + hist[1]*b2 - hist[2]*a1 - hist[3]*a2);
  63       hist[1]=hist[0]; hist[0]=in;
  64       hist[3]=hist[2]; *out1 = hist[2]=denormal_filter_double(out);
  65
  66       out1+=span;
  67     }
  68   }
  69
  70 private:
  71   double m_fpos;
  72   double m_a1,m_a2;
  73   double m_b0,m_b1,m_b2;
  74   double m_hist[WDL_RESAMPLE_MAX_FILTERS*WDL_RESAMPLE_MAX_NCH][4];
  75 };
  76
  77
  78 void inline WDL_Resampler::SincSample(WDL_ResampleSample *outptr, WDL_ResampleSample *inptr, double fracpos, int nch, WDL_SincFilterSample *filter, int filtsz)
  79 {
  80   int oversize=m_lp_oversize;
  81   int x;
  82   fracpos *= oversize;
  83   int ifpos=(int)fracpos;
  84   filter += oversize-1-ifpos;
  85   fracpos -= ifpos;
  86
  87   for (x = 0; x < nch; x ++)
  88   {
  89     double sum=0.0,sum2=0.0;
  90     WDL_SincFilterSample *fptr=filter;
  91     WDL_ResampleSample *iptr=inptr+x;
  92     int i=filtsz;
  93     while (i--)
  94     {
  95       sum += fptr[0]*iptr[0];
  96       sum2 += fptr[1]*iptr[0];
  97       iptr+=nch;
  98       fptr += oversize;
  99     }
 100     outptr[x]=sum*fracpos + sum2*(1.0-fracpos);
 101   }
 102
 103 }
 104
 105 void inline WDL_Resampler::SincSample1(WDL_ResampleSample *outptr, WDL_ResampleSample *inptr, double fracpos, WDL_SincFilterSample *filter, int filtsz)
 106 {
 107   int oversize=m_lp_oversize;
 108   fracpos *= oversize;
 109   int ifpos=(int)fracpos;
 110   filter += oversize-1-ifpos;
 111   fracpos -= ifpos;
 112
 113   double sum=0.0,sum2=0.0;
 114   WDL_SincFilterSample *fptr=filter;
 115   WDL_ResampleSample *iptr=inptr;
 116   int i=filtsz;
 117   while (i--)
 118   {
 119     sum += fptr[0]*iptr[0];
 120     sum2 += fptr[1]*iptr[0];
 121     iptr++;
 122     fptr += oversize;
 123   }
 124   outptr[0]=sum*fracpos+sum2*(1.0-fracpos);
 125
 126 }
 127
 128 void inline WDL_Resampler::SincSample2(WDL_ResampleSample *outptr, WDL_ResampleSample *inptr, double fracpos, WDL_SincFilterSample *filter, int filtsz)
 129 {
 130   int oversize=m_lp_oversize;
 131   fracpos *= oversize;
 132   int ifpos=(int)fracpos;
 133   filter += oversize-1-ifpos;
 134   fracpos -= ifpos;
 135
 136   double sum=0.0;
 137   double sum2=0.0;
 138   double sumb=0.0;
 139   double sum2b=0.0;
 140   WDL_SincFilterSample *fptr=filter;
 141   WDL_ResampleSample *iptr=inptr;
 142   int i=filtsz/2;
 143   while (i--)
 144   {
 145     sum += fptr[0]*iptr[0];
 146     sum2 += fptr[0]*iptr[1];
 147     sumb += fptr[1]*iptr[0];
 148     sum2b += fptr[1]*iptr[1];
 149     sum += fptr[oversize]*iptr[2];
 150     sum2 += fptr[oversize]*iptr[3];
 151     sumb += fptr[oversize+1]*iptr[2];
 152     sum2b += fptr[oversize+1]*iptr[3];
 153     iptr+=4;
 154     fptr += oversize*2;
 155   }
 156   outptr[0]=sum*fracpos + sumb*(1.0-fracpos);
 157   outptr[1]=sum2*fracpos + sum2b*(1.0-fracpos);
 158
 159 }
 160
 161
 162
 163 WDL_Resampler::WDL_Resampler()
 164 {
 165   m_filterq=0.707f;
 166   m_filterpos=0.693f; // .792 ?
 167
 168   m_sincoversize=0;
 169   m_lp_oversize=1;
 170   m_sincsize=0;
 171   m_filtercnt=1;
 172   m_interp=true;
 173   m_feedmode=false;
 174
 175   m_filter_coeffs_size=0;
 176   m_sratein=44100.0;
 177   m_srateout=44100.0;
 178   m_ratio=1.0;
 179   m_filter_ratio=-1.0;
 180   m_iirfilter=0;
 181
 182   Reset();
 183 }
 184
 185 WDL_Resampler::~WDL_Resampler()
 186 {
 187   delete m_iirfilter;
 188 }
 189
 190 void WDL_Resampler::Reset(double fracpos)
 191 {
 192   m_last_requested=0;
 193   m_filtlatency=0;
 194   m_fracpos=fracpos;
 195   m_samples_in_rsinbuf=0;
 196   if (m_iirfilter) m_iirfilter->Reset();
 197 }
 198
 199 void WDL_Resampler::SetMode(bool interp, int filtercnt, bool sinc, int sinc_size, int sinc_interpsize)
 200 {
 201   m_sincsize = sinc && sinc_size>= 4 ? sinc_size > 8192 ? 8192 : sinc_size : 0;
 202   m_sincoversize = m_sincsize  ? (sinc_interpsize<= 1 ? 1 : sinc_interpsize>=4096 ? 4096 : sinc_interpsize) : 1;
 203
 204   m_filtercnt = m_sincsize ? 0 : (filtercnt<=0?0 : filtercnt >= WDL_RESAMPLE_MAX_FILTERS ? WDL_RESAMPLE_MAX_FILTERS : filtercnt);
 205   m_interp=interp && !m_sincsize;
 206 //  char buf[512];
 207 //  sprintf(buf,"setting interp=%d, filtercnt=%d, sinc=%d,%d\n",m_interp,m_filtercnt,m_sincsize,m_sincoversize);
 208 //  OutputDebugString(buf);
 209
 210   if (!m_sincsize)
 211   {
 212     m_filter_coeffs.Resize(0);
 213     m_filter_coeffs_size=0;
 214   }
 215   if (!m_filtercnt)
 216   {
 217     delete m_iirfilter;
 218     m_iirfilter=0;
 219   }
 220 }
 221
 222 void WDL_Resampler::SetRates(double rate_in, double rate_out)
 223 {
 224   if (rate_in<1.0) rate_in=1.0;
 225   if (rate_out<1.0) rate_out=1.0;
 226   if (rate_in != m_sratein || rate_out != m_srateout)
 227   {
 228     m_sratein=rate_in;
 229     m_srateout=rate_out;
 230     m_ratio=m_sratein / m_srateout;
 231   }
 232 }
 233
 234
 235 void WDL_Resampler::BuildLowPass(double filtpos) // only called in sinc modes
 236 {
 237   int wantsize=m_sincsize;
 238   int wantinterp=m_sincoversize;
 239
 240   if (m_filter_ratio!=filtpos ||
 241       m_filter_coeffs_size != wantsize ||
 242       m_lp_oversize != wantinterp)
 243   {
 244     m_lp_oversize = wantinterp;
 245     m_filter_ratio=filtpos;
 246
 247     // build lowpass filter
 248     int allocsize = (wantsize+1)*m_lp_oversize;
 249     WDL_SincFilterSample *cfout=m_filter_coeffs.Resize(allocsize);
 250     if (m_filter_coeffs.GetSize()==allocsize)
 251     {
 252       m_filter_coeffs_size=wantsize;
 253
 254       int sz=wantsize*m_lp_oversize;
 255       int hsz=sz/2;
 256       double filtpower=0.0;
 257       double windowpos = 0.0;
 258       double dwindowpos = 2.0 * PI/(double)(sz);
 259       double dsincpos  = PI / m_lp_oversize * filtpos; // filtpos is outrate/inrate, i.e. 0.5 is going to half rate
 260       double sincpos = dsincpos * (double)(-hsz);
 261
 262       int x;
 263       for (x = -hsz; x < hsz+m_lp_oversize; x ++)
 264       {
 265         double val = 0.35875 - 0.48829 * cos(windowpos) + 0.14128 * cos(2*windowpos) - 0.01168 * cos(6*windowpos); // blackman-harris
 266         if (x) val *= sin(sincpos) / sincpos;
 267
 268         windowpos+=dwindowpos;
 269         sincpos += dsincpos;
 270
 271         cfout[hsz+x] = (WDL_SincFilterSample)val;
 272         if (x < hsz) filtpower += val;
 273       }
 274       filtpower = m_lp_oversize/filtpower;
 275       for (x = 0; x < sz+m_lp_oversize; x ++)
 276       {
 277         cfout[x] = (WDL_SincFilterSample) (cfout[x]*filtpower);
 278       }
 279     }
 280     else m_filter_coeffs_size=0;
 281
 282   }
 283 }
 284
 285 double WDL_Resampler::GetCurrentLatency()
 286 {
 287   double v=((double)m_samples_in_rsinbuf-m_filtlatency)/m_sratein;
 288
 289   if (v<0.0)v=0.0;
 290   return v;
 291 }
 292
 293 int WDL_Resampler::ResamplePrepare(int out_samples, int nch, WDL_ResampleSample **inbuffer)
 294 {
 295   if (nch > WDL_RESAMPLE_MAX_NCH || nch < 1) return 0;
 296
 297   int fsize=0;
 298   if (m_sincsize>1) fsize = m_sincsize;
 299
 300   int hfs=fsize/2;
 301   if (hfs>1 && m_samples_in_rsinbuf<hfs-1)
 302   {
 303     m_filtlatency+=hfs-1 - m_samples_in_rsinbuf;
 304
 305     m_samples_in_rsinbuf=hfs-1;
 306
 307     if (m_samples_in_rsinbuf>0)
 308     {
 309       WDL_ResampleSample *p = m_rsinbuf.Resize(m_samples_in_rsinbuf*nch,false);
 310       memset(p,0,sizeof(WDL_ResampleSample)*m_rsinbuf.GetSize());
 311     }
 312   }
 313
 314   int sreq = 0;
 315
 316   if (!m_feedmode) sreq = (int)(m_ratio * out_samples) + 4 + fsize - m_samples_in_rsinbuf;
 317   else sreq = out_samples;
 318
 319   if (sreq<0)sreq=0;
 320
 321 again:
 322   m_rsinbuf.Resize((m_samples_in_rsinbuf+sreq)*nch,false);
 323
 324   int sz = m_rsinbuf.GetSize()/(nch?nch:1) - m_samples_in_rsinbuf;
 325   if (sz!=sreq)
 326   {
 327     if (sreq>4 && !sz)
 328     {
 329       sreq/=2;
 330       goto again; // try again with half the size
 331     }
 332     // todo: notify of error?
 333     sreq=sz;
 334   }
 335
 336   *inbuffer = m_rsinbuf.Get() + m_samples_in_rsinbuf*nch;
 337
 338   m_last_requested=sreq;
 339   return sreq;
 340 }
 341
 342
 343
 344 int WDL_Resampler::ResampleOut(WDL_ResampleSample *out, int nsamples_in, int nsamples_out, int nch)
 345 {
 346   if (nch > WDL_RESAMPLE_MAX_NCH || nch < 1) return 0;
 347
 348   if (m_filtercnt>0)
 349   {
 350     if (m_ratio > 1.0 && nsamples_in > 0) // filter input
 351     {
 352       if (!m_iirfilter) m_iirfilter = new WDL_Resampler_IIRFilter;
 353
 354       int n=m_filtercnt;
 355       m_iirfilter->setParms((1.0/m_ratio)*m_filterpos,m_filterq);
 356
 357       WDL_ResampleSample *buf=(WDL_ResampleSample *)m_rsinbuf.Get() + m_samples_in_rsinbuf*nch;
 358       int a,x;
 359       int offs=0;
 360       for (x=0; x < nch; x ++)
 361         for (a = 0; a < n; a ++)
 362           m_iirfilter->Apply(buf+x,buf+x,nsamples_in,nch,offs++);
 363     }
 364   }
 365
 366   m_samples_in_rsinbuf += min(nsamples_in,m_last_requested); // prevent the user from corrupting the internal state
 367
 368
 369   int rsinbuf_availtemp = m_samples_in_rsinbuf;
 370
 371   if (nsamples_in < m_last_requested) // flush out to ensure we can deliver
 372   {
 373     int fsize=(m_last_requested-nsamples_in)*2 + m_sincsize*2;
 374
 375     int alloc_size=(m_samples_in_rsinbuf+fsize)*nch;
 376     WDL_ResampleSample *zb=m_rsinbuf.Resize(alloc_size,false);
 377     if (m_rsinbuf.GetSize()==alloc_size)
 378     {
 379       memset(zb+m_samples_in_rsinbuf*nch,0,fsize*nch*sizeof(WDL_ResampleSample));
 380       rsinbuf_availtemp = m_samples_in_rsinbuf+fsize;
 381     }
 382   }
 383
 384   int ret=0;
 385   double srcpos=m_fracpos;
 386   double drspos = m_ratio;
 387   WDL_ResampleSample *localin = m_rsinbuf.Get();
 388
 389   WDL_ResampleSample *outptr=out;
 390
 391   int ns=nsamples_out;
 392
 393   int outlatadj=0;
 394
 395   if (m_sincsize) // sinc interpolating
 396   {
 397     if (m_ratio > 1.0) BuildLowPass(1.0 / (m_ratio*1.03));
 398     else BuildLowPass(1.0);
 399
 400     int filtsz=m_filter_coeffs_size;
 401     int filtlen = rsinbuf_availtemp - filtsz;
 402     outlatadj=filtsz/2-1;
 403     WDL_SincFilterSample *filter=m_filter_coeffs.Get();
 404
 405     if (nch == 1)
 406     {
 407       while (ns--)
 408       {
 409         int ipos = (int)srcpos;
 410
 411         if (ipos >= filtlen-1)  break; // quit decoding, not enough input samples
 412
 413         SincSample1(outptr,localin + ipos,srcpos-ipos,filter,filtsz);
 414         outptr ++;
 415         srcpos+=drspos;
 416         ret++;
 417       }
 418     }
 419     else if (nch==2)
 420     {
 421       while (ns--)
 422       {
 423         int ipos = (int)srcpos;
 424
 425         if (ipos >= filtlen-1)  break; // quit decoding, not enough input samples
 426
 427         SincSample2(outptr,localin + ipos*2,srcpos-ipos,filter,filtsz);
 428         outptr+=2;
 429         srcpos+=drspos;
 430         ret++;
 431       }
 432     }
 433     else
 434     {
 435       while (ns--)
 436       {
 437         int ipos = (int)srcpos;
 438
 439         if (ipos >= filtlen-1)  break; // quit decoding, not enough input samples
 440
 441         SincSample(outptr,localin + ipos*nch,srcpos-ipos,nch,filter,filtsz);
 442         outptr += nch;
 443         srcpos+=drspos;
 444         ret++;
 445       }
 446     }
 447   }
 448   else if (!m_interp) // point sampling
 449   {
 450     if (nch == 1)
 451     {
 452       while (ns--)
 453       {
 454         int ipos = (int)srcpos;
 455         if (ipos >= rsinbuf_availtemp)  break; // quit decoding, not enough input samples
 456
 457         *outptr++ = localin[ipos];
 458         srcpos+=drspos;
 459         ret++;
 460       }
 461     }
 462     else if (nch == 2)
 463     {
 464       while (ns--)
 465       {
 466         int ipos = (int)srcpos;
 467         if (ipos >= rsinbuf_availtemp)  break; // quit decoding, not enough input samples
 468
 469         ipos+=ipos;
 470
 471         outptr[0] = localin[ipos];
 472         outptr[1] = localin[ipos+1];
 473         outptr+=2;
 474         srcpos+=drspos;
 475         ret++;
 476       }
 477     }
 478     else
 479       while (ns--)
 480       {
 481         int ipos = (int)srcpos;
 482         if (ipos >= rsinbuf_availtemp)  break; // quit decoding, not enough input samples
 483
 484         memcpy(outptr,localin + ipos*nch,nch*sizeof(WDL_ResampleSample));
 485         outptr += nch;
 486         srcpos+=drspos;
 487         ret++;
 488       }
 489   }
 490   else // linear interpolation
 491   {
 492     if (nch == 1)
 493     {
 494       while (ns--)
 495       {
 496         int ipos = (int)srcpos;
 497         double fracpos=srcpos-ipos;
 498
 499         if (ipos >= rsinbuf_availtemp-1)
 500         {
 501           break; // quit decoding, not enough input samples
 502         }
 503
 504         double ifracpos=1.0-fracpos;
 505         WDL_ResampleSample *inptr = localin + ipos;
 506         *outptr++ = inptr[0]*(ifracpos) + inptr[1]*(fracpos);
 507         srcpos+=drspos;
 508         ret++;
 509       }
 510     }
 511     else if (nch == 2)
 512     {
 513       while (ns--)
 514       {
 515         int ipos = (int)srcpos;
 516         double fracpos=srcpos-ipos;
 517
 518         if (ipos >= rsinbuf_availtemp-1)
 519         {
 520           break; // quit decoding, not enough input samples
 521         }
 522
 523         double ifracpos=1.0-fracpos;
 524         WDL_ResampleSample *inptr = localin + ipos*2;
 525         outptr[0] = inptr[0]*(ifracpos) + inptr[2]*(fracpos);
 526         outptr[1] = inptr[1]*(ifracpos) + inptr[3]*(fracpos);
 527         outptr += 2;
 528         srcpos+=drspos;
 529         ret++;
 530       }
 531     }
 532     else
 533     {
 534       while (ns--)
 535       {
 536         int ipos = (int)srcpos;
 537         double fracpos=srcpos-ipos;
 538
 539         if (ipos >= rsinbuf_availtemp-1)
 540         {
 541           break; // quit decoding, not enough input samples
 542         }
 543
 544         double ifracpos=1.0-fracpos;
 545         int ch=nch;
 546         WDL_ResampleSample *inptr = localin + ipos*nch;
 547         while (ch--)
 548         {
 549           *outptr++ = inptr[0]*(ifracpos) + inptr[nch]*(fracpos);
 550           inptr++;
 551         }
 552         srcpos+=drspos;
 553         ret++;
 554       }
 555     }
 556   }
 557
 558
 559   if (m_filtercnt>0)
 560   {
 561     if (m_ratio < 1.0 && ret>0) // filter output
 562     {
 563       if (!m_iirfilter) m_iirfilter = new WDL_Resampler_IIRFilter;
 564       int n=m_filtercnt;
 565       m_iirfilter->setParms(m_ratio*m_filterpos,m_filterq);
 566
 567       int x,a;
 568       int offs=0;
 569       for (x=0; x < nch; x ++)
 570         for (a = 0; a < n; a ++)
 571           m_iirfilter->Apply(out+x,out+x,ret,nch,offs++);
 572     }
 573   }
 574
 575
 576
 577   if (ret>0 && rsinbuf_availtemp>m_samples_in_rsinbuf) // we had to pad!!
 578   {
 579     // check for the case where rsinbuf_availtemp>m_samples_in_rsinbuf, decrease ret down to actual valid samples
 580     double adj=(srcpos-m_samples_in_rsinbuf + outlatadj) / drspos;
 581     if (adj>0)
 582     {
 583       ret -= (int) (adj + 0.5);
 584       if (ret<0)ret=0;
 585     }
 586   }
 587
 588   int isrcpos=(int)srcpos;
 589   m_fracpos = srcpos - isrcpos;
 590   m_samples_in_rsinbuf -= isrcpos;
 591   if (m_samples_in_rsinbuf <= 0) m_samples_in_rsinbuf=0;
 592   else
 593     memcpy(localin, localin + isrcpos*nch,m_samples_in_rsinbuf*sizeof(WDL_ResampleSample)*nch);
 594
 595
 596   return ret;
 597 }