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 <math.h>
Chris@2: #include <string.h>
xue@1: #include "wavelet.h"
xue@1: #include "matrix.h"
xue@1: 
Chris@5: /** \file wavelet.h */
Chris@5: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function csqrt: real implementation of complex square root z=sqrt(x)
xue@1: 
xue@1:   In: xr and xi: real and imaginary parts of x
xue@1:   Out:  zr and zi: real and imaginary parts of z=sqrt(x)
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void csqrt(double& zr, double& zi, double xr, double xi)
xue@1: {
xue@1:   if (xi==0)
xue@1:     if (xr>=0) zr=sqrt(xr), zi=0;
xue@1:     else zi=sqrt(-xr), zr=0;
xue@1:   else
xue@1:   {
xue@1:     double xm=sqrt(xr*xr+xi*xi);
xue@1:     double ri=sqrt((xm-xr)/2);
xue@1:     zr=xi/2/ri;
xue@1:     zi=ri;
xue@1:   }
xue@1: }//csqrt
xue@1: 
Chris@5: /**
xue@1:   function Daubechies: calculates the Daubechies filter of a given order p
xue@1: 
xue@1:   In: filter order p
xue@1:   Out:  h[2p]: the 2p FIR coefficients
xue@1: 
xue@1:   No reutrn value. The calculated filters are minimum phase, which means the energy concentrates at the
xue@1:   beginning. This is usually used for reconstruction. On the contrary, for wavelet analysis the filter
xue@1:   is mirrored.
xue@1: */
xue@1: void Daubechies(int p, double* h)
xue@1: {
xue@1: //initialize h(z)
xue@1:   double r01=pow(2, -p-p+1.5);
xue@1: 
xue@1:   h[0]=1;
xue@1:   for (int i=1; i<=p; i++)
xue@1:   {
xue@1:     h[i]=h[i-1]*(p+1-i)/i;
xue@1:   }
xue@1: 
xue@1:   //construct polynomial p
xue@1:   double *P=new double[p], *rp=new double[p], *ip=new double[p];
xue@1: 
xue@1:   P[p-1]=1;
xue@1:   double r02=1;
xue@1:   for (int i=p-1; i>0; i--)
xue@1:   {
xue@1:     double rate=(i+1-1.0)/(p-2.0+i+1);
xue@1:     P[i-1]=P[i]*rate;
xue@1:     r02/=rate;
xue@1:   }
xue@1:   Roots(p-1, P, rp, ip);
xue@1:   for (int i=0; i<p-1; i++)
xue@1:   {
xue@1:   //current length of h is p+1+i, h[0:p+i]
xue@1:     if (i<p-2 && rp[i]==rp[i+1] && ip[i]==-ip[i+1])
xue@1:     {
xue@1:       double ar=rp[i], ai=ip[i];
xue@1:       double bkr=-2*ar+1, bki=-2*ai, ckr=4*(ar*ar-ai*ai-ar), cki=4*(2*ar*ai-ai), dlr, dli;
xue@1:       csqrt(dlr, dli, ckr, cki);
xue@1:       double akr=bkr+dlr, aki=bki+dli;
xue@1:       if (akr*akr+aki*aki>1) akr=bkr-dlr, aki=bki-dli;
xue@1:       double ak1=-2*akr, ak2=akr*akr+aki*aki;
xue@1:       h[p+2+i]=ak2*h[p+i];
xue@1:       h[p+1+i]=ak2*h[p-1+i]+ak1*h[p+i];
xue@1:       for (int j=p+i; j>1; j--) h[j]=h[j]+ak1*h[j-1]+ak2*h[j-2];
xue@1:       h[1]=h[1]+ak1*h[0];
xue@1:       r02/=ak2;
xue@1:       i++;
xue@1:     }
xue@1:     else //real root of P
xue@1:     {
xue@1:       double ak, bk=-(2*rp[i]-1), delk=4*rp[i]*(rp[i]-1);
xue@1:       if (bk>0) ak=bk-sqrt(delk);
xue@1:       else ak=bk+sqrt(delk);
xue@1:       r02/=ak;
xue@1:       h[p+1+i]=-ak*h[p+i];
xue@1:       for (int j=p+i; j>0; j--) h[j]=h[j]-ak*h[j-1];
xue@1:     }
xue@1:   }
xue@1:   delete[] P; delete[] rp; delete[] ip;
xue@1:   r01=r01*sqrt(r02);
xue@1:   for (int i=0; i<p*2; i++) h[i]*=r01;
xue@1: }//Daubechies
xue@1: 
xue@1: /*
xue@1:     Periodic wavelet decomposition and reconstruction routines
xue@1: 
xue@1:     The wavelet transform of an N-point sequence is arranged in "interleaved" format
xue@1:     as another N-point sequance. Level 1 details are found at N/2 points 1, 3, 5, ...,
xue@1:     N-1; level 2 details are found at N/4 points 2, 6, ..., N-2; level 3 details are
xue@1:     found at N/8 points 4, 12, ..., N-4; etc.
xue@1: */
xue@1: 
Chris@5: /**
xue@1:   function dwtpqmf: in this implementation h and g are the same as reconstruction qmf filters. In fact
xue@1:   the actual filters used are their mirrors and filter origin are aligned to the ends of the real
xue@1:   filters, which turn out to be the starts of h and g.
xue@1: 
xue@1:   The inverse transform is idwtp(), the same as inversing dwtp().
xue@1: 
xue@1:   In: in[Count]:  waveform data
xue@1:       h[M], g[M]: quadratic mirror filter pair
xue@1:       N:  maximal time resolution
xue@1:         Count=kN, N=2^lN being the reciprocal of the most detailed frequency scale, i.e.
xue@1:         N=1 for no transforming at all, N=2 for dividing into approx. and detail,
xue@1:         N=4 for dividing into approx+detail(approx+detial), etc.
xue@1:         Count*2/N=2k gives the smallest length to be convolved with h and g.
xue@1:   Out: out[N], the wavelet transform, arranged in interleaved format.
xue@1: 
xue@1:   Returns maximal atom length (should equal N).
xue@1: */
xue@1: int dwtpqmf(double* in, int Count, int N, double* h, double* g, int M, double* out)
xue@1: {
xue@1:   double* tmp=new double[Count];
xue@1:   double *tmpa=tmp, *tmpla=in;
xue@1:   int C=Count, L=0, n=1;
xue@1: 
xue@1: A:
xue@1:   {
xue@1:     //C: signal length at current layer
xue@1:     //L: layer index, 0 being most detailed
xue@1:     //n: atom length on current layer, equals 2^L.
xue@1:     //C*n=(C<<L)=Count
xue@1:     double* tmpd=&tmpa[C/2];
xue@1:     for (int i=0; i<C; i+=2)
xue@1:     {
xue@1:       int i2=i/2;
xue@1:       tmpa[i2]=tmpla[i]*h[0];
xue@1:       tmpd[i2]=tmpla[i]*g[0];
xue@1:       for (int j=1; j<M; j++)
xue@1:       {
xue@1:         if (i+j<C)
xue@1:         {
xue@1:           tmpa[i2]+=tmpla[i+j]*h[j];
xue@1:           tmpd[i2]+=tmpla[i+j]*g[j];
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           tmpa[i2]+=tmpla[i+j-C]*h[j];
xue@1:           tmpd[i2]+=tmpla[i+j-C]*g[j];
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     for (int i=0; i*2+1<C; i++) out[(2*i+1)<<L]=tmpd[i];
xue@1:     for (int i=0; i*2<C; i++) out[i<<(L+1)]=tmpa[i];
xue@1:     n*=2;
xue@1:     if (n<N)
xue@1:     {
xue@1:       tmpla=tmpa;
xue@1:       tmpa=tmpd;
xue@1:       L++;
xue@1:       C/=2;
xue@1:       goto A;
xue@1:     }
xue@1:   }
xue@1:   delete[] tmp;
xue@1:   return n;
xue@1: }//dwtpqmf
xue@1: 
Chris@5: /**
xue@1:   function dwtp: in this implementation h and g can be different from mirrored reconstruction filters,
xue@1:   i.e. the biorthogonal transform. h[0] and g[0] are aligned at the ends of the filters, i.e. h[-M+1:0],
xue@1:   g[-M+1:0].
xue@1: 
xue@1:   In: in[Count]:  waveform data
xue@1:       h[-M+1:0], g[-M+1:0]: quadratic mirror filter pair
xue@1:       N:  maximal time resolution
xue@1:   Out: out[N], the wavelet transform, arranged in interleaved format.
xue@1: 
xue@1:   Returns maximal atom length (should equal N).
xue@1: */
xue@1: int dwtp(double* in, int Count, int N, double* h, double* g, int M, double* out)
xue@1: {
xue@1:   double* tmp=new double[Count];
xue@1:   double *tmpa=tmp, *tmpla=in;
xue@1:   int C=Count, L=0, n=1;
xue@1: 
xue@1: A:
xue@1:   {   
xue@1:     double* tmpd=&tmpa[C/2];
xue@1:     for (int i=0; i<C; i+=2)
xue@1:     {
xue@1:       int i2=i/2;
xue@1:       tmpa[i2]=tmpla[i]*h[0];
xue@1:       tmpd[i2]=tmpla[i]*g[0];
xue@1:       for (int j=-1; j>-M; j--)
xue@1:       {
xue@1:         if (i-j<C)
xue@1:         {
xue@1:           tmpa[i2]+=tmpla[i-j]*h[j];
xue@1:           tmpd[i2]+=tmpla[i-j]*g[j];
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           tmpa[i2]+=tmpla[i-j-C]*h[j];
xue@1:           tmpd[i2]+=tmpla[i-j-C]*g[j];
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     for (int i=0; i*2+1<C; i++) out[(2*i+1)<<L]=tmpd[i];
xue@1:     for (int i=0; i*2<C; i++) out[i<<(L+1)]=tmpa[i];
xue@1:     n*=2;
xue@1:     if (n<N)
xue@1:     {
xue@1:       tmpla=tmpa;
xue@1:       tmpa=tmpd;
xue@1:       L++;
xue@1:       C/=2;
xue@1:       goto A;
xue@1:     }
xue@1:   }
xue@1:   delete[] tmp;
xue@1:   return n;
xue@1: }//dwtp
xue@1: 
Chris@5: /**
xue@1:   function dwtp: in this implementation h and g can be different size. h[0] and g[0] are aligned at the
xue@1:   ends of the filters, i.e. h[-Mh+1:0], g[-Mg+1:0].
xue@1: 
xue@1:   In: in[Count]:  waveform data
xue@1:       h[-Mh+1:0], g[-Mg+1:0]: quadratic mirror filter pair
xue@1:       N:  maximal time resolution
xue@1:   Out: out[N], the wavelet transform, arranged in interleaved format.
xue@1: 
xue@1:   Returns maximal atom length (should equal N).
xue@1: */
xue@1: int dwtp(double* in, int Count, int N, double* h, int Mh, double* g, int Mg, double* out)
xue@1: {
xue@1:   double* tmp=new double[Count];
xue@1:   double *tmpa=tmp, *tmpla=in;
xue@1:   int C=Count, L=0, n=1;
xue@1: 
xue@1: A:
xue@1:   {   
xue@1:     double* tmpd=&tmpa[C/2];
xue@1:     for (int i=0; i<C; i+=2)
xue@1:     {
xue@1:       int i2=i/2;
xue@1:       tmpa[i2]=tmpla[i]*h[0];
xue@1:       tmpd[i2]=tmpla[i]*g[0];
xue@1:       for (int j=-1; j>-Mh; j--)
xue@1:       {
xue@1:         if (i-j<C)
xue@1:         {
xue@1:           tmpa[i2]+=tmpla[i-j]*h[j];
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           tmpa[i2]+=tmpla[i-j-C]*h[j];
xue@1:         }
xue@1:       }
xue@1:       for (int j=-1; j>-Mg; j--)
xue@1:       {
xue@1:         if (i-j<C)
xue@1:         {
xue@1:           tmpd[i2]+=tmpla[i-j]*g[j];
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           tmpd[i2]+=tmpla[i-j-C]*g[j];
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     for (int i=0; i*2+1<C; i++) out[(2*i+1)<<L]=tmpd[i];
xue@1:     for (int i=0; i*2<C; i++) out[i<<(L+1)]=tmpa[i];
xue@1:     n*=2;
xue@1:     if (n<N)
xue@1:     {
xue@1:       tmpla=tmpa;
xue@1:       tmpa=tmpd;
xue@1:       L++;
xue@1:       C/=2;
xue@1:       goto A;
xue@1:     }
xue@1:   }
xue@1:   delete[] tmp;
xue@1:   return n;
xue@1: }//dwtp
xue@1: 
Chris@5: /**
xue@1:   function dwtp: in this implementation h and g can be arbitrarily located: h from $sh to $eh, g from
xue@1:   $sg to $eg.
xue@1: 
xue@1:   In: in[Count]:  waveform data
xue@1:       h[sh:eh-1], g[sg:eg-1]: quadratic mirror filter pair
xue@1:       N:  maximal time resolution
xue@1:   Out: out[N], the wavelet transform, arranged in interleaved format.
xue@1: 
xue@1:   Returns maximal atom length (should equal N).
xue@1: */
xue@1: int dwtp(double* in, int Count, int N, double* h, int sh, int eh, double* g, int sg, int eg, double* out)
xue@1: {
xue@1:   double* tmp=new double[Count];
xue@1:   double *tmpa=tmp, *tmpla=in;
xue@1:   int C=Count, L=0, n=1;
xue@1: 
xue@1: A:
xue@1:   {
xue@1:     double* tmpd=&tmpa[C/2];
xue@1:     for (int i=0; i<C; i+=2)
xue@1:     {
xue@1:       int i2=i/2;
xue@1:       tmpa[i2]=0;//tmpla[i]*h[0];
xue@1:       tmpd[i2]=0;//tmpla[i]*g[0];
xue@1:       for (int j=sh; j<=eh; j++)
xue@1:       {
xue@1:         int ind=i-j;
xue@1:         if (ind>=C) tmpa[i2]+=tmpla[ind-C]*h[j];
xue@1:         else if (ind<0) tmpa[i2]+=tmpla[ind+C]*h[j];
xue@1:         else tmpa[i2]+=tmpla[ind]*h[j];
xue@1:       }
xue@1:       for (int j=sg; j<=eg; j++)
xue@1:       {
xue@1:         int ind=i-j;
xue@1:         if (ind>=C) tmpd[i2]+=tmpla[i-j-C]*g[j];
xue@1:         else if (ind<0) tmpd[i2]+=tmpla[i-j+C]*g[j];
xue@1:         else tmpd[i2]+=tmpla[i-j]*g[j];
xue@1:       }
xue@1:     }
xue@1:     for (int i=0; i*2+1<C; i++) out[(2*i+1)<<L]=tmpd[i];
xue@1:     for (int i=0; i*2<C; i++) out[i<<(L+1)]=tmpa[i];
xue@1:     n*=2;
xue@1:     if (n<N)
xue@1:     {
xue@1:       tmpla=tmpa;
xue@1:       tmpa=tmpd;
xue@1:       L++;
xue@1:       C/=2;
xue@1:       goto A;
xue@1:     }
xue@1:   }
xue@1:   delete[] tmp;
xue@1:   return n;
xue@1: }//dwtp
xue@1: 
Chris@5: /**
xue@1:   function idwtp: periodic wavelet reconstruction by qmf
xue@1: 
xue@1:   In: in[Count]: wavelet transform in interleaved format
xue@1:       h[M], g[M]: quadratic mirror filter pair
xue@1:       N:  maximal time resolution
xue@1:   Out: out[N], waveform data (detail level 0).
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void idwtp(double* in, int Count, int N, double* h, double* g, int M, double* out)
xue@1: {
xue@1:   double* tmp=new double[Count];
xue@1:   memcpy(out, in, sizeof(double)*Count);
xue@1:   int n=N, C=Count/N, L=log2(N)-1;
xue@1:   while (n>1)
xue@1:   {
xue@1:     memset(tmp, 0, sizeof(double)*C*2);
xue@1:     //2k<<L being the approx, (2k+1)<<L being the detail
xue@1:     for (int i=0; i<C; i++)
xue@1:     {
xue@1:       for (int j=0; j<M; j++)
xue@1:       {
xue@1:         if (i*2+j<C*2)
xue@1:         {
xue@1:           tmp[i*2+j]+=out[(2*i)<<L]*h[j]+out[(2*i+1)<<L]*g[j];
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           tmp[i*2+j-C*2]+=out[(2*i)<<L]*h[j]+out[(2*i+1)<<L]*g[j];
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     for (int i=0; i<C*2; i++) out[i<<L]=tmp[i];
xue@1:     n/=2;
xue@1:     C*=2;
xue@1:     L--;
xue@1:   }
xue@1:   delete[] tmp;
xue@1: }//idwtp
xue@1: 
Chris@5: /**
xue@1:   function idwtp: in which h and g can have different length.
xue@1: 
xue@1:   In: in[Count]: wavelet transform in interleaved format
xue@1:       h[Mh], g[Mg]: quadratic mirror filter pair
xue@1:       N:  maximal time resolution
xue@1:   Out: out[N], waveform data (detail level 0).
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void idwtp(double* in, int Count, int N, double* h, int Mh, double* g, int Mg, double* out)
xue@1: {
xue@1:   double* tmp=new double[Count];
xue@1:   memcpy(out, in, sizeof(double)*Count);
xue@1:   int n=N, C=Count/N, L=log2(N)-1;
xue@1:   while (n>1)
xue@1:   {
xue@1:     memset(tmp, 0, sizeof(double)*C*2);
xue@1:     //2k<<L being the approx, (2k+1)<<L being the detail
xue@1:     for (int i=0; i<C; i++)
xue@1:     {
xue@1:       for (int j=0; j<Mh; j++)
xue@1:       {
xue@1:         int ind=i*2+j+(Mg-Mh)/2;
xue@1:         if (ind>=C*2)
xue@1:         {
xue@1:           tmp[ind-C*2]+=out[(2*i)<<L]*h[j];
xue@1:         }
xue@1:         else if (ind<0)
xue@1:         {
xue@1:           tmp[ind+C*2]+=out[(2*i)<<L]*h[j];
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           tmp[ind]+=out[(2*i)<<L]*h[j];
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     for (int i=0; i<C; i++)
xue@1:     {
xue@1:       for (int j=0; j<Mg; j++)
xue@1:       {
xue@1:         int ind=i*2+j+(Mh-Mg)/2;
xue@1:         if (ind>=C*2)
xue@1:         {
xue@1:           tmp[ind-C*2]+=out[(2*i+1)<<L]*g[j];
xue@1:         }
xue@1:         else if (ind<0)
xue@1:         {
xue@1:           tmp[ind+C*2]+=out[(2*i+1)<<L]*g[j];
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           tmp[ind]+=out[(2*i+1)<<L]*g[j];
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     for (int i=0; i<C*2; i++) out[i<<L]=tmp[i];
xue@1:     n/=2;
xue@1:     C*=2;
xue@1:     L--;
xue@1:   }
xue@1:   delete[] tmp;
xue@1: }//idwtp
xue@1: 
Chris@5: /**
xue@1:   function idwtp: in which h and g can be arbitrarily located: h from $sh to $eh, g from $sg to $eg
xue@1: 
xue@1:   In: in[Count]: wavelet transform in interleaved format
xue@1:       h[sh:eh-1], g[sg:eg-1]: quadratic mirror filter pair
xue@1:       N: maximal time resolution
xue@1:   Out: out[N], waveform data (detail level 0).
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void idwtp(double* in, int Count, int N, double* h, int sh, int eh, double* g, int sg, int eg, double* out)
xue@1: {
xue@1:   double* tmp=new double[Count];
xue@1:   memcpy(out, in, sizeof(double)*Count);
xue@1:   int n=N, C=Count/N, L=log2(N)-1;
xue@1:   while (n>1)
xue@1:   {
xue@1:     memset(tmp, 0, sizeof(double)*C*2);
xue@1:     //2k<<L being the approx, (2k+1)<<L being the detail
xue@1:     for (int i=0; i<C; i++)
xue@1:     {
xue@1:       for (int j=sh; j<=eh; j++)
xue@1:       {
xue@1:         int ind=i*2+j;
xue@1:         if (ind>=C*2) tmp[ind-C*2]+=out[(2*i)<<L]*h[j];
xue@1:         else if (ind<0) tmp[ind+C*2]+=out[(2*i)<<L]*h[j];
xue@1:         else tmp[ind]+=out[(2*i)<<L]*h[j];
xue@1:       }
xue@1:     }
xue@1:     for (int i=0; i<C; i++)
xue@1:     {
xue@1:       for (int j=sg; j<=eg; j++)
xue@1:       {
xue@1:         int ind=i*2+j;
xue@1:         if (ind>=C*2) tmp[ind-C*2]+=out[(2*i+1)<<L]*g[j];
xue@1:         else if (ind<0) tmp[ind+C*2]+=out[(2*i+1)<<L]*g[j];
xue@1:         else tmp[ind]+=out[(2*i+1)<<L]*g[j];
xue@1:       }
xue@1:     }
xue@1:     for (int i=0; i<C*2; i++) out[i<<L]=tmp[i];
xue@1:     n/=2;
xue@1:     C*=2;
xue@1:     L--;
xue@1:   }
xue@1:   delete[] tmp;
xue@1: }//idwtp
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: 
xue@1: /*
xue@1:   Spline biorthogonal wavelet routines.
xue@1: 
xue@1:   Further reading: "Calculation of biorthogonal spline wavelets.pdf"
xue@1: */
xue@1: 
xue@1: //function Cmb: combination number C(n, k) (n>=k>=0)
xue@1: int Cmb(int n, int k)
xue@1: {
xue@1:   if (k>n/2) k=n-k;
xue@1:   int c=1;
xue@1:   for (int i=1; i<=k; i++) c=c*(n+1-i)/i;
xue@1:   return c;
xue@1: }
xue@1: 
Chris@5: /**
xue@1:   function splinewl: computes spline biorthogonal wavelet filters. This version of splinewl calcualtes
xue@1:   the positive-time half of h and hm coefficients only.
xue@1: 
xue@1:   p1 and p2 must have the same parity. If p1 is even, p1 coefficients will be returned in h1; if p1 is
xue@1:   odd, p1-1 coefficients will be returned in h1.
xue@1: 
xue@1:   Actual length of h is p1+1, of hm is p1+2p2-1. only a half of each is kept.
xue@1:   p even: h[0:p1/2] <- [p1/2:p1], hm[0:p1/2+p2-1] <- [p1/2+p2-1:p1+2p2-2]
xue@1:   p odd: h[0:(p1-1)/2] <- [(p1+1)/2:p1], hm[0:(p1-3)/2+p2] <- [(p1-1)/2+p2:p1+2p2-2]
xue@1:   i.e. h[0:hp1] <- [p1-hp1:p1], hm[0:hp1+p2-1] <- [p1-hp1-1+p2:p1+2p2-2]
xue@1: 
xue@1:   In: p1, p2: specify vanishing moments of h and hm
xue@1:   Out: h[] and hm[] as specified above.
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void splinewl(int p1, int p2, double* h, double* hm)
xue@1: {
xue@1:   int hp1=p1/2, hp2=p2/2;
xue@1:   int q=(p1+p2)/2;
xue@1:   h[hp1]=sqrt(2.0)*pow(2, -p1);
xue@1: //  h1[hp1]=1;
xue@1:   for (int i=1, j=hp1-1; i<=hp1; i++, j--)
xue@1:   {
xue@1:     h[j]=h[j+1]*(p1+1-i)/i;
xue@1:   }
xue@1: 
xue@1:   double *_hh1=new double[p2+1], *_hh2=new double[2*q];
xue@1:   double *hh1=&_hh1[p2-hp2], *hh2=&_hh2[q];
xue@1: 
xue@1:   hh1[hp2]=sqrt(2.0)*pow(2, -p2);
xue@1:   for (int i=1, j=hp2-1; i<=hp2; i++, j--)
xue@1:   {
xue@1:     hh1[j]=hh1[j+1]*(p2+1-i)/i;
xue@1:   }
xue@1:   if (p2%2) //p2 is odd
xue@1:   {
xue@1:     for (int i=0; i<=hp2; i++) hh1[-i-1]=hh1[i];
xue@1:   }
xue@1:   else //p2 even
xue@1:   {
xue@1:     for (int i=1; i<=hp2; i++) hh1[-i]=hh1[i];
xue@1:   }
xue@1: 
xue@1:   memset(_hh2, 0, sizeof(double)*2*q);
xue@1:   for (int n=1-q; n<=q-1; n++)
xue@1:   {
xue@1:     int k=abs(n);
xue@1:     int CC1=Cmb(q-1+k, k), CC2=Cmb(2*k, k-n);  //CC=1.0*C(q-1+k, k)*C(2*k, k-n)
xue@1:     for (; k<=q-1; k++)
xue@1:     {
xue@1:       hh2[n]=hh2[n]+1.0*CC1*CC2*pow(0.25, k);
xue@1:       CC1=CC1*(q+k)/(k+1);
xue@1:       CC2=CC2*(2*k+1)*(2*k+2)/((k+1-n)*(k+1+n));
xue@1:     }
xue@1:     hh2[n]*=pow(-1, n);
xue@1:   }
xue@1: 
xue@1:   //hh1[hp2-p2:hp2], hh2[1-q:q-1]
xue@1:   //h2=conv(hh1, hh2), but the positive half only
xue@1:   memset(hm, 0, sizeof(double)*(hp1+p2));
xue@1:   for (int i=hp2-p2; i<=hp2; i++)
xue@1:     for (int j=1-q; j<=q-1; j++)
xue@1:   {
xue@1:     if (i+j>=0 && i+j<hp1+p2)
xue@1:       hm[i+j]+=hh1[i]*hh2[j];
xue@1:   }
xue@1:   
xue@1:   delete[] _hh1;
xue@1:   delete[] _hh2;
xue@1: }//splinewl
xue@1: 
xue@1: 
Chris@5: /**
xue@1:   function splinewl: calculates the analysis and reconstruction filter pairs of spline biorthogonal
xue@1:   wavelet (h, g) and (hm, gm). h has the size p1+1, hm has the size p1+2p2-1.
xue@1: 
xue@1:   If p1+1 is odd, then all four filters are symmetric; if not, then h and hm are symmetric, while g and
xue@1:   gm are anti-symmetric.
xue@1: 
xue@1:   The concatenation of filters h with hm (or g with gm) introduces a time shift of p1+p2-1, which is the
xue@1:   return value multiplied by -1.
xue@1: 
xue@1:   If normmode==1, the results are normalized so that ||h||^2=||g||^2=1;
xue@1:   if normmode==2, the results are normalized so that ||hm||^2=||gm||^2=1,
xue@1:   if normmode==3, the results are normalized so that ||h||^2==||g||^2=||hm||^2=||gm||^2.
xue@1: 
xue@1:   If a *points* buffer is specified, the function returns the starting and ending
xue@1:     positions (inclusive) of h, hm, g, and gm, in the order of (hs, he, hms, hme,
xue@1:     gs, ge, gms, gme), as ps[0]~ps[7].
xue@1: 
xue@1:   In: p1 and p2, specify vanishing moments of h and hm, respectively.
xue@1:       normmode: mode for normalization
xue@1:   Out: h[p1+1], g[p1+1], hm[p1+2p2-1], gm[p1+2p2-1], points[8] (optional)
xue@1: 
xue@1:   Returns -p1-p2+1.
xue@1: */
xue@1: int splinewl(int p1, int p2, double* h, double* hm, double* g, double* gm, int normmode, int* points)
xue@1: {
xue@1:   int lf=p1+1, lb=p1+2*p2-1;
xue@1:   int hlf=lf/2, hlb=lb/2;
xue@1: 
xue@1:   double *h1=&h[hlf], *h2=&hm[hlb];
xue@1:   int hp1=p1/2, hp2=p2/2;
xue@1:   int q=(p1+p2)/2;
xue@1:   h1[hp1]=sqrt(2.0)*pow(2, -p1);
xue@1: //  h1[hp1]=2*pow(2, -p1);
xue@1:   for (int i=1, j=hp1-1; i<=hp1; i++, j--) h1[j]=h1[j+1]*(p1+1-i)/i;
xue@1: 
xue@1:   double *_hh1=new double[p2+1+2*q];
xue@1:   double *_hh2=&_hh1[p2+1];
xue@1:   double *hh1=&_hh1[p2-hp2], *hh2=&_hh2[q];
xue@1:   hh1[hp2]=sqrt(2.0)*pow(2, -p2);
xue@1: //  hh1[hp2]=pow(2, -p2);
xue@1:   for (int i=1, j=hp2-1; i<=hp2; i++, j--) hh1[j]=hh1[j+1]*(p2+1-i)/i;
xue@1:   if (p2%2) for (int i=0; i<=hp2; i++) hh1[-i-1]=hh1[i];
xue@1:   else for (int i=1; i<=hp2; i++) hh1[-i]=hh1[i];
xue@1:   memset(_hh2, 0, sizeof(double)*2*q);
xue@1:   for (int n=1-q; n<=q-1; n++)
xue@1:   {
xue@1:     int k=abs(n);
xue@1:     int CC1=Cmb(q-1+k, k), CC2=Cmb(2*k, k-n);  //CC=1.0*C(q-1+k, k)*C(2*k, k-n)
xue@1:     for (int k=abs(n); k<=q-1; k++)
xue@1:     {
xue@1:       hh2[n]=hh2[n]+1.0*CC1*CC2*pow(0.25, k);
xue@1:       CC1=CC1*(q+k)/(k+1);
xue@1:       CC2=CC2*(2*k+1)*(2*k+2)/((k+1-n)*(k+1+n));
xue@1:     }
xue@1:     hh2[n]*=pow(-1, n);
xue@1:   }
xue@1:   //hh1[hp2-p2:hp2], hh2[1-q:q-1]
xue@1:   //h2=conv(hh1, hh2), but the positive half only
xue@1:   memset(h2, 0, sizeof(double)*(hp1+p2));
xue@1:   for (int i=hp2-p2; i<=hp2; i++) for (int j=1-q; j<=q-1; j++)
xue@1:     if (i+j>=0 && i+j<hp1+p2) h2[i+j]+=hh1[i]*hh2[j];
xue@1:   delete[] _hh1;
xue@1: 
xue@1:   int hs, he, hms, hme, gs, ge, gms, gme;
xue@1:   if (lf%2)
xue@1:   {
xue@1:     hs=-hlf, he=hlf, hms=-hlb, hme=hlb;
xue@1:     gs=-hlb+1, ge=hlb+1, gms=-hlf-1, gme=hlf-1;
xue@1:   }
xue@1:   else
xue@1:   {
xue@1:     hs=-hlf, he=hlf-1, hms=-hlb+1, hme=hlb;
xue@1:     gs=-hlb, ge=hlb-1, gms=-hlf+1, gme=hlf;
xue@1:   }
xue@1: 
xue@1:   if (lf%2)
xue@1:   {
xue@1:     for (int i=1; i<=hlf; i++) h1[-i]=h1[i];
xue@1:     for (int i=1; i<=hlb; i++) h2[-i]=h2[i];
xue@1:     double* _g=&g[hlb-1], *_gm=&gm[hlf+1];
xue@1:     for (int i=gs; i<=ge; i++) _g[i]=(i%2)?h2[1-i]:-h2[1-i];
xue@1:     for (int i=gms; i<=gme; i++) _gm[i]=(i%2)?h1[-1-i]:-h1[-1-i];
xue@1:   }
xue@1:   else
xue@1:   {
xue@1:     for (int i=0; i<hlf; i++) h1[-i-1]=h1[i];
xue@1:     for (int i=0; i<hlb; i++) h2[-i-1]=h2[i];
xue@1:     h2=&h2[-1];
xue@1:     double *_g=&g[hlb], *_gm=&gm[hlf-1];
xue@1:     for (int i=gs; i<=ge; i++) _g[i]=(i%2)?-h2[-i]:h2[-i];
xue@1:     for (int i=gms; i<=gme; i++) _gm[i]=(i%2)?-h1[-i]:h1[-i];
xue@1:   }
xue@1: 
xue@1:   if (normmode)
xue@1:   {
xue@1:     double sumh=0; for (int i=0; i<=he-hs; i++) sumh+=h[i]*h[i];
xue@1:     double sumhm=0; for (int i=0; i<=hme-hms; i++) sumhm+=hm[i]*hm[i];
xue@1:     if (normmode==1)
xue@1:     {
xue@1:       double rr=sqrt(sumh);
xue@1:       for (int i=0; i<=hme-hms; i++) hm[i]*=rr;
xue@1:       rr=1.0/rr;
xue@1:       for (int i=0; i<=he-hs; i++) h[i]*=rr;
xue@1:       rr=sqrt(sumhm);
xue@1:       for (int i=0; i<=gme-gms; i++) gm[i]*=rr;
xue@1:       rr=1.0/rr;
xue@1:       for (int i=0; i<=ge-gs; i++) g[i]*=rr;
xue@1:     }
xue@1:     else if (normmode==2)
xue@1:     {
xue@1:       double rr=sqrt(sumh);
xue@1:       for (int i=0; i<=ge-gs; i++) g[i]*=rr;
xue@1:       rr=1.0/rr;
xue@1:       for (int i=0; i<=gme-gms; i++) gm[i]*=rr;
xue@1:       rr=sqrt(sumhm);
xue@1:       for (int i=0; i<=he-hs; i++) h[i]*=rr;
xue@1:       rr=1.0/rr;
xue@1:       for (int i=0; i<=hme-hms; i++) hm[i]*=rr;
xue@1:     }
xue@1:     else if (normmode==3)
xue@1:     {
xue@1:       double rr=pow(sumh/sumhm, 0.25);
xue@1:       for (int i=0; i<=hme-hms; i++) hm[i]*=rr;
xue@1:       for (int i=0; i<=ge-gs; i++) g[i]*=rr;
xue@1:       rr=1.0/rr;
xue@1:       for (int i=0; i<=he-hs; i++) h[i]*=rr;
xue@1:       for (int i=0; i<=gme-gms; i++) gm[i]*=rr;
xue@1:     }
xue@1:   }
xue@1: 
xue@1:   if (points)
xue@1:   {
xue@1:     points[0]=hs, points[1]=he, points[2]=hms, points[3]=hme;
xue@1:     points[4]=gs, points[5]=ge, points[6]=gms, points[7]=gme;
xue@1:   }
xue@1:   return -p1-p2+1;
xue@1: }//splinewl
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function wavpacqmf: calculate pseudo local cosine transforms using wavelet packet
xue@1: 
xue@1:   In: data[Count], Count=fr*WID, waveform data
xue@1:       WID: largest scale, equals 2^ORDER
xue@1:       wid: smallest scale, euqals 2^order
xue@1:       h[M], g[M]: quadratic mirror filter pair, fr>2*M
xue@1:   Out: spec[0][fr][WID], spec[1][2*fr][WID/2], ..., spec[ORDER-order-1][FR][wid]
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void wavpacqmf(double*** spec, double* data, int Count, int WID, int wid, int M, double* h, double* g)
xue@1: {
xue@1:   int fr=Count/WID, ORDER=log2(WID), order=log2(wid);
xue@1:   double* _data1=new double[Count*2];
xue@1:   double *data1=_data1, *data2=&_data1[Count];
xue@1:   //the qmf always filters data1 and puts the results to data2
xue@1:   memcpy(data1, data, sizeof(double)*Count);
xue@1:   int l=0, C=fr*WID, FR=1;
xue@1:   double *ldata, *ldataa, *ldatad;
xue@1:   while (l<ORDER)
xue@1:   {
xue@1:     //qmf
xue@1:     for (int f=0; f<FR; f++)
xue@1:     {
xue@1:       ldata=&data1[f*C];
xue@1:       if (f%2==0)
xue@1:         ldataa=&data2[f*C], ldatad=&data2[f*C+C/2];
xue@1:       else
xue@1:         ldatad=&data2[f*C], ldataa=&data2[f*C+C/2];
xue@1: 
xue@1:       memset(&data2[f*C], 0, sizeof(double)*C);
xue@1:       for (int i=0; i<C; i+=2)
xue@1:       {
xue@1:         int i2=i/2;
xue@1:         ldataa[i2]=ldata[i]*h[0];
xue@1:         ldatad[i2]=ldata[i]*g[0];
xue@1:         for (int j=1; j<M; j++)
xue@1:         {
xue@1:           if (i+j<C)
xue@1:           {
xue@1:             ldataa[i2]+=ldata[i+j]*h[j];
xue@1:             ldatad[i2]+=ldata[i+j]*g[j];
xue@1:           }
xue@1:           else
xue@1:           {
xue@1:             ldataa[i2]+=ldata[i+j-C]*h[j];
xue@1:             ldatad[i2]+=ldata[i+j-C]*g[j];
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     double *tmp=data2; data2=data1; data1=tmp;
xue@1:     l++;
xue@1:     C=(C>>1);
xue@1:     FR=(FR<<1);
xue@1:     if (l>=order)
xue@1:     {
xue@1:       for (int f=0; f<FR; f++)
xue@1:         for(int i=0; i<C; i++)
xue@1:           spec[ORDER-l][i][f]=data1[f*C+i];
xue@1:     }
xue@1:   }
xue@1: 
xue@1:   delete[] _data1;
xue@1: }//wavpacqmf
xue@1: 
Chris@5: /**
xue@1:   function iwavpacqmf: inverse transform of wavpacqmf
xue@1: 
xue@1:   In: spec[Fr][Wid], Fr>M*2
xue@1:       h[M], g[M], quadratic mirror filter pair
xue@1:   Out: data[Fr*Wid]
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void iwavpacqmf(double* data, double** spec, int Fr, int Wid, int M, double* h, double* g)
xue@1: {
xue@1:   int Count=Fr*Wid, Order=log2(Wid);
xue@1:   double* _data1=new double[Count];
xue@1:   double *data1, *data2, *ldata, *ldataa, *ldatad;
xue@1:   if (Order%2) data1=_data1, data2=data;
xue@1:   else data1=data, data2=_data1;
xue@1:   //data pass to buffer
xue@1:   for (int i=0, t=0; i<Wid; i++)
xue@1:     for (int j=0; j<Fr; j++)
xue@1:       data1[t++]=spec[j][i];
xue@1: 
xue@1:   int l=Order;
xue@1:   int C=Fr;
xue@1:   int K=Wid/2;
xue@1:   while (l>0)
xue@1:   {
xue@1:     memset(data2, 0, sizeof(double)*Count);
xue@1:     for (int k=0; k<K; k++)
xue@1:     {
xue@1:       if (k%2==0) ldataa=&data1[2*k*C], ldatad=&data1[(2*k+1)*C];
xue@1:       else ldatad=&data1[2*k*C], ldataa=&data1[(2*k+1)*C];
xue@1:       ldata=&data2[2*k*C];
xue@1:       //qmf
xue@1:       for (int i=0; i<C; i++)
xue@1:       {
xue@1:         for (int j=0; j<M; j++)
xue@1:         {
xue@1:           if (i*2+j<C*2)
xue@1:           {
xue@1:             ldata[i*2+j]+=ldataa[i]*h[j]+ldatad[i]*g[j];
xue@1:           }
xue@1:           else
xue@1:           {
xue@1:             ldata[i*2+j-C*2]+=ldataa[i]*h[j]+ldatad[i]*g[j];
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:     }
xue@1: 
xue@1:     double *tmp=data2; data2=data1; data1=tmp;
xue@1:     l--;
xue@1:     C=(C<<1);
xue@1:     K=(K>>1);
xue@1:   }
xue@1:   delete[] _data1;
xue@1: }//iwavpacqmf
xue@1: 
Chris@5: /**
xue@1:   function wavpac: calculate pseudo local cosine transforms using wavelet packet,
xue@1: 
xue@1:   In: data[Count], Count=fr*WID, waveform data
xue@1:       WID: largest scale, equals 2^ORDER
xue@1:       wid: smallest scale, euqals 2^order
xue@1:       h[hs:he-1], g[gs:ge-1]: filter pair
xue@1:   Out: spec[0][fr][WID], spec[1][2*fr][WID/2], ..., spec[ORDER-order-1][FR][wid]
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void wavpac(double*** spec, double* data, int Count, int WID, int wid, double* h, int hs, int he, double* g, int gs, int ge)
xue@1: {
xue@1:   int fr=Count/WID, ORDER=log2(WID), order=log2(wid);
xue@1:   double* _data1=new double[Count*2];
xue@1:   double *data1=_data1, *data2=&_data1[Count];
xue@1:   //the qmf always filters data1 and puts the results to data2
xue@1:   memcpy(data1, data, sizeof(double)*Count);
xue@1:   int l=0, C=fr*WID, FR=1;
xue@1:   double *ldata, *ldataa, *ldatad;
xue@1:   while (l<ORDER)
xue@1:   {
xue@1:     //qmf
xue@1:     for (int f=0; f<FR; f++)
xue@1:     {
xue@1:       ldata=&data1[f*C];
xue@1:       if (f%2==0)
xue@1:         ldataa=&data2[f*C], ldatad=&data2[f*C+C/2];
xue@1:       else
xue@1:         ldatad=&data2[f*C], ldataa=&data2[f*C+C/2];
xue@1: 
xue@1:       memset(&data2[f*C], 0, sizeof(double)*C);
xue@1:       for (int i=0; i<C; i+=2)
xue@1:       {
xue@1:         int i2=i/2;
xue@1:         ldataa[i2]=0;//ldata[i]*h[0];
xue@1:         ldatad[i2]=0;//ldata[i]*g[0];
xue@1:         for (int j=hs; j<=he; j++)
xue@1:         {
xue@1:           int ind=i-j;
xue@1:           if (ind>=C)
xue@1:           {
xue@1:             ldataa[i2]+=ldata[ind-C]*h[j];
xue@1:           }
xue@1:           else if (ind<0)
xue@1:           {
xue@1:             ldataa[i2]+=ldata[ind+C]*h[j];
xue@1:           }
xue@1:           else
xue@1:           {
xue@1:             ldataa[i2]+=ldata[ind]*h[j];
xue@1:           }
xue@1:         }
xue@1:         for (int j=gs; j<=ge; j++)
xue@1:         {
xue@1:           int ind=i-j;
xue@1:           if (ind>=C)
xue@1:           {
xue@1:             ldatad[i2]+=ldata[ind-C]*g[j];
xue@1:           }
xue@1:           else if (ind<0)
xue@1:           {
xue@1:             ldatad[i2]+=ldata[ind+C]*g[j];
xue@1:           }
xue@1:           else
xue@1:           {
xue@1:             ldatad[i2]+=ldata[ind]*g[j];
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     double *tmp=data2; data2=data1; data1=tmp;
xue@1:     l++;
xue@1:     C=(C>>1);
xue@1:     FR=(FR<<1);
xue@1:     if (l>=order)
xue@1:     {
xue@1:       for (int f=0; f<FR; f++)
xue@1:         for(int i=0; i<C; i++)
xue@1:           spec[ORDER-l][i][f]=data1[f*C+i];
xue@1:     }
xue@1:   }
xue@1: 
xue@1:   delete[] _data1;
xue@1: }//wavpac
xue@1: 
Chris@5: /**
xue@1:   function iwavpac: inverse transform of wavpac
xue@1: 
xue@1:   In: spec[Fr][Wid]
xue@1:       h[hs:he-1], g[gs:ge-1], reconstruction filter pair
xue@1:   Out: data[Fr*Wid]
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void iwavpac(double* data, double** spec, int Fr, int Wid, double* h, int hs, int he, double* g, int gs, int ge)
xue@1: {
xue@1:   int Count=Fr*Wid, Order=log2(Wid);
xue@1:   double* _data1=new double[Count];
xue@1:   double *data1, *data2, *ldata, *ldataa, *ldatad;
xue@1:   if (Order%2) data1=_data1, data2=data;
xue@1:   else data1=data, data2=_data1;
xue@1:   //data pass to buffer
xue@1:   for (int i=0, t=0; i<Wid; i++)
xue@1:     for (int j=0; j<Fr; j++)
xue@1:       data1[t++]=spec[j][i];
xue@1: 
xue@1:   int l=Order;
xue@1:   int C=Fr;
xue@1:   int K=Wid/2;
xue@1:   while (l>0)
xue@1:   {
xue@1:     memset(data2, 0, sizeof(double)*Count);
xue@1:     for (int k=0; k<K; k++)
xue@1:     {
xue@1:       if (k%2==0) ldataa=&data1[2*k*C], ldatad=&data1[(2*k+1)*C];
xue@1:       else ldatad=&data1[2*k*C], ldataa=&data1[(2*k+1)*C];
xue@1:       ldata=&data2[2*k*C];
xue@1:       //qmf
xue@1:       for (int i=0; i<C; i++)
xue@1:       {
xue@1:         for (int j=hs; j<=he; j++)
xue@1:         {
xue@1:           int ind=i*2+j;
xue@1:           if (ind>=C*2)
xue@1:           {
xue@1:             ldata[ind-C*2]+=ldataa[i]*h[j];
xue@1:           }
xue@1:           else if (ind<0)
xue@1:           {
xue@1:             ldata[ind+C*2]+=ldataa[i]*h[j];
xue@1:           }
xue@1:           else
xue@1:           {
xue@1:             ldata[ind]+=ldataa[i]*h[j];
xue@1:           }
xue@1:         }
xue@1:         for (int j=gs; j<=ge; j++)
xue@1:         {
xue@1:           int ind=i*2+j;
xue@1:           if (ind>=C*2)
xue@1:           {
xue@1:             ldata[ind-C*2]+=ldatad[i]*g[j];
xue@1:           }
xue@1:           else if (ind<0)
xue@1:           {
xue@1:             ldata[ind+C*2]+=ldatad[i]*g[j];
xue@1:           }
xue@1:           else
xue@1:           {
xue@1:             ldata[ind]+=ldatad[i]*g[j];
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:     }
xue@1: 
xue@1:     double *tmp=data2; data2=data1; data1=tmp;
xue@1:     l--;
xue@1:     C=(C<<1);
xue@1:     K=(K>>1);
xue@1:   }
xue@1:   delete[] _data1;
xue@1: }//iwavpac