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