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

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

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

  1 /*
  2  *
  3  * spline2.c
  4  *
  5  * 2D uniform cubic B-spline functions.
  6  *
  7  */
  8 
  9 #include <values.h>
 10 #include <math.h>
 11 #include <tina/sys.h>
 12 #include <tina/math.h>
 13 #include <tina/mathfuncs.h>
 14 /*
 15 #include <tina/vision.h>
 16 #include <tina/visionfuncs.h>
 17 */
 18 #include <tina/splinefuncs.h>
 19 
 20 /*
 21 Allocate 2D uniform cubic spline with n independent control points.
 22 Type specifies end conditions:
 23 SPLINE_PERIODIC: periodic spline, period n,
 24 SPLINE_NATURAL : zero second derivative end conditions,
 25 SPLINE_TANGENT : given tangent end conditions.
 26 */
 27  Spline2 *spline2_make(int type, int n)
 28 {
 29     Spline2 *spline = talloc(Spline2);
 30     spline->type = type;
 31     spline->n = n;
 32     spline->x = spline_make(type, n);
 33     spline->y = spline_make(type, n);
 34     return (spline);
 35 }
 36 
 37 /*
 38 As spline2_make but does not allocate control points.
 39 */
 40  Spline2 *spline2_alloc(int type, int n)
 41 {
 42     Spline2 *spline = talloc(Spline2);
 43 
 44     spline->type = type;
 45     spline->n = n;
 46     spline->x = NULL;
 47     spline->y = NULL;
 48     return (spline);
 49 }
 50 
 51  void spline2_print(FILE *pf, Spline2 *spline)
 52 {
 53     int i;
 54     if (spline==NULL||pf==NULL) return;
 55     fprintf(pf,"type %d points %d \n",spline->type,spline->n);
 56     for (i=-1;i<spline->n+2;i++)
 57        fprintf(pf,"x %3.2f y %3.2f \n",spline->x->p[i],spline->y->p[i]);
 58 }
 59 
 60  Spline2 *spline2_read(FILE *pf)
 61 {
 62     int i, n, type;
 63     Spline2 *newspline;
 64     if (pf==NULL) return(NULL);
 65 
 66     fscanf(pf,"%*s %d %*s %d",&type,&n);  
 67     newspline = spline2_make(type, n);
 68     for (i=-1;i<n+2;i++)
 69        fscanf(pf,"%*s %lf %*s %lf",&(newspline->x->p[i]),&(newspline->y->p[i]));
 70 
 71     return(newspline);
 72 }
 73 
 74 /*
 75 Allocate and return copy of a 2D-spline.
 76 */
 77  Spline2 *spline2_copy(Spline2 * spline)
 78 {
 79     Spline2 *newspline;
 80     if (spline == NULL)
 81         return (NULL);
 82     newspline = talloc(Spline2);
 83     newspline->type = spline->type;
 84     newspline->n = spline->n;
 85     newspline->x = spline_copy(spline->x);
 86     newspline->y = spline_copy(spline->y);
 87     return (newspline);
 88 }
 89 
 90 /*
 91 Free 2D-spline and asssociated memory.
 92 */
 93  void spline2_free(Spline2 * spline)
 94 {
 95     if (spline == NULL)
 96         return;
 97     spline_free(spline->x);
 98     spline_free(spline->y);
 99     rfree(spline);
100 }
101 
102 /*
103 Evaluate 2D-spline at parameter t.
104 */
105  Vec2 spline2_eval(Spline2 * spline, double t)
106 {
107     double x = spline_eval(spline->x, t);
108     double y = spline_eval(spline->y, t);
109     return (vec2(x, y));
110 }
111 
112 static Vec2 p00;
113 static Spline2 *spline00;
114 static double dist(double t)
115 {
116     return (vec2_dist(spline2_eval(spline00, t), p00));
117 }
118 
119 static int tol(double a, double b)
120 {
121     return (fabs(a - b) > 0.01);
122 }
123 
124  double spline2_natural_param(Spline2 * spline, Vec2 p)
125 {
126     int n = spline->n;
127     double t, t1, t2, tmin = 0.0, d, dmin = MAXFLOAT;
128 
129     for (t = 0.0; t < n - 1.0; t++)
130     {
131         d = vec2_dist(spline2_eval(spline, t), p);
132         if (dmin > d)
133         {
134             dmin = d;
135             tmin = t;
136         }
137     }
138     if (tmin == 0.0)
139     {
140         t1 = 0.0;
141         t2 = 1.0;
142     } else if (tmin == n - 1.0)
143     {
144         t1 = n - 2.0;
145         t2 = n - 1.0;
146     } else
147     {
148         t1 = tmin - 1.0;
149         t2 = tmin + 1.0;
150     }
151     spline00 = spline;
152     p00 = p;
153     t = golden_section(dist, t1, t2, tol, NULL, NULL);
154     return (t);
155 }
156 
157 static double spline2_periodic_param(Spline2 * spline, Vec2 p)
158 {
159     int n = spline->n;
160     double t, t1, t2, tmin = 0.0, d, dmin = MAXFLOAT;
161 
162     for (t = 0.0; t < n; t++)
163     {
164         d = vec2_dist(spline2_eval(spline, t), p);
165         if (dmin > d)
166         {
167             dmin = d;
168             tmin = t;
169         }
170     }
171     t1 = tmin - 1.0;
172     t2 = tmin + 1.0;
173     spline00 = spline;
174     p00 = p;
175     t = golden_section(dist, t1, t2, tol, NULL, NULL);
176     if (t < 0.0)
177         t += n;
178     if (t >= n)
179         t -= n;
180     return (t);
181 }
182 
183 /*
184 Return parameter of point on 2D-spline closest to point p.
185 */
186  double spline2_param(Spline2 * spline, Vec2 p)
187 {
188     switch (spline->type)
189     {
190         case SPLINE_NATURAL:
191         case SPLINE_TANGENT:
192             return (spline2_natural_param(spline, p));
193         case SPLINE_PERIODIC:
194             return (spline2_periodic_param(spline, p));
195     }
196     return (0.0);
197 }
198 
199 /*
200 Return closest point on spline to p.
201 */
202  Vec2 spline2_closest(Spline2 *spline, Vec2 p)
203 {
204     double t = spline2_param(spline, p);
205     return(spline2_eval(spline, t));
206 }
207  
208 
209 /*
210 Return distance from point p of closest point on 2D-spline.
211 */
212  double spline2_dist(Spline2 * spline, Vec2 p)
213 {
214     double t = spline2_param(spline, p);
215     return(vec2_dist(p, spline2_eval(spline, t)));
216 }
217 
218 /*
219 Interplate an (already allocated) 2D-spline through data values
220 q[0], ... , q[n-1].
221 */
222  void spline2_interpolate(Spline2 * spline, Vec2 * p)
223 {
224 
225     int i, l, u;
226     double *x, *y;
227     switch(spline->type)
228     {
229         case SPLINE_TANGENT:
230             l = -1;
231             u = spline->n+1;
232             break;
233         case SPLINE_NATURAL:
234         case SPLINE_PERIODIC:
235             l = 0;
236             u = spline->n;
237             break;
238         default:
239             return;
240     }
241     x = tvector_alloc(l, u, double);
242     y = tvector_alloc(l, u, double);
243     for (i = l; i < u; i++)
244     {
245         x[i] = vec2_x(p[i]);
246         y[i] = vec2_y(p[i]);
247     }
248     spline_interpolate(spline->x, x);
249     spline_interpolate(spline->y, y);
250     tvector_free(x, l, double);
251     tvector_free(y, l, double);
252 }
253 
254 
255 /*
256 Interplate an (already allocated) 2D-spline through data points
257 stored in list points.
258 */
259 Spline2 *spline2_interpolate_list(int type, List * points)
260 {
261 
262     List *ptr;
263     Spline2 *spline;
264     int i, n, l, u;
265     double *x, *y;
266     switch(type)
267     {
268         case SPLINE_TANGENT:
269             n = list_length(points)-2;
270             l = -1;
271             u = n+1;
272             break;
273         case SPLINE_NATURAL:
274         case SPLINE_PERIODIC:
275             n = list_length(points);
276             l = 0;
277             u = n;
278             break;
279         default:
280             return(NULL);
281     }
282     x = tvector_alloc(l, u, double);
283     y = tvector_alloc(l, u, double);
284     spline = spline2_make(type, n);
285     for (i = l, ptr = points; i < u; i++, ptr = ptr->next)
286     {
287         Vec2 *p = (Vec2 *) ptr->to;
288         x[i] = vec2_x(*p);
289         y[i] = vec2_y(*p);
290     }
291     spline_interpolate(spline->x, x);
292     spline_interpolate(spline->y, y);
293     tvector_free(x, l, double);
294     tvector_free(y, l, double);
295     return (spline);
296 }
297 
298 Spline2 *spline2_interpolate_ddlist(int type, List * points)
299 {
300 
301     List *ptr;
302     Spline2 *spline;
303     int i, n, l, u;
304     double *x, *y;
305     switch(type)
306     {
307         case SPLINE_TANGENT:
308             n = dd_list_length(points)-2;
309             l = -1;
310             u = n+1;
311             break;
312         case SPLINE_NATURAL:
313         case SPLINE_PERIODIC:
314             n = dd_list_length(points);
315             l = 0;
316             u = n;
317             break;
318         default:
319             return(NULL);
320     }
321     x = tvector_alloc(l, u, double);
322     y = tvector_alloc(l, u, double);
323     spline = spline2_make(type, n);
324     for (i = l, ptr = points; i < u; i++, ptr = ptr->next)
325     {
326         Vec2 *p = (Vec2 *) ptr->to;
327         x[i] = vec2_x(*p);
328         y[i] = vec2_y(*p);
329     }
330     spline_interpolate(spline->x, x);
331     spline_interpolate(spline->y, y);
332     tvector_free(x, l, double);
333     tvector_free(y, l, double);
334     return (spline);
335 }
336 
337 /*
338 Makes 2D-spline interpolate a different point p at knot ip.
339 */
340  void spline2_replace_point(Spline2 * spline, int ip, Vec2 p)
341 {
342     spline_replace_point(spline->x, ip, vec2_x(p));
343     spline_replace_point(spline->y, ip, vec2_y(p));
344 }
345 
346 /*
347 Makes 2D-spline interpolate a new point p after knot ibelow.
348 */
349  void spline2_add_point(Spline2 * spline, int ibelow, Vec2 p)
350 {
351     spline_add_point(spline->x, ibelow, vec2_x(p));
352     spline_add_point(spline->y, ibelow, vec2_y(p));
353     spline->n++;
354 }
355 
356 /*
357 Deletes interpolation point at knot ip from 2D-spline.
358 */
359  void spline2_delete_point(Spline2 * spline, int i)
360 {
361     if (spline->n < 3)
362         return;
363     spline_delete_point(spline->x, i);
364     spline_delete_point(spline->y, i);
365     spline->n--;
366 }
367 
368 /*
369 Given an array of n-2D-splines in stack, interpolate a 2D-spline
370 between them at parameter 0<=t<=n-1;
371 */
372 Spline2 *spline2_stack_interp(int n, Spline2 ** spline, double t)
373 {
374     Spline **x, **y;
375     Spline2 *s;
376     int i;
377 
378     x = tvector_alloc(0, n, Spline *);
379     y = tvector_alloc(0, n, Spline *);
380     for (i = 0; i < n; i++)
381     {
382         x[i] = spline[i]->x;
383         y[i] = spline[i]->y;
384     }
385     s = spline2_alloc(spline[0]->type, spline[0]->n);
386     s->x = spline_stack_interp(n, x, t);
387     s->y = spline_stack_interp(n, y, t);
388     tvector_free(x, 0, Spline *);
389     tvector_free(y, 0, Spline *);
390 
391     return (s);
392 }
393 
394 
395 /*
396 Apply rot translate and scale to 2D spline.
397 */
398 void spline2_rts(Spline2 *spline, Mat2 r, Vec2 t, double s)
399 {
400     int i;
401     if(spline == NULL)
402         return;
403     for(i = 0; i < spline->n; i++)
404     {
405         Vec2 p = spline2_eval(spline, i);
406         p = vec2_sum(t, vec2_times(s, mat2_vprod(r, p)));
407         spline2_replace_point(spline, i, p);
408     }
409 }
410 

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