~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

Linux Cross Reference
Tina4/src/math/splines/spline.c

Version: ~
Architecture: ~ [ i386 ] ~ [ alpha ] ~ [ m68k ] ~ [ mips ] ~ [ ppc ] ~ [ sparc ] ~ [ sparc64 ] ~

  1 /**@(#)
  2 *
  3 * spline.c
  4 *
  5 * Uniform cubic B-spline functions.
  6 *
  7 */
  8 #include <stdio.h>
  9 #include <math.h>
 10 #include <tina/sys.h>
 11 #include <tina/math.h>
 12 #include <tina/mathfuncs.h>
 13 
 14 
 15 /*
 16 Allocate 1D uniform cubic spline with n independent control points.
 17 Type specifies end conditions:
 18 SPLINE_PERIODIC: periodic spline, period n,
 19 SPLINE_NATURAL : zero second derivative end conditions,
 20 SPLINE_TANGENT : given tangent end conditions.
 21 */
 22 Spline *spline_make(int type, int n)
 23 {
 24     Spline *spline = talloc(Spline);
 25     spline->type = type;
 26     spline->n = n;
 27     spline->p = tvector_alloc(-1, n+2, double);
 28     return (spline);
 29 }
 30 
 31 /*
 32 Allocate and return copy of a 1D-spline.
 33 */
 34 Spline *spline_copy(Spline * spline)
 35 {
 36     int n;
 37     Spline *newspline;
 38     if (spline == NULL)
 39         return(NULL);
 40     newspline = talloc(Spline);
 41     newspline->type = spline->type;
 42     n = newspline->n = spline->n;
 43     newspline->p = tvector_copy(spline->p, -1, n+2, double);
 44     return (newspline);
 45 }
 46 
 47 /*
 48 Free 1D-spline and asssociated memory.
 49 */
 50 void spline_free(Spline * spline)
 51 {
 52     if (spline == NULL)
 53         return;
 54     tvector_free(spline->p, -1, double);
 55     rfree(spline);
 56 }
 57 
 58 /*
 59 1D B-spline segment evaluation (0<u<1).
 60 */
 61 static double bspline(double c0, double c1, double c2, double c3, double u)
 62 {
 63     double u2 = u*u, u3 = u*u*u;
 64     double b0 = u3/6;
 65     double b1 = (1+3*u+3*u2-3*u3)/6;
 66     double b2 = (4-6*u2+3*u3)/6;
 67     double b3 = (1-3*u+3*u2-u3)/6;
 68     return(c0*b3+c1*b2+c2*b1+c3*b0);
 69 }
 70 
 71 /*
 72 Evaluate 1D-spline at parameter t.
 73 */
 74 double spline_eval(Spline * spline, double t)
 75 {
 76     int i, n;
 77     double u, *p;
 78     if (spline == NULL)
 79         return(0.0);
 80     n = spline->n;
 81     p = spline->p;
 82     switch (spline->type)
 83     {
 84         case SPLINE_NATURAL:
 85         case SPLINE_TANGENT:
 86             if (t <= 1.0)
 87                 i = 0;
 88             else if (t >= n - 1.0)
 89                 i = n - 2;
 90             else
 91                 i = floor(t);
 92             u = t-i;
 93             break;
 94         case SPLINE_PERIODIC:
 95             if(t < 0.0)
 96                 t += ceil(-t/n)*n;
 97             if(t >= n)
 98                 t -= floor(t/n)*n;
 99             i = floor(t);
100             break;
101         default:
102             return 0.0;
103     }
104     u = t-i;
105     return(bspline(p[i-1], p[i], p[i+1], p[i+2], u));
106 }
107 
108 static double p00;
109 static Spline *spline00;
110 static double dist(double t)
111 {
112     return fabs(spline_eval(spline00, t) - p00);
113 }
114 
115 static int tol(double a, double b)
116 {
117     return fabs(a - b) > 0.001;
118 }
119 
120 /*
121 Parameter of point on open 1D-spline closest to point p.
122 */
123 static double spline_open_param(Spline * spline, double p)
124 {
125     int n = spline->n;
126     double t = 0.0, t1, t2, tmin, d, dmin = MAXFLOAT;
127 
128     for (t = 0.0; t < n - 1.0; t++)
129     {
130         d = fabs(spline_eval(spline, t) - p);
131         if (dmin > d)
132         {
133             dmin = d;
134             tmin = t;
135         }
136     }
137     if (tmin == 0.0)
138     {
139         t1 = 0.0;
140         t2 = 1.0;
141     }
142     else if (tmin == n - 1.0)
143     {
144         t1 = n - 2.0;
145         t2 = n - 1.0;
146     }
147     else
148     {
149         t1 = tmin - 1.0;
150         t2 = tmin + 1.0;
151     }
152     spline00 = spline;
153     p00 = p;
154     t = golden_section(dist, t1, t2, tol, NULL, NULL);
155     return t;
156 }
157 
158 /*
159 Parameter of point on periodic 1D-spline closest to point p.
160 */
161 double spline_periodic_param(Spline * spline, double p)
162 {
163     int n = spline->n;
164     double t = 0.0, t1, t2, tmin, d, dmin = MAXFLOAT;
165 
166     for (t = 0.0; t < n; t++)
167     {
168         d = fabs(spline_eval(spline, t) - p);
169         if (dmin > d)
170         {
171             dmin = d;
172             tmin = t;
173         }
174     }
175 
176     t1 = tmin - 1.0;
177     t2 = tmin + 1.0;
178     spline00 = spline;
179     p00 = p;
180     t = golden_section(dist, t1, t2, tol, NULL, NULL);
181 
182     if (t < 0.0)
183     {
184         t += n;
185     }
186     else if (t >= n)
187     {
188         t -= n;
189     }
190     return t;
191 }
192 
193 /*
194 Return parameter of point on 1D-spline closest to point p.
195 */
196 double spline_param(Spline * spline, double p)
197 {
198     switch (spline->type)
199     {
200             case SPLINE_NATURAL:
201             case SPLINE_TANGENT:
202             return (spline_open_param(spline, p));
203         case SPLINE_PERIODIC:
204             return (spline_periodic_param(spline, p));
205     }
206     return(0.0);
207 }
208 
209 /*
210 Interplate an (already allocated) 1D-spline through data values
211 q[0], ... , q[n-1].
212 */
213 void spline_interpolate(Spline * spline, double *q)
214 {
215     int i, n = spline->n;
216     double *a = tvector_alloc(0, n, double);
217     double *b = tvector_alloc(0, n, double);
218     double *c = tvector_alloc(0, n, double);
219     double *p = spline->p;
220     for (i = 0; i < n; i++)
221     {
222         p[i] = q[i];
223         a[i] = 1.0/6.0;
224         b[i] = 2.0/3.0;
225         c[i] = 1.0/6.0;
226     }
227     switch (spline->type)
228     {
229         case SPLINE_NATURAL:
230             a[0] = c[0] = 0;
231             b[0] = 1.0;
232             a[n-1] = c[n-1] = 0;
233             b[n-1] = 1.0;
234             hrs_solve(n, a, b, c, p);
235             p[-1] = 2*p[0]-p[1];
236             p[n] = 2*p[n-1]-p[n-2];
237             break;
238         case SPLINE_TANGENT:
239             a[0] = 0;  c[0] = 1.0/3.0;
240             p[0] += q[-1]/3.0;
241             a[n-1] = 1.0/3.0;  c[n-1] = 0;
242             p[n-1] -= q[n]/3.0;
243             hrs_solve(n, a, b, c, p);
244             p[-1] = p[1]-2*q[-1];
245             p[n] = p[n-2]+2*q[n];
246             break;
247         case SPLINE_PERIODIC:
248             hrs_solve(n, a, b, c, p);
249             p[-1] = p[n-1];
250             p[n] = p[0];
251             p[n+1] = p[1];
252             break;
253     }
254     tvector_free(a, 0, double);
255     tvector_free(b, 0, double);
256 }
257 
258 /*
259 Makes 1D-spline interpolate a different value p at knot ip.
260 */
261 void spline_replace_point(Spline * spline, int ip, double p)
262 {
263     int i, n = spline->n;
264     double *pnew = tvector_alloc(0, n, double);
265     for (i = 0; i < n; i++)
266         pnew[i] = spline_eval(spline, i);
267     pnew[ip] = p;
268     spline_interpolate(spline, pnew);
269     tvector_free(pnew, 0, double);
270 }
271 
272 /*
273 Makes 1D-spline interpolate a new value p after knot ibelow.
274 */
275 void spline_add_point(Spline * spline, int ibelow, double p)
276 {
277     int i, n = spline->n;
278     double *pnew = tvector_alloc(0, n + 1, double);
279 
280     for (i = 0; i <= ibelow; i++)
281         pnew[i] = spline_eval(spline, i);
282     pnew[ibelow + 1] = p;
283     for (i = ibelow + 2; i < n + 1; i++)
284         pnew[i] = spline_eval(spline, i-1);
285 
286     tvector_free(spline->p, -1, double);
287     spline->n++;
288     spline->p = tvector_alloc(-1, n+3, double);
289     spline_interpolate(spline, pnew);
290     tvector_free(pnew, 0, double);
291 }
292 
293 /*
294 Deletes interpolation point at knot ip from 1D-spline.
295 */
296 void spline_delete_point(Spline * spline, int ip)
297 {
298     int i, n = spline->n;
299     double *pnew;
300 
301     if (spline->n < 3)
302         return;
303     pnew = tvector_alloc(0, n-1, double);
304     for (i = 0; i < ip; i++)
305         pnew[i] = spline_eval(spline, i);
306     for (i = ip; i < n - 1; i++)
307         pnew[i] = spline_eval(spline, i+1);
308     tvector_free(spline->p, -1, double);
309     spline->n--;
310     spline->p = tvector_alloc(-1, n+1, double);
311     spline_interpolate(spline, pnew);
312     tvector_free(pnew, 0, double);
313 }
314 
315 
316 /*
317 Given an array of n-1D-splines in stack, interpolate a 1D-spline
318 between them at parameter 0<=t<=n-1;
319 */
320 Spline *spline_stack_interp(int n, Spline ** stack, double t)
321 {
322     Spline *horiz, *vert;
323     double *p, *q;
324     int i, j, m = stack[0]->n;
325     int type = stack[0]->type;
326 
327     p = tvector_alloc(0, n, double);
328     q = tvector_alloc(0, m, double);
329     vert = spline_make(SPLINE_NATURAL, n);
330     for (j = 0; j < m; j++)
331     {
332         for (i = 0; i < n; i++)
333             p[i] = stack[i]->p[j];
334         spline_interpolate(vert, p);
335         q[j] = spline_eval(vert, t);
336     }
337     horiz = spline_make(type, m);
338     spline_interpolate(horiz, q);
339     tvector_free(p, 0, double);
340     tvector_free(q, 0, double);
341     spline_free(vert);
342     return(horiz);
343 }
344 

~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

This page was automatically generated by the LXR engine.
Visit the LXR main site for more information.