annotate splines.cpp @ 13:de3961f74f30 tip

Add Linux/gcc Makefile; build fix

/*
    Harmonic sinusoidal modelling and tools

    C++ code package for harmonic sinusoidal modelling and relevant signal processing.
    Centre for Digital Music, Queen Mary, University of London.
    This file copyright 2011 Wen Xue.

    This program is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public License as
    published by the Free Software Foundation; either version 2 of the
    License, or (at your option) any later version.
*/
//---------------------------------------------------------------------------

#include "splines.h"

/** \file splines.h */

//---------------------------------------------------------------------------
/**
  function tridiagonal: solves linear system A[N][N]x[N]=d[N] where A is tridiagonal

  In: tridiagonal matrix A[N][N] gives as three vectors - lower subdiagonal
        a[1:N-1], main diagonal b[0:N-1], upper subdiagonal c[0:N-2]
      vector d[N]
  Out:  vector d[N] now containing x[N]

  No return value.
*/
void tridiagonal(int N, double* a, double* b, double* c, double* d)
{
  for (int k=1; k<N; k++)
  {
    double m=a[k]/b[k-1];
    b[k]=b[k]-m*c[k-1];
    d[k]=d[k]-m*d[k-1];
  }
  d[N-1]=d[N-1]/b[N-1];
  for (int k=N-2; k>=0; k--)
    d[k]=(d[k]-c[k]*d[k+1])/b[k];
}//tridiagonal

/**
  function CubicSpline: computing piece-wise trinomial coefficients of a cubic spline

  In: x[N+1], y[N+1]: knots
      bordermode: boundary mode of cubic spline
        bordermode=0: natural: y''(x0)=y''(x_N)=0
        bordermode=1: quadratic runout: y''(x0)=y''(x1), y''(x_N)=y''(x_(N-1))
        bordermode=2: cubic runout: y''(x0)=2y''(x1)-y''(x2), the same at the end point
      mode: spline format
        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]
        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]
  Out: a[N],b[N],c[N],d[N]: piecewise trinomial coefficients
       data[]: cubic spline computed for [xstart:xinterval:xend], if specified
  No return value. On start d must be allocated no less than N+1 storage units.
*/
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)
{
  double *h=new double[N];
  for (int i=0; i<N; i++) h[i]=x[i+1]-x[i];

  for (int i=0; i<N-1; i++)
  {
    a[i]=h[i];
    b[i]=2*(h[i]+h[i+1]);
    c[i]=h[i+1];
    d[i+1]=6*((y[i+2]-y[i+1])/h[i+1]-(y[i+1]-y[i])/h[i]);
  }
  a[0]=0; c[N-2]=0;

  if (bordermode==1)
  {
    b[0]+=h[0];
    b[N-2]+=h[N-1];
  }
  else if (bordermode==2)
  {
    b[0]+=2*h[0]; c[0]-=h[0];
    b[N-2]+=2*h[N-1]; a[N-2]-=h[N-1];
  }

  tridiagonal(N-1, a, b, c, &d[1]);

  if (bordermode==0)
  {
    d[0]=0; d[N]=0;
  }
  else if (bordermode==1)
  {
    d[0]=d[1]; d[N]=d[N-1];
  }
  else if (bordermode==2)
  {
    d[0]=2*d[1]-d[2];
    d[N]=2*d[N-1]-d[N-2];
  }

  for (int i=0; i<N; i++)
  {
    double zi=d[i], zi1=d[i+1], xi=x[i], xi1=x[i+1], hi=h[i];
    double co1=y[i+1]/hi-hi*zi1/6, co2=y[i]/hi-hi*zi/6;
    if (mode==0)
    {
      a[i]=(zi1-zi)/6/hi;
      b[i]=(-xi*zi1+xi1*zi)/2/hi;
      c[i]=(zi1*xi*xi-zi*xi1*xi1)/2/hi+co1-co2;
      d[i]=(-zi1*xi*xi*xi+zi*xi1*xi1*xi1)/6/hi-co1*xi+co2*xi1;
    }
    else if (mode==1)
    {
      a[i]=(zi1-zi)/6/hi;
      b[i]=zi/2;
      c[i]=-zi*hi/2+co1-co2;
      d[i]=zi*hi*hi/6+co2*hi;
    }
    zi=zi1;
  }
  delete[] h;
  if (data)
  {
    if (xend<xstart) xend=x[N], xstart=x[0];
    int Count=(xend-xstart)/xinterval+1;
    for (int i=0, n=0; i<Count; i++)
    {
      double xx=xstart+i*xinterval;
      while (n<N-1 && xx>x[n+1]) n++;
      if (mode==1) xx=xx-x[n];
      data[i]=d[n]+xx*(c[n]+xx*(b[n]+xx*a[n]));
    }
  }
}//CubicSpline

