paulxstretch/Source/WDL/resample.cpp

/*
    WDL - resample.cpp
    Copyright (C) 2010 and later Cockos Incorporated

    This software is provided 'as-is', without any express or implied
    warranty.  In no event will the authors be held liable for any damages
    arising from the use of this software.

    Permission is granted to anyone to use this software for any purpose,
    including commercial applications, and to alter it and redistribute it
    freely, subject to the following restrictions:

    1. The origin of this software must not be misrepresented; you must not
       claim that you wrote the original software. If you use this software
       in a product, an acknowledgment in the product documentation would be
       appreciated but is not required.
    2. Altered source versions must be plainly marked as such, and must not be
       misrepresented as being the original software.
    3. This notice may not be removed or altered from any source distribution.
      
    You may also distribute this software under the LGPL v2 or later.

*/

#include "resample.h"
#include <math.h>

#include "denormal.h"

#ifndef PI
#define PI 3.1415926535897932384626433832795
#endif

class WDL_Resampler::WDL_Resampler_IIRFilter
{
public:
  WDL_Resampler_IIRFilter() 
  { 
    m_fpos=-1;
    Reset(); 
  }
  ~WDL_Resampler_IIRFilter()
  {
  }

  void Reset() 
  { 
    memset(m_hist,0,sizeof(m_hist)); 
  }

  void setParms(double fpos, double Q)
  {
    if (fabs(fpos-m_fpos)<0.000001) return;
    m_fpos=fpos;

    double pos = fpos * PI;
    double cpos=cos(pos);
    double spos=sin(pos);
    
    double alpha=spos/(2.0*Q);
    
    double sc=1.0/( 1 + alpha);
    m_b1 = (1-cpos) * sc;
    m_b2 = m_b0 = m_b1*0.5;
    m_a1 =  -2 * cpos * sc;
    m_a2 = (1-alpha)*sc;

  }

  void Apply(WDL_ResampleSample *in1, WDL_ResampleSample *out1, int ns, int span, int w)
  {
    double b0=m_b0,b1=m_b1,b2=m_b2,a1=m_a1,a2=m_a2;
    double *hist=m_hist[w];
    while (ns--)
    {
      double in=*in1;
      in1+=span;
      double out = (double) ( in*b0 + hist[0]*b1 + hist[1]*b2 - hist[2]*a1 - hist[3]*a2);
      hist[1]=hist[0]; hist[0]=in;
      hist[3]=hist[2]; *out1 = hist[2]=denormal_filter_double(out);

      out1+=span;
    }
  }

private:
  double m_fpos;
  double m_a1,m_a2;
  double m_b0,m_b1,m_b2;
  double m_hist[WDL_RESAMPLE_MAX_FILTERS*WDL_RESAMPLE_MAX_NCH][4];
};


void inline WDL_Resampler::SincSample(WDL_ResampleSample *outptr, const WDL_ResampleSample *inptr, double fracpos, int nch, const WDL_SincFilterSample *filter, int filtsz)
{
  const int oversize=m_lp_oversize;

  fracpos *= oversize;
  const int ifpos=(int)fracpos;
  filter += (oversize-ifpos) * filtsz;
  fracpos -= ifpos;

  int x;
  for (x = 0; x < nch; x ++)
  {
    double sum=0.0,sum2=0.0;
    const WDL_SincFilterSample *fptr2=filter;
    const WDL_SincFilterSample *fptr=fptr2 - filtsz;
    const WDL_ResampleSample *iptr=inptr+x;
    int i=filtsz/2;
    while (i--)
    {
      sum += fptr[0]*iptr[0]; 
      sum2 += fptr2[0]*iptr[0]; 
      sum += fptr[1]*iptr[nch]; 
      sum2 += fptr2[1]*iptr[nch]; 
      iptr+=nch*2;
      fptr+=2;
      fptr2+=2;
    }
    outptr[x]=sum*fracpos + sum2*(1.0-fracpos);
  }

}

