Debian Codegraph @

libsrc/math/spline.c

raw
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 }