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

Linux Cross Reference
Tina6/tina-libs/tina/geometry/geomSpline_ucbs2.c

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

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

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