void savgol(double *c, int np, int nl, int nr, int ld, int m) {
/*-------------------------------------------------------------------------------------------
USES lubksb,ludcmp given below.
Returns in c(np), in wrap-around order (see reference) consistent with the argument respns
in routine convlv, a set of Savitzky-Golay filter coefficients. nl is the number of leftward
(past) data points used, while nr is the number of rightward (future) data points, making
the total number of data points used nl+nr+1. ld is the order of the derivative desired
(e.g., ld = 0 for smoothed function). m is the order of the smoothing polynomial, also
equal to the highest conserved moment; usual values are m = 2 or m = 4.
-------------------------------------------------------------------------------------------*/
int imj,ipj,j,k,kk,mm;
double d,fac,sum,**a,*b;
if (np < nl+nr+1 || nl < 0 || nr < 0 || ld > m || nl+nr < m)
nrerror("bad args in savgol");
int *indx= intvector(1,m+1);
a=matrix(1,m+1,1,m+1);
b=vector(1,m+1);
for (ipj=0;ipj<=(m << 1);ipj++)
{//Set up the normal equations of the desired least-squares fit
sum=(ipj ? 0.0 : 1.0);
for (k=1;k<=nr;k++)
sum += pow((double)k,(double)ipj);
for (k=1;k<=nl;k++)
sum += pow((double)-k,(double)ipj);
mm=IMIN(ipj,2*m-ipj);
for (imj = -mm;imj<=mm;imj+=2)
a[1+(ipj+imj)/2][1+(ipj-imj)/2]=sum;
}
ludcmp(a, m+1, indx, &d); //Solve them: LU decomposition.
for (j=1;j<=m+1;j++)
b[j]=0.0;
b[ld+1]=1.0; //Right-hand side vector is unit vector, depending on which derivative we want.
lubksb(a,m+1,indx,b); //Get one row of the inverse matrix.
for (kk=1;kk<=np;kk++)
c[kk]=0.0; //Zero the output array (it may be bigger than number of coefficients).
for (k = -nl;k<=nr;k++)
{
sum=b[1]; //Each Savitzky-Golay coefficient is the dot product
//of powers of an integer with the inverse matrix row.
fac=1.0;
for (mm=1;mm<=m;mm++)
sum += b[mm+1]*(fac *= k);
kk=((np-k) % np)+1; //Store in wrap-around order.
c[kk]=sum;
}
free_vector(b,1,m+1);
free_matrix(a,1,m+1,1,m+1);
free_intvector(indx,1,m+1);
}
void SmoothFilter::smoothSavGol(double *, double *y)
{
double *s = new double[d_n];
int nl = d_smooth_points;
int nr = d_sav_gol_points;
int np = nl+nr+1;
double *c = vector(1, np);
//seek shift index for given case nl, nr, m (see savgol).
int *index = intvector(1, np);
index[1]=0;
int i, j=3;
for (i=2; i<=nl+1; i++)
{// index(2)=-1; index(3)=-2; index(4)=-3; index(5)=-4; index(6)=-5
index[i]=i-j;
j += 2;
}
j=2;
for (i=nl+2; i<=np; i++)
{// index(7)= 5; index(8)= 4; index(9)= 3; index(10)=2; index(11)=1
index[i]=i-j;
j += 2;
}
//calculate Savitzky-Golay filter coefficients.
savgol(c, np, nl, nr, 0, d_polynom_order);
for (i=0; i<d_n; i++){// Apply filter to input data.
s[i]=0.0;
for (j=1; j<=np; j++){
int it = i+index[j];
if (it >=0 && it < d_n)//skip left points that do not exist.
s[i] += c[j]*y[i+index[j]];
}
}
for (i = 0; i<d_n; i++)
y[i] = s[i];
delete[] s;
free_vector(c, 1, np);
free_intvector(index, 1, np);
}