void inline WDL_Resampler::SincSample1(WDL_ResampleSample *outptr, const WDL_ResampleSample *inptr, double fracpos, const WDL_SincFilterSample *filter, int filtsz)
{
  const int oversize=m_lp_oversize;
  fracpos *= oversize;
  const int ifpos=(int)fracpos;
  fracpos -= ifpos;

  double sum=0.0,sum2=0.0;
  const WDL_SincFilterSample *fptr2=filter + (oversize-ifpos) * filtsz;
  const WDL_SincFilterSample *fptr=fptr2 - filtsz;
  const WDL_ResampleSample *iptr=inptr;
  int i=filtsz/2;
  while (i--)
  {
    sum += fptr[0]*iptr[0]; 
    sum2 += fptr2[0]*iptr[0];
    sum += fptr[1]*iptr[1]; 
    sum2 += fptr2[1]*iptr[1];
    iptr+=2;
    fptr+=2;
    fptr2+=2;
  }
  outptr[0]=sum*fracpos+sum2*(1.0-fracpos);
}

void inline WDL_Resampler::SincSample2(WDL_ResampleSample *outptr, const WDL_ResampleSample *inptr, double fracpos, const WDL_SincFilterSample *filter, int filtsz)
{
  const int oversize=m_lp_oversize;
  fracpos *= oversize;
  const int ifpos=(int)fracpos;
  fracpos -= ifpos;

  const WDL_SincFilterSample *fptr2=filter + (oversize-ifpos) * filtsz;
  const WDL_SincFilterSample *fptr=fptr2 - filtsz;

  double sum=0.0;
  double sum2=0.0;
  double sumb=0.0;
  double sum2b=0.0;
  const WDL_ResampleSample *iptr=inptr;
  int i=filtsz/2;
  while (i--)
  {
    sum += fptr[0]*iptr[0];
    sum2 += fptr[0]*iptr[1];
    sumb += fptr2[0]*iptr[0];
    sum2b += fptr2[0]*iptr[1];
    sum += fptr[1]*iptr[2];
    sum2 += fptr[1]*iptr[3];
    sumb += fptr2[1]*iptr[2];
    sum2b += fptr2[1]*iptr[3];
    iptr+=4;
    fptr+=2;
    fptr2+=2;
  }
  outptr[0]=sum*fracpos + sumb*(1.0-fracpos);
  outptr[1]=sum2*fracpos + sum2b*(1.0-fracpos);

}



WDL_Resampler::WDL_Resampler()
{
  m_filterq=0.707f;
  m_filterpos=0.693f; // .792 ?

  m_sincoversize=0;
  m_lp_oversize=1; 
  m_sincsize=0;
  m_filtercnt=1;
  m_interp=true;
  m_feedmode=false;

  m_filter_coeffs_size=0; 
  m_sratein=44100.0; 
  m_srateout=44100.0; 
  m_ratio=1.0; 
  m_filter_ratio=-1.0; 
  m_iirfilter=0;

  Reset(); 
}

WDL_Resampler::~WDL_Resampler()
{
  delete m_iirfilter;
}

void WDL_Resampler::Reset(double fracpos)
{
  m_last_requested=0;
  m_filtlatency=0;
  m_fracpos=fracpos; 
  m_samples_in_rsinbuf=0; 
  if (m_iirfilter) m_iirfilter->Reset();   
}

void WDL_Resampler::SetMode(bool interp, int filtercnt, bool sinc, int sinc_size, int sinc_interpsize)
{
  m_sincsize = sinc && sinc_size>= 4 ? sinc_size > 8192 ? 8192 : (sinc_size&~1) : 0;
  m_sincoversize = m_sincsize  ? (sinc_interpsize<= 1 ? 1 : sinc_interpsize>=8192 ? 8192 : sinc_interpsize) : 1;

  m_filtercnt = m_sincsize ? 0 : (filtercnt<=0?0 : filtercnt >= WDL_RESAMPLE_MAX_FILTERS ? WDL_RESAMPLE_MAX_FILTERS : filtercnt);
  m_interp=interp && !m_sincsize;
//  char buf[512];
//  sprintf(buf,"setting interp=%d, filtercnt=%d, sinc=%d,%d\n",m_interp,m_filtercnt,m_sincsize,m_sincoversize);
//  OutputDebugString(buf);

  if (!m_sincsize) 
  {
    m_filter_coeffs.Resize(0);
    m_filter_coeffs_size=0;
  }
  if (!m_filtercnt) 
  {
    delete m_iirfilter;
    m_iirfilter=0;
  }
}

