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

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

~ [ 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.