1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
|
#include <stdlib.h>
/* 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);
}
|