void WDL_Resampler::SetRates(double rate_in, double rate_out) 
{
  if (rate_in<1.0) rate_in=1.0;
  if (rate_out<1.0) rate_out=1.0;
  if (rate_in != m_sratein || rate_out != m_srateout)
  {
    m_sratein=rate_in; 
    m_srateout=rate_out;  
    m_ratio=m_sratein / m_srateout;
  }
}


void WDL_Resampler::BuildLowPass(double filtpos) // only called in sinc modes
{
  const int wantsize=m_sincsize;
  const int wantinterp=m_sincoversize;

  if (m_filter_ratio!=filtpos || 
      m_filter_coeffs_size != wantsize ||
      m_lp_oversize != wantinterp)
  {
    m_lp_oversize = wantinterp;
    m_filter_ratio=filtpos;

    // build lowpass filter
    const int allocsize = wantsize*(m_lp_oversize+1);
    WDL_SincFilterSample *cfout=m_filter_coeffs.Resize(allocsize);
    if (m_filter_coeffs.GetSize()==allocsize)
    {
      m_filter_coeffs_size=wantsize;

      const double dwindowpos = 2.0 * PI/(double)wantsize;
      const double dsincpos  = PI * filtpos; // filtpos is outrate/inrate, i.e. 0.5 is going to half rate
      const int hwantsize=wantsize/2;

      double filtpower=0.0;
      WDL_SincFilterSample *ptrout = cfout;
      int slice;
      for (slice=0;slice<=wantinterp;slice++)
      {
        const double frac = slice / (double)wantinterp;
        const int center_x = slice == 0 ? hwantsize : slice == wantinterp ? hwantsize-1 : -1;

        int x;
        for (x=0;x<wantsize;x++)
        {          
          if (x==center_x) 
          {
            // we know this will be 1.0
            *ptrout++ = 1.0;
          }
          else
          {
            const double xfrac = frac + x;
            const double windowpos = dwindowpos * xfrac;
            const double sincpos = dsincpos * (xfrac - hwantsize);

            // blackman-harris * sinc
            const double val = (0.35875 - 0.48829 * cos(windowpos) + 0.14128 * cos(2*windowpos) - 0.01168 * cos(3*windowpos)) * sin(sincpos) / sincpos; 
            if (slice<wantinterp) filtpower+=val;        
            *ptrout++ = (WDL_SincFilterSample)val;
          }

        }
      }

      filtpower = wantinterp/(filtpower+1.0);
      int x;
      for (x = 0; x < allocsize; x ++) 
      {
        cfout[x] = (WDL_SincFilterSample) (cfout[x]*filtpower);
      }
    }
    else m_filter_coeffs_size=0;

  }
}

double WDL_Resampler::GetCurrentLatency() 
{ 
  double v=((double)m_samples_in_rsinbuf-m_filtlatency)/m_sratein;
  
  if (v<0.0)v=0.0;
  return v;
}

int WDL_Resampler::ResamplePrepare(int out_samples, int nch, WDL_ResampleSample **inbuffer) 
{   
  if (nch > WDL_RESAMPLE_MAX_NCH || nch < 1) return 0;

  int fsize=0;
  if (m_sincsize>1) fsize = m_sincsize;

  int hfs=fsize/2;
  if (hfs>1 && m_samples_in_rsinbuf<hfs-1)
  {
    m_filtlatency+=hfs-1 - m_samples_in_rsinbuf;

    m_samples_in_rsinbuf=hfs-1;

    if (m_samples_in_rsinbuf>0)
    {      
      WDL_ResampleSample *p = m_rsinbuf.Resize(m_samples_in_rsinbuf*nch,false);
      memset(p,0,sizeof(WDL_ResampleSample)*m_rsinbuf.GetSize());
    }
  }

  int sreq = 0;
    
  if (!m_feedmode) sreq = (int)(m_ratio * out_samples) + 4 + fsize - m_samples_in_rsinbuf;
  else sreq = out_samples;

  if (sreq<0)sreq=0;
  
again:
  m_rsinbuf.Resize((m_samples_in_rsinbuf+sreq)*nch,false);

  int sz = m_rsinbuf.GetSize()/(nch?nch:1) - m_samples_in_rsinbuf;
  if (sz!=sreq)
  {
    if (sreq>4 && !sz)
    {
      sreq/=2;
      goto again; // try again with half the size
    }
    // todo: notify of error?
    sreq=sz;
  }

  *inbuffer = m_rsinbuf.Get() + m_samples_in_rsinbuf*nch;

  m_last_requested=sreq;
  return sreq;
}



