| 1 |
#include <stdlib.h> |
| 2 |
|
| 3 |
/* solve tridiagonal system, a lower, b diagonal, c upper, d right hand side */ |
| 4 |
void tridiagonal_solve(double * a, double * b, double * c, double *d, int n, double * r) |
| 5 |
{ |
| 6 |
int i; |
| 7 |
c[0] = c[0] / b[0]; |
| 8 |
d[0] = d[0] / b[0]; |
| 9 |
for (i = 1; i < n; i++) { |
| 10 |
double divisor = (b[i] - a[i] * c[i-1]); |
| 11 |
c[i] = c[i] / divisor; |
| 12 |
d[i] = (d[i] - a[i] * d[i-1]) / divisor; |
| 13 |
} |
| 14 |
|
| 15 |
/* backward substitution */ |
| 16 |
r[n - 1] = d[n - 1]; |
| 17 |
for (i = n - 2; i >= 0; i--) { |
| 18 |
r[i] = d[i] - c[i] * r[i + 1]; |
| 19 |
} |
| 20 |
} |
| 21 |
|
| 22 |
|
| 23 |
/* natural spline parametesr wity y'' = 0 boundary condition |
| 24 |
* to be used by nat_spline_int */ |
| 25 |
void nat_spline(const float * x, const float *y, int n, double * p) |
| 26 |
{ |
| 27 |
double * a = malloc(n * sizeof(*a)); |
| 28 |
double * b = malloc(n * sizeof(*b)); |
| 29 |
double * c = malloc(n * sizeof(*c)); |
| 30 |
double * d = malloc(n * sizeof(*d)); |
| 31 |
int i; |
| 32 |
|
| 33 |
double xf = x[1] - x[0]; |
| 34 |
double xb = x[n-1] - x[n-2]; |
| 35 |
/* build rhs with natural boundary condition (y'' = 0) */ |
| 36 |
d[0] = 3*((y[1] - y[0]) / (xf * xf)); |
| 37 |
for (i = 1; i < n - 1; i++) { |
| 38 |
double h = (x[i] - x[i - 1]); |
| 39 |
double h2 = (x[i+1] - x[i]); |
| 40 |
d[i] = 3 * ((y[i] - y[i-1])/(h*h) + (y[i+1]-y[i]) / (h2*h2)); |
| 41 |
} |
| 42 |
d[n - 1] = 3*(y[n-1] - y[n-2]) / (xb*xb); |
| 43 |
|
| 44 |
/* build tridiagonal matrix a lower, b diag, c upper*/ |
| 45 |
b[0] = 2 / xf; |
| 46 |
c[0] = 1 / xf; |
| 47 |
for (i = 1; i < n - 1; i++) { |
| 48 |
double h = (x[i] - x[i - 1]); |
| 49 |
double h2 = (x[i+1] - x[i]); |
| 50 |
a[i] = 1/ h; |
| 51 |
b[i] = (2/h + 2/h2); |
| 52 |
c[i] = 1/ h2; |
| 53 |
} |
| 54 |
a[n-1] = 1 / xb; |
| 55 |
b[n-1] = 2 / xb; |
| 56 |
|
| 57 |
tridiagonal_solve(a, b, c, d, n, p); |
| 58 |
|
| 59 |
free(a); |
| 60 |
free(b); |
| 61 |
free(c); |
| 62 |
free(d); |
| 63 |
|
| 64 |
return; |
| 65 |
} |
| 66 |
|
| 67 |
/* spline interpolation at point xv, p parameters from nat_spline |
| 68 |
* x must be ascending |
| 69 |
* last evaluated spline position storage, must be NULL if xv is smaller than |
| 70 |
* that of the last call */ |
| 71 |
double nat_spline_int(const float * x, const float * y, double * p, int n, float xv, |
| 72 |
int * last) |
| 73 |
{ |
| 74 |
int jl = 0; |
| 75 |
int jh = 1; |
| 76 |
int j; |
| 77 |
int l = last ? *last : 0; |
| 78 |
for (j = l; j < n - 1; j++) { |
| 79 |
if (xv >= x[j]) { |
| 80 |
jl = j; |
| 81 |
jh = j + 1; |
| 82 |
} |
| 83 |
if (xv < x[j]) { |
| 84 |
break; |
| 85 |
} |
| 86 |
} |
| 87 |
if (last) { |
| 88 |
*last = jl; |
| 89 |
} |
| 90 |
{ |
| 91 |
double t = (xv - x[jl]) / (x[jh] - x[jl]); |
| 92 |
double pa = p[jl] * (x[jh]-x[jl]) - (y[jh] - y[jl]); |
| 93 |
double pb = -p[jh] * (x[jh]-x[jl]) + (y[jh] - y[jl]); |
| 94 |
return (1-t)*y[jl] + t*y[jh] + t*(1-t)*(pa*(1-t)+pb*t); |
| 95 |
} |
| 96 |
} |
| 97 |
|
| 98 |
/* prepare 2d splines, fortran indexing (z and p2d ragged) */ |
| 99 |
void nat_spline2d(const float *y, float **z, int nx, int ny, double ** p2d) |
| 100 |
{ |
| 101 |
int i; |
| 102 |
for (i = 0; i < nx; i++) { |
| 103 |
nat_spline(&y[1], &(z[i + 1][1]), ny, &(p2d[i + 1][1])); |
| 104 |
} |
| 105 |
} |
| 106 |
|
| 107 |
/* interpolate 2d data along ninterpolate points of xpos |
| 108 |
* fortran indexing (z and p2d ragged) |
| 109 |
* xpos and interpolated 0 indexing*/ |
| 110 |
void nat_spline2d_int(const float * x, const float *y, float **z, |
| 111 |
double **p2d, |
| 112 |
int nx, int ny, |
| 113 |
int ninterpolate, float * xpos, float ypos, float * interpolated) |
| 114 |
{ |
| 115 |
float * yytmp = malloc(nx * sizeof(*yytmp)); |
| 116 |
double * p = malloc(ny * sizeof(*p)); |
| 117 |
int i; |
| 118 |
int last = 0; |
| 119 |
|
| 120 |
for (i = 0; i < nx; i++) { |
| 121 |
yytmp[i] = nat_spline_int(&y[1], &(z[i + 1][1]), &(p2d[i + 1][1]), ny, ypos, &last); |
| 122 |
} |
| 123 |
nat_spline(x, yytmp, nx, p); |
| 124 |
|
| 125 |
last = 0; |
| 126 |
/* interpolate points */ |
| 127 |
for (i = 0; i < ninterpolate; i++) { |
| 128 |
interpolated[i] = nat_spline_int(&x[1], yytmp, p, nx, xpos[i], &last); |
| 129 |
} |
| 130 |
free(p); |
| 131 |
free(yytmp); |
| 132 |
} |
| 133 |
|
| 134 |
/*************************************************************** |
| 135 |
* |
| 136 |
* hsplint(): spline interpolation based on Hermite polynomials. |
| 137 |
* |
| 138 |
***************************************************************/ |
| 139 |
double hsplint( double xp, double *x, float *y, int n, int *istart ) |
| 140 |
/* double xp; x-value to interpolate */ |
| 141 |
/* double *x; x-array [1..n] */ |
| 142 |
/* float *y; y-array [1..n] */ |
| 143 |
/* int *istart; initial index */ |
| 144 |
{ |
| 145 |
double yp1, yp2, yp; |
| 146 |
double xpi, xpi1, l1, l2, lp1, lp2; |
| 147 |
int i; |
| 148 |
|
| 149 |
if ( xp < x[1] || xp > x[n] ) |
| 150 |
return(0.0); |
| 151 |
|
| 152 |
for ( i = *istart; i <= n && xp >= x[i]; i++ ) |
| 153 |
; |
| 154 |
|
| 155 |
*istart = i; |
| 156 |
i--; |
| 157 |
|
| 158 |
lp1 = 1.0 / (x[i] - x[i+1]); |
| 159 |
lp2 = -lp1; |
| 160 |
|
| 161 |
if ( i == 1 ) |
| 162 |
yp1 = (y[2] - y[1]) / (x[2] - x[1]); |
| 163 |
else |
| 164 |
yp1 = (y[i+1] - y[i-1]) / (x[i+1] - x[i-1]); |
| 165 |
|
| 166 |
if ( i >= n - 1 ) |
| 167 |
yp2 = (y[n] - y[n-1]) / (x[n] - x[n-1]); |
| 168 |
else |
| 169 |
yp2 = (y[i+2] - y[i]) / (x[i+2] - x[i]); |
| 170 |
|
| 171 |
xpi1 = xp - x[i+1]; |
| 172 |
xpi = xp - x[i]; |
| 173 |
l1 = xpi1*lp1; |
| 174 |
l2 = xpi*lp2; |
| 175 |
|
| 176 |
yp = y[i]*(1 - 2.0*lp1*xpi)*l1*l1 + |
| 177 |
y[i+1]*(1 - 2.0*lp2*xpi1)*l2*l2 + |
| 178 |
yp1*xpi*l1*l1 + yp2*xpi1*l2*l2; |
| 179 |
|
| 180 |
return(yp); |
| 181 |
} |