xue@11: /*
xue@11:     Harmonic sinusoidal modelling and tools
xue@11: 
xue@11:     C++ code package for harmonic sinusoidal modelling and relevant signal processing.
xue@11:     Centre for Digital Music, Queen Mary, University of London.
xue@11:     This file copyright 2011 Wen Xue.
xue@11: 
xue@11:     This program is free software; you can redistribute it and/or
xue@11:     modify it under the terms of the GNU General Public License as
xue@11:     published by the Free Software Foundation; either version 2 of the
xue@11:     License, or (at your option) any later version.
xue@11: */
xue@1: //---------------------------------------------------------------------------
xue@1: 
xue@1: #include "splines.h"
xue@1: 
Chris@5: /** \file splines.h */
Chris@5: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function tridiagonal: solves linear system A[N][N]x[N]=d[N] where A is tridiagonal
xue@1: 
xue@1:   In: tridiagonal matrix A[N][N] gives as three vectors - lower subdiagonal
xue@1:         a[1:N-1], main diagonal b[0:N-1], upper subdiagonal c[0:N-2]
xue@1:       vector d[N]
xue@1:   Out:  vector d[N] now containing x[N]
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void tridiagonal(int N, double* a, double* b, double* c, double* d)
xue@1: {
xue@1:   for (int k=1; k<N; k++)
xue@1:   {
xue@1:     double m=a[k]/b[k-1];
xue@1:     b[k]=b[k]-m*c[k-1];
xue@1:     d[k]=d[k]-m*d[k-1];
xue@1:   }
xue@1:   d[N-1]=d[N-1]/b[N-1];
xue@1:   for (int k=N-2; k>=0; k--)
xue@1:     d[k]=(d[k]-c[k]*d[k+1])/b[k];
xue@1: }//tridiagonal
xue@1: 
Chris@5: /**
xue@1:   function CubicSpline: computing piece-wise trinomial coefficients of a cubic spline
xue@1: 
xue@1:   In: x[N+1], y[N+1]: knots
xue@1:       bordermode: boundary mode of cubic spline
xue@1:         bordermode=0: natural: y''(x0)=y''(x_N)=0
xue@1:         bordermode=1: quadratic runout: y''(x0)=y''(x1), y''(x_N)=y''(x_(N-1))
xue@1:         bordermode=2: cubic runout: y''(x0)=2y''(x1)-y''(x2), the same at the end point
xue@1:       mode: spline format
xue@1:         mode=0: spline defined on the l'th piece as y(x)=a[l]x^3+b[l]x^2+c[l]x+d[l]
xue@1:         mode=1: spline defined on the l'th piece as y(x)=a[l]t^3+b[l]t^2+c[l]t+d[l], t=x-x[l]
xue@1:   Out: a[N],b[N],c[N],d[N]: piecewise trinomial coefficients
xue@1:        data[]: cubic spline computed for [xstart:xinterval:xend], if specified
xue@1:   No return value. On start d must be allocated no less than N+1 storage units.
xue@1: */
xue@1: void CubicSpline(int N, double* a, double* b, double* c, double* d, double* x, double* y, int bordermode, int mode, double* data, double xinterval, double xstart, double xend)
xue@1: {
xue@1:   double *h=new double[N];
xue@1:   for (int i=0; i<N; i++) h[i]=x[i+1]-x[i];
xue@1: 
xue@1:   for (int i=0; i<N-1; i++)
xue@1:   {
xue@1:     a[i]=h[i];
xue@1:     b[i]=2*(h[i]+h[i+1]);
xue@1:     c[i]=h[i+1];
xue@1:     d[i+1]=6*((y[i+2]-y[i+1])/h[i+1]-(y[i+1]-y[i])/h[i]);
xue@1:   }
xue@1:   a[0]=0; c[N-2]=0;
xue@1: 
xue@1:   if (bordermode==1)
xue@1:   {
xue@1:     b[0]+=h[0];
xue@1:     b[N-2]+=h[N-1];
xue@1:   }
xue@1:   else if (bordermode==2)
xue@1:   {
xue@1:     b[0]+=2*h[0]; c[0]-=h[0];
xue@1:     b[N-2]+=2*h[N-1]; a[N-2]-=h[N-1];
xue@1:   }
xue@1: 
xue@1:   tridiagonal(N-1, a, b, c, &d[1]);
xue@1: 
xue@1:   if (bordermode==0)
xue@1:   {
xue@1:     d[0]=0; d[N]=0;
xue@1:   }
xue@1:   else if (bordermode==1)
xue@1:   {
xue@1:     d[0]=d[1]; d[N]=d[N-1];
xue@1:   }
xue@1:   else if (bordermode==2)
xue@1:   {
xue@1:     d[0]=2*d[1]-d[2];
xue@1:     d[N]=2*d[N-1]-d[N-2];
xue@1:   }
xue@1: 
xue@1:   for (int i=0; i<N; i++)
xue@1:   {
xue@1:     double zi=d[i], zi1=d[i+1], xi=x[i], xi1=x[i+1], hi=h[i];
xue@1:     double co1=y[i+1]/hi-hi*zi1/6, co2=y[i]/hi-hi*zi/6;
xue@1:     if (mode==0)
xue@1:     {
xue@1:       a[i]=(zi1-zi)/6/hi;
xue@1:       b[i]=(-xi*zi1+xi1*zi)/2/hi;
xue@1:       c[i]=(zi1*xi*xi-zi*xi1*xi1)/2/hi+co1-co2;
xue@1:       d[i]=(-zi1*xi*xi*xi+zi*xi1*xi1*xi1)/6/hi-co1*xi+co2*xi1;
xue@1:     }
xue@1:     else if (mode==1)
xue@1:     {
xue@1:       a[i]=(zi1-zi)/6/hi;
xue@1:       b[i]=zi/2;
xue@1:       c[i]=-zi*hi/2+co1-co2;
xue@1:       d[i]=zi*hi*hi/6+co2*hi;
xue@1:     }
xue@1:     zi=zi1;
xue@1:   }
xue@1:   delete[] h;
xue@1:   if (data)
xue@1:   {
xue@1:     if (xend<xstart) xend=x[N], xstart=x[0];
xue@1:     int Count=(xend-xstart)/xinterval+1;
xue@1:     for (int i=0, n=0; i<Count; i++)
xue@1:     {
xue@1:       double xx=xstart+i*xinterval;
xue@1:       while (n<N-1 && xx>x[n+1]) n++;
xue@1:       if (mode==1) xx=xx-x[n];
xue@1:       data[i]=d[n]+xx*(c[n]+xx*(b[n]+xx*a[n]));
xue@1:     }
xue@1:   }
xue@1: }//CubicSpline
xue@1: 
Chris@5: /**
xue@1:   function CubicSpline: computing piece-wise trinomial coefficients of a cubic spline with uniformly placed knots
xue@1: 
xue@1:   In: y[N+1]: spline values at knots (0, h, 2h, ..., Nh)
xue@1:       bordermode: boundary mode of cubic spline
xue@1:         bordermode=0: natural: y''(x0)=y''(x_N)=0
xue@1:         bordermode=1: quadratic runout: y''(x0)=y''(x1), y''(x_N)=y''(x_(N-1))
xue@1:         bordermode=2: cubic runout: y''(x0)=2y''(x1)-y''(x2), the same at the end point
xue@1:        mode: spline format
xue@1:         mode=0: spline defined on the l'th piece as y(x)=a[l]x^3+b[l]x^2+c[l]x+d[l]
xue@1:         mode=1: spline defined on the l'th piece as y(x)=a[l]t^3+b[l]t^2+c[l]t+d[l], t=x-x[l]
xue@1:   Out:  a[N],b[N],c[N],d[N]: piecewise trinomial coefficients
xue@1:         data[]: cubic spline computed for [xstart:xinterval:xend], if specified
xue@1:   No return value. On start d must be allocated no less than N+1 storage units.
xue@1: */
xue@1: void CubicSpline(int N, double* a, double* b, double* c, double* d, double h, double* y, int bordermode, int mode, double* data, double xinterval, double xstart, double xend)
xue@1: {
xue@1:   for (int i=0; i<N-1; i++)
xue@1:   {
xue@1:     a[i]=h;
xue@1:     b[i]=4*h;
xue@1:     c[i]=h;
xue@1:     d[i+1]=6*((y[i+2]-y[i+1])/h-(y[i+1]-y[i])/h);
xue@1:   }
xue@1:   a[0]=0; c[N-2]=0;
xue@1: 
xue@1:   if (bordermode==1)
xue@1:   {
xue@1:     b[0]+=h;
xue@1:     b[N-2]+=h;
xue@1:   }
xue@1:   else if (bordermode==2)
xue@1:   {
xue@1:     b[0]+=2*h; c[0]-=h;
xue@1:     b[N-2]+=2*h; a[N-2]-=h;
xue@1:   }
xue@1: 
xue@1:   tridiagonal(N-1, a, b, c, &d[1]);
xue@1: 
xue@1:   if (bordermode==0)
xue@1:   {
xue@1:     d[0]=0; d[N]=0;
xue@1:   }
xue@1:   else if (bordermode==1)
xue@1:   {
xue@1:     d[0]=d[1]; d[N]=d[N-1];
xue@1:   }
xue@1:   else if (bordermode==2)
xue@1:   {
xue@1:     d[0]=2*d[1]-d[2];
xue@1:     d[N]=2*d[N-1]-d[N-2];
xue@1:   }
xue@1: 
xue@1:   for (int i=0; i<N; i++)
xue@1:   {
xue@1:     double zi=d[i], zi1=d[i+1];
xue@1:     if (mode==0)
xue@1:     {
xue@1:       double co1=y[i+1]/h-h*zi1/6, co2=y[i]/h-h*zi/6;
xue@1:       a[i]=(zi1-zi)/6/h;
xue@1:       b[i]=(-i*zi1+(i+1)*zi)/2;
xue@1:       c[i]=(zi1*i*i*h-zi*(i+1)*h*(i+1))/2+co1-co2;
xue@1:       d[i]=(-zi1*i*h*i*h*i+zi*(i+1)*h*(i+1)*h*(i+1))/6-co1*i*h+co2*(i+1)*h;
xue@1:     }
xue@1:     else if (mode==1)
xue@1:     {
xue@1:       a[i]=(zi1-zi)/6/h;
xue@1:       b[i]=zi/2;
xue@1:       c[i]=-(zi*2+zi1)*h/6+(y[i+1]-y[i])/h;
xue@1:       d[i]=y[i];
xue@1:     }
xue@1:     zi=zi1;
xue@1:   }
xue@1:   if (data)
xue@1:   {
xue@1:     if (xend<xstart) xend=N*h, xstart=0;
xue@1:     int Count=(xend-xstart)/xinterval+1;
xue@1:     for (int i=0, n=0; i<Count; i++)
xue@1:     {
xue@1:       double xx=xstart+i*xinterval;
xue@1:       while (n<N-1 && xx>(n+1)*h) n++;
xue@1:       if (mode==1) xx=xx-n*h;
xue@1:       data[i]=d[n]+xx*(c[n]+xx*(b[n]+xx*a[n]));
xue@1:     }
xue@1:   }
xue@1: }//CubicSpline
xue@1: 
Chris@5: /**
xue@1:   function Smooth_Interpolate: smoothly interpolate/extrapolate P-piece spline from P-1 values. The
xue@1:   interpolation scheme is chosen according to P:
xue@1:     P-1=1:  constant
xue@1:     P-1=2:  linear function
xue@1:     P-1=3:  quadratic function
xue@1:     P-1>=4: cubic spline
xue@1: 
xue@1:   In: xind[P-1], x[P-1]: knot points
xue@1:   Out:  y[N]: smoothly interpolated signal, y(xind[p])=x[p], p=0, ..., P-2.
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void Smooth_Interpolate(double* y, int N, int P, double* x, double* xind)
xue@1: {
xue@1:   if (P==2)
xue@1:   {
xue@1:     for (int n=0; n<N; n++) y[n]=x[0];
xue@1:   }
xue@1:   else if (P==3)
xue@1:   {
xue@1:     double alp=(x[1]-x[0])/(xind[1]-xind[0]);
xue@1:     for (int n=0; n<N; n++) y[n]=x[0]+(n-xind[0])*alp;
xue@1:   }
xue@1:   else if (P==4)
xue@1:   {
xue@1:     double x0=xind[0], x1=xind[1], x2=xind[2], y0=x[0], y1=x[1], y2=x[2];
xue@1:     double ix0_12=y0/((x0-x1)*(x0-x2)), ix1_02=y1/((x1-x0)*(x1-x2)), ix2_01=y2/((x2-x0)*(x2-x1));
xue@1:     double a=ix0_12+ix1_02+ix2_01,
xue@1:            b=-(x1+x2)*ix0_12-(x0+x2)*ix1_02-(x0+x1)*ix2_01,
xue@1:            c=x1*x2*ix0_12+x0*x2*ix1_02+x0*x1*ix2_01;
xue@1:     for (int n=0; n<N; n++) y[n]=c+n*(b+n*a);
xue@1:   }
xue@1:   else
xue@1:   {
xue@1:     double *fa=new double[P*4], *fb=&fa[P], *fc=&fa[P*2], *fd=&fa[P*3];
xue@1:     CubicSpline(P-2, fa, fb, fc, fd, xind, x, 0, 1, y, 1, 0, N-1);
xue@1:     delete[] fa;
xue@1:   }
xue@1: }//Smooth_Interpolate