int WDL_Resampler::ResampleOut(WDL_ResampleSample *out, int nsamples_in, int nsamples_out, int nch)
{
  if (nch > WDL_RESAMPLE_MAX_NCH || nch < 1) return 0;

  if (m_filtercnt>0)
  {
    if (m_ratio > 1.0 && nsamples_in > 0) // filter input
    {
      if (!m_iirfilter) m_iirfilter = new WDL_Resampler_IIRFilter;

      int n=m_filtercnt;
      m_iirfilter->setParms((1.0/m_ratio)*m_filterpos,m_filterq);

      WDL_ResampleSample *buf=(WDL_ResampleSample *)m_rsinbuf.Get() + m_samples_in_rsinbuf*nch;
      int a,x;
      int offs=0;
      for (x=0; x < nch; x ++)
        for (a = 0; a < n; a ++)
          m_iirfilter->Apply(buf+x,buf+x,nsamples_in,nch,offs++);
    }
  }

  // prevent the caller from corrupting the internal state
  m_samples_in_rsinbuf += nsamples_in < m_last_requested ? nsamples_in : m_last_requested; 

  int rsinbuf_availtemp = m_samples_in_rsinbuf;

  if (nsamples_in < m_last_requested) // flush out to ensure we can deliver
  {
    int fsize=(m_last_requested-nsamples_in)*2 + m_sincsize*2;

    int alloc_size=(m_samples_in_rsinbuf+fsize)*nch;
    WDL_ResampleSample *zb=m_rsinbuf.Resize(alloc_size,false);
    if (m_rsinbuf.GetSize()==alloc_size)
    {
      memset(zb+m_samples_in_rsinbuf*nch,0,fsize*nch*sizeof(WDL_ResampleSample));
      rsinbuf_availtemp = m_samples_in_rsinbuf+fsize;
    }
  }

  int ret=0;
  double srcpos=m_fracpos;
  double drspos = m_ratio;
  WDL_ResampleSample *localin = m_rsinbuf.Get();

  WDL_ResampleSample *outptr=out;

  int ns=nsamples_out;

  int outlatadj=0;

  if (m_sincsize) // sinc interpolating
  {
    if (m_ratio > 1.0) BuildLowPass(1.0 / (m_ratio*1.03));
    else BuildLowPass(1.0);

    int filtsz=m_filter_coeffs_size;
    int filtlen = rsinbuf_availtemp - filtsz;
    outlatadj=filtsz/2-1;
    WDL_SincFilterSample *filter=m_filter_coeffs.Get();   

    if (nch == 1)
    {
      while (ns--)
      {
        int ipos = (int)srcpos;

        if (ipos >= filtlen-1)  break; // quit decoding, not enough input samples

        SincSample1(outptr,localin + ipos,srcpos-ipos,filter,filtsz);
        outptr ++;
        srcpos+=drspos;
        ret++;
      }
    }
    else if (nch==2)
    {
      while (ns--)
      {
        int ipos = (int)srcpos;

        if (ipos >= filtlen-1)  break; // quit decoding, not enough input samples

        SincSample2(outptr,localin + ipos*2,srcpos-ipos,filter,filtsz);
        outptr+=2;
        srcpos+=drspos;
        ret++;
      }
    }
    else
    {
      while (ns--)
      {
        int ipos = (int)srcpos;

        if (ipos >= filtlen-1)  break; // quit decoding, not enough input samples

        SincSample(outptr,localin + ipos*nch,srcpos-ipos,nch,filter,filtsz);
        outptr += nch;
        srcpos+=drspos;
        ret++;
      }
    }
  }
  else if (!m_interp) // point sampling
  {
    if (nch == 1)
    {
      while (ns--)
      {
        int ipos = (int)srcpos;
        if (ipos >= rsinbuf_availtemp)  break; // quit decoding, not enough input samples

        *outptr++ = localin[ipos];
        srcpos+=drspos;
        ret++;
      }
    }
    else if (nch == 2)
    {
      while (ns--)
      {
        int ipos = (int)srcpos;
        if (ipos >= rsinbuf_availtemp)  break; // quit decoding, not enough input samples

        ipos+=ipos;

        outptr[0] = localin[ipos];
        outptr[1] = localin[ipos+1];
        outptr+=2;
        srcpos+=drspos;
        ret++;
      }
    }
    else
      while (ns--)
      {
        int ipos = (int)srcpos;
        if (ipos >= rsinbuf_availtemp)  break; // quit decoding, not enough input samples
    
        memcpy(outptr,localin + ipos*nch,nch*sizeof(WDL_ResampleSample));
        outptr += nch;
        srcpos+=drspos;
        ret++;
      }
  }
  else // linear interpolation
  {
    if (nch == 1)
    {
      while (ns--)
      {
        int ipos = (int)srcpos;
        double fracpos=srcpos-ipos; 

        if (ipos >= rsinbuf_availtemp-1) 
        {
          break; // quit decoding, not enough input samples
        }

        double ifracpos=1.0-fracpos;
        WDL_ResampleSample *inptr = localin + ipos;
        *outptr++ = inptr[0]*(ifracpos) + inptr[1]*(fracpos);
        srcpos+=drspos;
        ret++;
      }
    }
    else if (nch == 2)
    {
      while (ns--)
      {
        int ipos = (int)srcpos;
        double fracpos=srcpos-ipos; 

        if (ipos >= rsinbuf_availtemp-1) 
        {
          break; // quit decoding, not enough input samples
        }

        double ifracpos=1.0-fracpos;
        WDL_ResampleSample *inptr = localin + ipos*2;
        outptr[0] = inptr[0]*(ifracpos) + inptr[2]*(fracpos);
        outptr[1] = inptr[1]*(ifracpos) + inptr[3]*(fracpos);
        outptr += 2;
        srcpos+=drspos;
        ret++;
      }
    }
    else
    {
      while (ns--)
      {
        int ipos = (int)srcpos;
        double fracpos=srcpos-ipos; 

        if (ipos >= rsinbuf_availtemp-1) 
        {
          break; // quit decoding, not enough input samples
        }

        double ifracpos=1.0-fracpos;
        int ch=nch;
        WDL_ResampleSample *inptr = localin + ipos*nch;
        while (ch--)
        {
          *outptr++ = inptr[0]*(ifracpos) + inptr[nch]*(fracpos);
          inptr++;
        }
        srcpos+=drspos;
        ret++;
      }
    }
  }


  if (m_filtercnt>0)
  {
    if (m_ratio < 1.0 && ret>0) // filter output
    {
      if (!m_iirfilter) m_iirfilter = new WDL_Resampler_IIRFilter;
      int n=m_filtercnt;
      m_iirfilter->setParms(m_ratio*m_filterpos,m_filterq);

      int x,a;
      int offs=0;
      for (x=0; x < nch; x ++)
        for (a = 0; a < n; a ++)
          m_iirfilter->Apply(out+x,out+x,ret,nch,offs++);
    }
  }

  

  if (ret>0 && rsinbuf_availtemp>m_samples_in_rsinbuf) // we had to pad!!
  {
    // check for the case where rsinbuf_availtemp>m_samples_in_rsinbuf, decrease ret down to actual valid samples
    double adj=(srcpos-m_samples_in_rsinbuf + outlatadj) / drspos;
    if (adj>0)
    {
      ret -= (int) (adj + 0.5);
      if (ret<0)ret=0;
    }
  }

  int isrcpos=(int)srcpos;
  if (isrcpos > m_samples_in_rsinbuf) isrcpos=m_samples_in_rsinbuf;
  m_fracpos = srcpos - isrcpos;
  m_samples_in_rsinbuf -= isrcpos;
  if (m_samples_in_rsinbuf <= 0) m_samples_in_rsinbuf=0;
  else
    memmove(localin, localin + isrcpos*nch,m_samples_in_rsinbuf*sizeof(WDL_ResampleSample)*nch);


  return ret;
}