/**
  function CubicSpline: computing piece-wise trinomial coefficients of a cubic spline with uniformly placed knots

  In: y[N+1]: spline values at knots (0, h, 2h, ..., Nh)
      bordermode: boundary mode of cubic spline
        bordermode=0: natural: y''(x0)=y''(x_N)=0
        bordermode=1: quadratic runout: y''(x0)=y''(x1), y''(x_N)=y''(x_(N-1))
        bordermode=2: cubic runout: y''(x0)=2y''(x1)-y''(x2), the same at the end point
       mode: spline format
        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]
        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]
  Out:  a[N],b[N],c[N],d[N]: piecewise trinomial coefficients
        data[]: cubic spline computed for [xstart:xinterval:xend], if specified
  No return value. On start d must be allocated no less than N+1 storage units.
*/
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)
{
  for (int i=0; i<N-1; i++)
  {
    a[i]=h;
    b[i]=4*h;
    c[i]=h;
    d[i+1]=6*((y[i+2]-y[i+1])/h-(y[i+1]-y[i])/h);
  }
  a[0]=0; c[N-2]=0;

  if (bordermode==1)
  {
    b[0]+=h;
    b[N-2]+=h;
  }
  else if (bordermode==2)
  {
    b[0]+=2*h; c[0]-=h;
    b[N-2]+=2*h; a[N-2]-=h;
  }

  tridiagonal(N-1, a, b, c, &d[1]);

  if (bordermode==0)
  {
    d[0]=0; d[N]=0;
  }
  else if (bordermode==1)
  {
    d[0]=d[1]; d[N]=d[N-1];
  }
  else if (bordermode==2)
  {
    d[0]=2*d[1]-d[2];
    d[N]=2*d[N-1]-d[N-2];
  }

  for (int i=0; i<N; i++)
  {
    double zi=d[i], zi1=d[i+1];
    if (mode==0)
    {
      double co1=y[i+1]/h-h*zi1/6, co2=y[i]/h-h*zi/6;
      a[i]=(zi1-zi)/6/h;
      b[i]=(-i*zi1+(i+1)*zi)/2;
      c[i]=(zi1*i*i*h-zi*(i+1)*h*(i+1))/2+co1-co2;
      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;
    }
    else if (mode==1)
    {
      a[i]=(zi1-zi)/6/h;
      b[i]=zi/2;
      c[i]=-(zi*2+zi1)*h/6+(y[i+1]-y[i])/h;
      d[i]=y[i];
    }
    zi=zi1;
  }
  if (data)
  {
    if (xend<xstart) xend=N*h, xstart=0;
    int Count=(xend-xstart)/xinterval+1;
    for (int i=0, n=0; i<Count; i++)
    {
      double xx=xstart+i*xinterval;
      while (n<N-1 && xx>(n+1)*h) n++;
      if (mode==1) xx=xx-n*h;
      data[i]=d[n]+xx*(c[n]+xx*(b[n]+xx*a[n]));
    }
  }
}//CubicSpline

/**
  function Smooth_Interpolate: smoothly interpolate/extrapolate P-piece spline from P-1 values. The
  interpolation scheme is chosen according to P:
    P-1=1:  constant
    P-1=2:  linear function
    P-1=3:  quadratic function
    P-1>=4: cubic spline

  In: xind[P-1], x[P-1]: knot points
  Out:  y[N]: smoothly interpolated signal, y(xind[p])=x[p], p=0, ..., P-2.

  No return value.
*/
void Smooth_Interpolate(double* y, int N, int P, double* x, double* xind)
{
  if (P==2)
  {
    for (int n=0; n<N; n++) y[n]=x[0];
  }
  else if (P==3)
  {
    double alp=(x[1]-x[0])/(xind[1]-xind[0]);
    for (int n=0; n<N; n++) y[n]=x[0]+(n-xind[0])*alp;
  }
  else if (P==4)
  {
    double x0=xind[0], x1=xind[1], x2=xind[2], y0=x[0], y1=x[1], y2=x[2];
    double ix0_12=y0/((x0-x1)*(x0-x2)), ix1_02=y1/((x1-x0)*(x1-x2)), ix2_01=y2/((x2-x0)*(x2-x1));
    double a=ix0_12+ix1_02+ix2_01,
           b=-(x1+x2)*ix0_12-(x0+x2)*ix1_02-(x0+x1)*ix2_01,
           c=x1*x2*ix0_12+x0*x2*ix1_02+x0*x1*ix2_01;
    for (int n=0; n<N; n++) y[n]=c+n*(b+n*a);
  }
  else
  {
    double *fa=new double[P*4], *fb=&fa[P], *fc=&fa[P*2], *fd=&fa[P*3];
    CubicSpline(P-2, fa, fb, fc, fd, xind, x, 0, 1, y, 1, 0, N-1);
    delete[] fa;
  }
}//Smooth_Interpolate