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

Linux Cross Reference
Tina4/src/vision/spline/ucbs2.c

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

  1 /**@(#)
  2 **/
  3 #include <math.h>
  4 #include <tina/sys.h>
  5 #include <tina/sysfuncs.h>
  6 #include <tina/math.h>
  7 #include <tina/mathfuncs.h>
  8 #include <tina/vision.h>
  9 #include <tina/visionfuncs.h>
 10 
 11 /**
 12 values of basis cubics at  u
 13 **/
 14 
 15 void    ucbs2_basis_val(double u, double *b0, double *b1, double *b2, double *b3)
 16 {
 17     double  u1 = 1 - u;
 18 
 19     *b0 = u * u * u / 6.0;
 20     *b1 = (1.0 + u * (3.0 + u * (3.0 - u * 3.0))) / 6.0;
 21     *b2 = (4.0 + u * u * (-6.0 + u * 3.0)) / 6.0;
 22     *b3 = u1 * u1 * u1 / 6.0;
 23 }
 24 
 25 /**
 26 
 27 evaluate uniform cubic spline at t in [0, n)
 28 **/
 29 
 30 Vec2    ucbs2_eval(Ucbs2 * ucbs, double t)
 31 {
 32     int     i;
 33     double  u = 0.0;
 34     double x, y;
 35     double  b0, b1, b2, b3;
 36     double  cx0, cx1, cx2, cx3;
 37     double  cy0, cy1, cy2, cy3;
 38     double *cx = ucbs->cx;
 39     double *cy = ucbs->cy;
 40 
 41     i = (int)floor(t);
 42     switch (ucbs->type)         /** if i out of range **/
 43     {
 44     case LOOP:                  /** make periodic **/
 45         u = t - i;
 46         while (i < 0)
 47             i += ucbs->n;
 48         i %= ucbs->n;
 49         break;
 50     case STRING:                /** extrapolate last cubic **/
 51         if (i < 0)
 52             i = 0;
 53         else if (i > ucbs->n - 2)
 54             i = ucbs->n - 2;
 55         u = t - i;
 56         break;
 57     }
 58 
 59     cx0 = cx[i];
 60     cy0 = cy[i];
 61     cx1 = cx[i + 1];
 62     cy1 = cy[i + 1];
 63     cx2 = cx[i + 2];
 64     cy2 = cy[i + 2];
 65     if (i == ucbs->n - 1)
 66     {
 67         cx3 = cx[2];
 68         cy3 = cy[2];
 69     } else
 70     {
 71         cx3 = cx[i + 3];
 72         cy3 = cy[i + 3];
 73     }
 74 
 75     ucbs2_basis_val(u, &b0, &b1, &b2, &b3);
 76     x = b3 * cx0 + b2 * cx1 + b1 * cx2 + b0 * cx3;
 77     y = b3 * cy0 + b2 * cy1 + b1 * cy2 + b0 * cy3;
 78     return (vec2(x, y));
 79 }
 80 
 81 /* find the spline parameter value approximately closest to point p */
 82 
 83 static double resolution = 0.25;
 84 static Vec2 v_close = {Vec2_id};
 85 static Ucbs2 *ucbs_close;
 86 
 87 void    ucbs_set_resolution(double res)
 88 {
 89     resolution = res * res;
 90 }
 91 
 92 static int tol(double t1, double t2)
 93 {
 94     Vec2    v1 = {Vec2_id};
 95     Vec2    v2 = {Vec2_id};
 96 
 97     v1 = ucbs2_eval(ucbs_close, t1);
 98     v2 = ucbs2_eval(ucbs_close, t2);
 99     return (vec2_sqrmod(vec2_diff(v2, v1)) > resolution);
100 }
101 
102 static double dist_to_v(double t)
103 {
104     Vec2    v = {Vec2_id};
105 
106     v = ucbs2_eval(ucbs_close, t);
107     return (vec2_sqrmod(vec2_diff(v, v_close)));
108 }
109 
110 /* find closest between params using golden section method */
111 double  ucbs2_closest_param_to(Ucbs2 * ucbs, Vec2 v, int i)
112 {
113     double  t1, t2;
114 
115     /* BUGFIX v1 & v2 unused */
116     /* Vec2    v1, v2; */
117 
118     if (ucbs == NULL)
119         return (0.0);
120 
121     v_close = v;
122     ucbs_close = ucbs;
123 
124     t1 = i - 1.0;
125     t2 = i + 1.0;
126 
127     /** commented to allow extrapolation
128     if (ucbs->type!=LOOP)
129     {
130         if (i==0)
131            t1 = i;
132         if (i==ucbs->n-1)
133            t2 = ucbs->n-1;
134     }
135     **/
136     /* BUGFIX v1 unused */
137     /* v1 = ucbs2_eval(ucbs, t1); */
138     /* BUGFIX v2 unused */
139     /* v2 = ucbs2_eval(ucbs, t2); */
140     return (golden_section(dist_to_v, t1, t2, tol, NULL,
141                            NULL));
142 }
143 
144 double  ucbs2_param(Ucbs2 * ucbs, Vec2 v)
145 {
146     int     n;
147     int     i, mini = 0;
148     double  t, d, mind;
149 
150     if (ucbs == NULL)
151         return (0.0);
152 
153     n = ucbs->n;
154     mind = vec2_sqrmod(vec2_diff(v, ucbs2_eval(ucbs, 0.0)));
155     for (i = 0; i < n; ++i)
156     {
157         d = vec2_sqrmod(vec2_diff(v, ucbs2_eval(ucbs, (double) i)));
158 
159         if (d < mind)
160         {
161             mind = d;
162             mini = i;
163         }
164     }
165 
166     t = ucbs2_closest_param_to(ucbs, v, mini);
167 
168     if (ucbs->type == LOOP)
169     {
170         if (t < 0)
171             t += n;
172         else if (t > n)
173             t -= n;
174         return (t);
175     } else
176         return (t);
177 }
178 
179 /**
180 make a spline with given number of knots
181 **/
182 
183 Ucbs2  *ucbs2_make(int type, int n)
184 {
185     Ucbs2  *ucbs = ts_ralloc(Ucbs2);
186 
187     ucbs->type = type;
188     ucbs->n = n;
189     ucbs->cx = dvector_alloc(0, n + 2);
190     ucbs->cy = dvector_alloc(0, n + 2);
191     ucbs->props = NULL;
192 
193     return (ucbs);
194 }
195 
196 Ucbs2  *ucbs2_copy(Ucbs2 * old)
197 {
198     Ucbs2  *new;
199 
200     if (old == NULL)
201         return (NULL);
202 
203     new = ts_ralloc(Ucbs2);
204     new->type = old->type;
205     new->n = old->n;
206     new->cx = dvector_dcopy(old->cx, 0, old->n + 2);
207     new->cy = dvector_dcopy(old->cy, 0, old->n + 2);
208     new->props = proplist_copy(old->props);
209 
210     return (new);
211 }
212 
213 void    ucbs2_free(Ucbs2 * ucbs)
214 {
215     if (ucbs == NULL)
216         return;
217     dvector_free((void *) ucbs->cx, 0);
218     dvector_free((void *) ucbs->cy, 0);
219     rfree((void *) ucbs);
220 }
221 
222 static void interpolate(int type, int n, double *x, double *cx)
223 {
224     int     i;
225     double *a = dvector_alloc(0, n);
226     double *b = dvector_alloc(0, n);
227     double *c = dvector_alloc(0, n);
228 
229     for (i = 0; i < n; i++)
230         cx[i + 1] = x[i];
231 
232     /** end conditions **/
233     switch (type)
234     {
235     case LOOP:
236         /** periodic **/
237         a[0] = c[0] = 1.0 / 6.0;
238         b[0] = 2.0 / 3.0;
239         a[n - 1] = c[n - 1] = 1.0 / 6.0;
240         b[n - 1] = 2.0 / 3.0;
241         break;
242     case STRING:
243         /** natural **/
244         a[0] = c[0] = 0.0;
245         b[0] = 1.0;
246         a[n - 1] = c[n - 1] = 0.0;
247         b[n - 1] = 1.0;
248         break;
249     }
250 
251     for (i = 1; i < n - 1; i++)
252     {
253         a[i] = c[i] = 1.0 / 6.0;
254         b[i] = 2.0 / 3.0;
255     }
256 
257     triadiag(n, a, b, c, cx + 1);
258     dvector_free((void *) a, 0);
259     dvector_free((void *) b, 0);
260     dvector_free((void *) c, 0);
261 }
262 
263 void    ucbs2_end_conditions(Ucbs2 * ucbs)
264 {
265     double *cx = ucbs->cx;
266     double *cy = ucbs->cy;
267     int     n = ucbs->n;
268 
269     /** end conditions **/
270     switch (ucbs->type)
271     {
272     case LOOP:
273         /** periodic **/
274         cx[0] = cx[n];
275         cx[n + 1] = cx[1];
276         cy[0] = cy[n];
277         cy[n + 1] = cy[1];
278         break;
279     case STRING:
280         /** natural **/
281         cx[0] = 2.0 * cx[1] - cx[2];
282         cx[n + 1] = 2.0 * cx[n] - cx[n - 1];
283         cy[0] = 2.0 * cy[1] - cy[2];
284         cy[n + 1] = 2.0 * cy[n] - cy[n - 1];
285         break;
286     }
287 }
288 
289 void    ucbs2_interpolate(Ucbs2 * ucbs, double *x, double *y)
290 {
291     interpolate(ucbs->type, ucbs->n, x, ucbs->cx);
292     interpolate(ucbs->type, ucbs->n, y, ucbs->cy);
293     ucbs2_end_conditions(ucbs);
294 }
295 
296 /* delete a control point from a ucbs */
297 void    ucbs2_delete_control(Ucbs2 * ucbs, int t)
298 
299 /* the control point in range 1 to n */
300 {
301     double *x, *y;
302     int     i, n;
303     Vec2    v = {Vec2_id};
304 
305     t--;                        /* parameter value of control point */
306     if (ucbs == NULL || t < 0 || t > ucbs->n)
307         return;
308     n = ucbs->n - 1;
309 
310     if (n == 0)
311     {
312         ucbs->n = 0;
313         return;
314     }
315     x = dvector_alloc(0, n);
316     y = dvector_alloc(0, n);
317 
318     for (i = 0; i < t; i++)
319     {
320         v = ucbs2_eval(ucbs, (double) i);
321         x[i] = vec2_x(v);
322         y[i] = vec2_y(v);
323     }
324     for (; i < n; ++i)
325     {
326         v = ucbs2_eval(ucbs, i + 1.0);
327         x[i] = vec2_x(v);
328         y[i] = vec2_y(v);
329     }
330 
331     ucbs->n = n;
332     ucbs2_interpolate(ucbs, x, y);
333     dvector_free((void *) x, 0);
334     dvector_free((void *) y, 0);
335 }
336 
337 /* add a new control point to ucbs */
338 void    ucbs2_add_control(Ucbs2 * ucbs, int t, Vec2 vt)
339 
340 /* new control point added after t in range 1 to n */
341 /* new interpolation point must pass through vt */
342 {
343     double *x, *y;
344     int     i, n;
345     Vec2    v = {Vec2_id};
346 
347     t--;                        /* parameter value of control point */
348     if (ucbs == NULL || t < 0 || t >= ucbs->n)
349         return;
350     n = ucbs->n + 1;
351 
352     x = dvector_alloc(0, n);
353     y = dvector_alloc(0, n);
354 
355     for (i = 0; i <= t; i++)
356     {
357         v = ucbs2_eval(ucbs, (double) i);
358         x[i] = vec2_x(v);
359         y[i] = vec2_y(v);
360     }
361 
362     x[i] = vec2_x(vt);
363     y[i] = vec2_y(vt);
364     i++;
365 
366     for (; i < n; i++)
367     {
368         v = ucbs2_eval(ucbs, i - 1.0);
369         x[i] = vec2_x(v);
370         y[i] = vec2_y(v);
371     }
372 
373     dvector_free((void *) ucbs->cx, 0);
374     dvector_free((void *) ucbs->cy, 0);
375     ucbs->cx = dvector_alloc(0, n + 2);
376     ucbs->cy = dvector_alloc(0, n + 2);
377     ucbs->n = n;
378     ucbs2_interpolate(ucbs, x, y);
379     dvector_free((void *) x, 0);
380     dvector_free((void *) y, 0);
381 }
382 
383 /* add a new control point to ucbs: version above is strange */
384 void    ucbs2_add_point(Ucbs2 * ucbs, int t, Vec2 vt)
385 
386 /* control point added after t */
387 /* new interpolation point must pass through vt */
388 {
389     double *x, *y;
390     int     i, n;
391     Vec2    v = {Vec2_id};
392 
393     if (ucbs == NULL || t < 0 || t >= ucbs->n)
394         return;
395     n = ucbs->n + 1;
396 
397     x = dvector_alloc(0, n);
398     y = dvector_alloc(0, n);
399 
400     for (i = 0; i <= t; i++)
401     {
402         v = ucbs2_eval(ucbs, (double) i);
403         x[i] = vec2_x(v);
404         y[i] = vec2_y(v);
405     }
406     x[i] = vec2_x(vt);
407     y[i] = vec2_y(vt);
408     i++;
409     for (; i < n; i++)
410     {
411         v = ucbs2_eval(ucbs, i - 1.0);
412         x[i] = vec2_x(v);
413         y[i] = vec2_y(v);
414     }
415 
416     dvector_free((void *) ucbs->cx, 0);
417     dvector_free((void *) ucbs->cy, 0);
418     ucbs->cx = dvector_alloc(0, n + 2);
419     ucbs->cy = dvector_alloc(0, n + 2);
420     ucbs->n = n;
421     ucbs2_interpolate(ucbs, x, y);
422     dvector_free((void *) x, 0);
423     dvector_free((void *) y, 0);
424 }
425 
426 /* delete a control point from a ucbs: version above is strange */
427 void    ucbs2_delete_point(Ucbs2 * ucbs, int t)
428 
429 /* the control point */
430 {
431     double *x, *y;
432     int     i, n;
433     Vec2    v = {Vec2_id};
434 
435     if (ucbs == NULL || t < 0 || t > ucbs->n || ucbs->n < 6)
436         return;
437     n = ucbs->n - 1;
438 
439     if (n == 0)
440     {
441         ucbs->n = 0;
442         return;
443     }
444     x = dvector_alloc(0, n);
445     y = dvector_alloc(0, n);
446 
447     for (i = 0; i < t; i++)
448     {
449         v = ucbs2_eval(ucbs, (double) i);
450         x[i] = vec2_x(v);
451         y[i] = vec2_y(v);
452     }
453     for (; i < n; ++i)
454     {
455         v = ucbs2_eval(ucbs, i + 1.0);
456         x[i] = vec2_x(v);
457         y[i] = vec2_y(v);
458     }
459 
460     ucbs->n = n;
461     ucbs2_interpolate(ucbs, x, y);
462     dvector_free((void *) x, 0);
463     dvector_free((void *) y, 0);
464 }
465 
466 void    ucbs2_replace_point(Ucbs2 * ucbs, int ip, Vec2 p)
467 
468 
469 /* new i'th interpolation point must pass through p */
470 {
471     double *x, *y;
472     int     i, n;
473 
474     if (ucbs == NULL || ip < 0 || ip > ucbs->n)
475         return;
476 
477     n = ucbs->n;
478     x = dvector_alloc(0, n);
479     y = dvector_alloc(0, n);
480 
481     /* old interpolation points */
482     for (i = 0; i < n; i++)
483     {
484         Vec2    v = {Vec2_id};
485 
486         v = ucbs2_eval(ucbs, (double) i);
487         x[i] = vec2_x(v);
488         y[i] = vec2_y(v);
489     }
490 
491     /* replace point i = ip */
492     x[ip] = vec2_x(p);
493     y[ip] = vec2_y(p);
494 
495     ucbs2_interpolate(ucbs, x, y);
496     dvector_free((void *) x, 0);
497     dvector_free((void *) y, 0);
498 }
499 
500 Ucbs2  *ucbs2_interpolate_list(int type, List * points)
501 {
502     double *x, *y;
503     Ucbs2  *ucbs;
504     List   *ptr;
505     int     i, n;
506 
507     n = list_length(points);
508     ucbs = ucbs2_make(type, n);
509     x = dvector_alloc(0, n);
510     y = dvector_alloc(0, n);
511     for (i = 0, ptr = points; i < n; i++, ptr = ptr->next)
512     {
513         Vec2   *v = (Vec2 *) ptr->to;
514 
515         x[i] = vec2_x(*v);
516         y[i] = vec2_y(*v);
517     }
518     interpolate(ucbs->type, ucbs->n, x, ucbs->cx);
519     interpolate(ucbs->type, ucbs->n, y, ucbs->cy);
520     dvector_free((void *) x, 0);
521     dvector_free((void *) y, 0);
522     ucbs2_end_conditions(ucbs);
523 
524     return (ucbs);
525 }
526 
527 void    ucbs2_interpolate_kwsnake(Ucbs2 * ucbs, Kwsnake * kwsnake, int *mask)
528 {
529     double *x, *y;
530     int     i, n;
531 
532     if (ucbs == NULL || kwsnake == NULL || ucbs->n > kwsnake->n)
533         return;
534 
535     n = ucbs->n;
536 
537     if (mask == NULL)
538     {
539         ucbs2_interpolate(ucbs, kwsnake->x, kwsnake->y);
540         return;
541     }
542     x = dvector_alloc(0, n);
543     y = dvector_alloc(0, n);
544 
545     for (i = 0; i < n; ++i)
546     {
547         x[i] = kwsnake->x[mask[i]];
548         y[i] = kwsnake->y[mask[i]];
549     }
550 
551     ucbs2_interpolate(ucbs, x, y);
552     dvector_free((void *) x, 0);
553     dvector_free((void *) y, 0);
554 }
555 
556 Ucbs2  *ucbs2_of_str2(Tstring * es, int ds)
557 {
558     int     n;
559     Ucbs2  *ucbs;
560     double *x, *y;
561 
562     if (es == NULL)
563         return (NULL);
564 
565     n = MAX(5, es->count / ds);
566     ucbs = ucbs2_make(es->type, n);
567     x = dvector_alloc(0, n);
568     y = dvector_alloc(0, n);
569     str2_get_knots(es, n, x, y);
570     ucbs2_interpolate(ucbs, x, y);
571     dvector_free((void *) x, 0);
572     dvector_free((void *) y, 0);
573     return (ucbs);
574 }
575 
576 Tstring *str2_of_ucbs2(Ucbs2 * ucbs)
577 {
578     double  t1 = 0.0, t2 = 0.0;
579 
580     if (ucbs == NULL)
581         return (NULL);
582 
583     switch (ucbs->type)
584     {
585     case LOOP:
586         t2 = ucbs->n;
587         break;
588     case STRING:
589         t2 = ucbs->n - 1;
590         break;
591     }
592     return (str2_of_curve2(ucbs2_eval, (void *) ucbs, t1, t2, NULL));
593 }
594 

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