1 /***************************************************************************
2
3 file : spline.cpp
4 created : Mon Apr 17 13:51:00 CET 2000
5 copyright : (C) 2000-2002 by Bernhard Wymann
6 email : berniw@bluewin.ch
7 version : $Id: spline.cpp,v 1.1 2004/01/20 13:45:23 berniw Exp $
8
9 ***************************************************************************/
10
11 /***************************************************************************
12 * *
13 * This program is free software; you can redistribute it and/or modify *
14 * it under the terms of the GNU General Public License as published by *
15 * the Free Software Foundation; either version 2 of the License, or *
16 * (at your option) any later version. *
17 * *
18 ***************************************************************************/
19
20 #include <math.h>
21 #include <stdlib.h>
22 #include "spline.h"
23
24 #ifdef DMALLOC
25 #include "dmalloc.h"
26 #endif
27
28
29 /* solving tridiagonal nxn matrix with Givens-Rotations in linear time O(n)
30 [ a1 b1 0 0 0 .......... ]
31 [ c1 a2 b2 0 0 ...........]
32 [ 0 c2 a3 b3 0 ...........]
33 [ ...........................]
34 [ ................... b(n-1) ]
35 [ ................ c(n-1) an ]
36 */
37
tridiagonal(int dim,SplineEquationData * tmp,double * x)38 void tridiagonal(int dim, SplineEquationData *tmp, double *x)
39 {
40 double cos, sin, h, t;
41 int i;
42
43 dim--;
44 tmp[dim].b = 0.0;
45 for (i = 0; i < dim; i++) {
46 if (tmp[i].c != 0.0) {
47 t = tmp[i].a / tmp[i].c;
48 sin = 1.0 / sqrt(1.0 + t*t);
49 cos = t * sin;
50 tmp[i].a = tmp[i].a*cos + tmp[i].c*sin;
51 h = tmp[i].b;
52 tmp[i].b = h*cos + tmp[i+1].a*sin;
53 tmp[i+1].a = -h*sin + tmp[i+1].a*cos;
54 tmp[i].c = tmp[i+1].b*sin;
55 tmp[i+1].b = tmp[i+1].b*cos;
56
57 h = x[i];
58 x[i] = h*cos + x[i+1]*sin;
59 x[i+1] = -h*sin + x[i+1]*cos;
60 }
61 }
62
63 x[dim] = x[dim]/tmp[dim].a;
64 x[dim-1] = (x[dim-1] - tmp[dim-1].b*x[dim]) / tmp[dim-1].a;
65
66 for (i = dim - 2; i >= 0; i--) {
67 x[i] = (x[i] - tmp[i].b*x[i+1] - tmp[i].c*x[i+2]) / tmp[i].a;
68 }
69 }
70
71
72 /* solving tridiagonal nxn matrix for two vectors with Givens-Rotations in linear time O(n) */
tridiagonal2(int dim,SplineEquationData2 * tmp)73 void tridiagonal2(int dim, SplineEquationData2 *tmp)
74 {
75 double cos, sin, h, t;
76 int i;
77
78 dim--;
79 tmp[dim].b = 0.0;
80 for (i = 0; i < dim; i++) {
81 if (tmp[i].c != 0.0) {
82 t = tmp[i].a / tmp[i].c;
83 sin = 1.0 / sqrt(1.0 + t*t);
84 cos = t * sin;
85 tmp[i].a = tmp[i].a*cos + tmp[i].c*sin;
86 h = tmp[i].b;
87 tmp[i].b = h*cos + tmp[i+1].a*sin;
88 tmp[i+1].a = -h*sin + tmp[i+1].a*cos;
89 tmp[i].c = tmp[i+1].b*sin;
90 tmp[i+1].b = tmp[i+1].b*cos;
91
92 h = tmp[i].x1;
93 tmp[i].x1 = h*cos + tmp[i+1].x1*sin;
94 tmp[i+1].x1 = -h*sin + tmp[i+1].x1*cos;
95
96 h = tmp[i].x2;
97 tmp[i].x2 = h*cos + tmp[i+1].x2*sin;
98 tmp[i+1].x2 = -h*sin + tmp[i+1].x2*cos;
99 }
100 }
101
102 tmp[dim].x1 = tmp[dim].x1 / tmp[dim].a;
103 tmp[dim-1].x1 = (tmp[dim-1].x1 - tmp[dim-1].b*tmp[dim].x1) / tmp[dim-1].a;
104
105 tmp[dim].x2 = tmp[dim].x2 / tmp[dim].a;
106 tmp[dim-1].x2 = (tmp[dim-1].x2 - tmp[dim-1].b*tmp[dim].x2) / tmp[dim-1].a;
107
108 for (i = dim - 2; i >= 0; i--) {
109 tmp[i].x1 = (tmp[i].x1 - tmp[i].b*tmp[i+1].x1 - tmp[i].c*tmp[i+2].x1) / tmp[i].a;
110 tmp[i].x2 = (tmp[i].x2 - tmp[i].b*tmp[i+1].x2 - tmp[i].c*tmp[i+2].x2) / tmp[i].a;
111 }
112 }
113
114
115 /* compute the slopes of the spline points with periodic constraints */
slopesp(int dim,const double * const x,const double * const y,double * const ys)116 void slopesp(int dim, const double *const x, const double *const y, double *const ys)
117 {
118 SplineEquationData2 *tmp = (SplineEquationData2 *) malloc(sizeof(SplineEquationData2)*dim);
119 int i;
120
121 dim--;
122 for (i = 0; i < dim; i++) {
123 tmp[i].h = x[i+1] - x[i];
124 tmp[i].d = (y[i+1]-y[i]) / (tmp[i].h*tmp[i].h);
125 }
126
127 for (i = 1; i < dim; i++) {
128 tmp[i].a = 2.0/tmp[i-1].h + 2.0/tmp[i].h;
129 tmp[i].b = 1.0/tmp[i].h;
130 tmp[i].c = tmp[i].b;
131 ys[i] = 3.0 * (tmp[i].d+tmp[i-1].d);
132 }
133
134 tmp[0].b = 1.0/tmp[0].h;
135 tmp[0].c = tmp[0].b;
136 tmp[0].a = (2.0*tmp[0].b + 1.0/tmp[dim-1].h);
137 tmp[dim-1].a = 2.0/tmp[dim-2].h + 1.0/tmp[dim-1].h;
138
139 for (i = 1; i < dim; i++) {
140 tmp[i].x1 = 0.0; tmp[i].x2 = 3.0 * (tmp[i].d+tmp[i-1].d);
141 }
142
143 tmp[0].x1 = 1.0;
144 tmp[dim-1].x1 = 1.0;
145 tmp[0].x2 = 3.0 * (tmp[0].d+tmp[dim-1].d);
146
147 tridiagonal2(dim, tmp);
148
149 double factor = (tmp[0].x2+tmp[dim-1].x2) / (tmp[0].x1+tmp[dim-1].x1+tmp[dim-1].h);
150 for (i = 0; i < dim; i++) {
151 ys[i] = tmp[i].x2 - factor*tmp[i].x1;
152 }
153 ys[dim] = ys[0];
154
155 free(tmp);
156 }
157
158
159 /* compute the slopes of the spline points with natural constraints */
slopesn(int dim,const double * const x,const double * const y,double * const ys)160 void slopesn(int dim, const double *const x, const double *const y, double *const ys)
161 {
162 SplineEquationData *tmp = (SplineEquationData *) malloc(sizeof(SplineEquationData)*dim);
163 int i;
164
165 dim--;
166 for (i = 0; i < dim; i++) {
167 tmp[i].h = x[i+1]-x[i];
168 tmp[i].d = (y[i+1]-y[i]) / (tmp[i].h*tmp[i].h);
169 }
170
171 for (i = 1; i < dim; i++) {
172 tmp[i].a = 2.0/tmp[i-1].h + 2.0/tmp[i].h;
173 tmp[i].b = 1.0/tmp[i].h;
174 tmp[i].c = tmp[i].b;
175 ys[i] = 3.0 * (tmp[i].d+tmp[i-1].d);
176 }
177
178 tmp[0].b = 1.0/tmp[0].h;
179 tmp[0].c = tmp[0].b;
180 tmp[0].a = 2.0*tmp[0].b;
181 tmp[dim].a = 2.0/tmp[dim-1].h;
182 ys[0] = 3.0*tmp[0].d;
183 ys[dim] = 3.0*tmp[dim-1].d;
184
185 tridiagonal(dim+1, tmp, ys);
186
187 free(tmp);
188 }
189
190
191 /* compute the slopes for 2-dim curve, sums euclidian distances as parameter, periodic */
parametricslopesp(int dim,const double * const x,const double * const y,double * const xs,double * const ys,double * const s)192 void parametricslopesp(
193 int dim,
194 const double *const x,
195 const double *const y,
196 double *const xs,
197 double *const ys,
198 double *const s
199 )
200 {
201 s[0] = 0.0;
202 for (int i = 1; i < dim; i++) {
203 s[i] = s[i-1] + sqrt((x[i]-x[i-1])*(x[i]-x[i-1]) + (y[i]-y[i-1])*(y[i]-y[i-1]));
204 }
205 slopesp(dim, s, x, xs);
206 slopesp(dim, s, y, ys);
207 }
208
209
210 /* compute the slopes for 2-dim curve, sums euclidian distances as parameter, natural */
parametricslopesn(int dim,const double * const x,const double * const y,double * const xs,double * const ys,double * const s)211 void parametricslopesn(
212 int dim,
213 const double *const x,
214 const double *const y,
215 double *const xs,
216 double *const ys,
217 double *const s
218 )
219 {
220 s[0] = 0.0;
221 for (int i = 1; i < dim; i++) {
222 s[i] = s[i-1] + sqrt((x[i]-x[i-1])*(x[i]-x[i-1]) + (y[i]-y[i-1])*(y[i]-y[i-1]));
223 }
224 slopesn(dim, s, x, xs);
225 slopesn(dim, s, y, ys);
226 }
227
228
229 /* compute the y value for a given z */
spline(int dim,double z,const double * const x,const double * const y,const double * const ys)230 double spline(
231 int dim,
232 double z,
233 const double *const x,
234 const double *const y,
235 const double *const ys
236 )
237 {
238 int i, a, b;
239 double t, a0, a1, a2, a3, h;
240
241 a = 0; b = dim-1;
242 do {
243 i = (a + b) / 2;
244 if (x[i] <= z) a = i; else b = i;
245 } while ((a + 1) != b);
246 i = a;
247 h = x[i+1] - x[i];
248 t = (z-x[i]) / h;
249 a0 = y[i];
250 a1 = y[i+1] - a0;
251 a2 = a1 - h*ys[i];
252 a3 = h * ys[i+1] - a1;
253 a3 -= a2;
254 return a0 + (a1 + (a2 + a3*t) * (t-1))*t;
255 }
256
257