WDL/resample.cpp

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