#include /* solve tridiagonal system, a lower, b diagonal, c upper, d right hand side */ void tridiagonal_solve(double * a, double * b, double * c, double *d, int n, double * r) { int i; c[0] = c[0] / b[0]; d[0] = d[0] / b[0]; for (i = 1; i < n; i++) { double divisor = (b[i] - a[i] * c[i-1]); c[i] = c[i] / divisor; d[i] = (d[i] - a[i] * d[i-1]) / divisor; } /* backward substitution */ r[n - 1] = d[n - 1]; for (i = n - 2; i >= 0; i--) { r[i] = d[i] - c[i] * r[i + 1]; } } /* natural spline parametesr wity y'' = 0 boundary condition * to be used by nat_spline_int */ void nat_spline(const float * x, const float *y, int n, double * p) { double * a = malloc(n * sizeof(*a)); double * b = malloc(n * sizeof(*b)); double * c = malloc(n * sizeof(*c)); double * d = malloc(n * sizeof(*d)); int i; double xf = x[1] - x[0]; double xb = x[n-1] - x[n-2]; /* build rhs with natural boundary condition (y'' = 0) */ d[0] = 3*((y[1] - y[0]) / (xf * xf)); for (i = 1; i < n - 1; i++) { double h = (x[i] - x[i - 1]); double h2 = (x[i+1] - x[i]); d[i] = 3 * ((y[i] - y[i-1])/(h*h) + (y[i+1]-y[i]) / (h2*h2)); } d[n - 1] = 3*(y[n-1] - y[n-2]) / (xb*xb); /* build tridiagonal matrix a lower, b diag, c upper*/ b[0] = 2 / xf; c[0] = 1 / xf; for (i = 1; i < n - 1; i++) { double h = (x[i] - x[i - 1]); double h2 = (x[i+1] - x[i]); a[i] = 1/ h; b[i] = (2/h + 2/h2); c[i] = 1/ h2; } a[n-1] = 1 / xb; b[n-1] = 2 / xb; tridiagonal_solve(a, b, c, d, n, p); free(a); free(b); free(c); free(d); return; } /* spline interpolation at point xv, p parameters from nat_spline * x must be ascending * last evaluated spline position storage, must be NULL if xv is smaller than * that of the last call */ double nat_spline_int(const float * x, const float * y, double * p, int n, float xv, int * last) { int jl = 0; int jh = 1; int j; int l = last ? *last : 0; for (j = l; j < n - 1; j++) { if (xv >= x[j]) { jl = j; jh = j + 1; } if (xv < x[j]) { break; } } if (last) { *last = jl; } { double t = (xv - x[jl]) / (x[jh] - x[jl]); double pa = p[jl] * (x[jh]-x[jl]) - (y[jh] - y[jl]); double pb = -p[jh] * (x[jh]-x[jl]) + (y[jh] - y[jl]); return (1-t)*y[jl] + t*y[jh] + t*(1-t)*(pa*(1-t)+pb*t); } } /* prepare 2d splines, fortran indexing (z and p2d ragged) */ void nat_spline2d(const float *y, float **z, int nx, int ny, double ** p2d) { int i; for (i = 0; i < nx; i++) { nat_spline(&y[1], &(z[i + 1][1]), ny, &(p2d[i + 1][1])); } } /* interpolate 2d data along ninterpolate points of xpos * fortran indexing (z and p2d ragged) * xpos and interpolated 0 indexing*/ void nat_spline2d_int(const float * x, const float *y, float **z, double **p2d, int nx, int ny, int ninterpolate, float * xpos, float ypos, float * interpolated) { float * yytmp = malloc(nx * sizeof(*yytmp)); double * p = malloc(ny * sizeof(*p)); int i; int last = 0; for (i = 0; i < nx; i++) { yytmp[i] = nat_spline_int(&y[1], &(z[i + 1][1]), &(p2d[i + 1][1]), ny, ypos, &last); } nat_spline(x, yytmp, nx, p); last = 0; /* interpolate points */ for (i = 0; i < ninterpolate; i++) { interpolated[i] = nat_spline_int(&x[1], yytmp, p, nx, xpos[i], &last); } free(p); free(yytmp); } /*************************************************************** * * hsplint(): spline interpolation based on Hermite polynomials. * ***************************************************************/ double hsplint( double xp, double *x, float *y, int n, int *istart ) /* double xp; x-value to interpolate */ /* double *x; x-array [1..n] */ /* float *y; y-array [1..n] */ /* int *istart; initial index */ { double yp1, yp2, yp; double xpi, xpi1, l1, l2, lp1, lp2; int i; if ( xp < x[1] || xp > x[n] ) return(0.0); for ( i = *istart; i <= n && xp >= x[i]; i++ ) ; *istart = i; i--; lp1 = 1.0 / (x[i] - x[i+1]); lp2 = -lp1; if ( i == 1 ) yp1 = (y[2] - y[1]) / (x[2] - x[1]); else yp1 = (y[i+1] - y[i-1]) / (x[i+1] - x[i-1]); if ( i >= n - 1 ) yp2 = (y[n] - y[n-1]) / (x[n] - x[n-1]); else yp2 = (y[i+2] - y[i]) / (x[i+2] - x[i]); xpi1 = xp - x[i+1]; xpi = xp - x[i]; l1 = xpi1*lp1; l2 = xpi*lp2; yp = y[i]*(1 - 2.0*lp1*xpi)*l1*l1 + y[i+1]*(1 - 2.0*lp2*xpi1)*l2*l2 + yp1*xpi*l1*l1 + yp2*xpi1*l2*l2; return(yp); }