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

Linux Cross Reference
Tina4/src/vision/spline/kws_run.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 void    kwsnake_correct(Kwsnake * kwsnake, double *fx, double *fy, double dt)
 12 {
 13     int     i;
 14 
 15     for (i = 0; i < kwsnake->n; i++)
 16     {
 17         kwsnake->x[i] -= dt * fx[i];
 18         kwsnake->y[i] -= dt * fy[i];
 19     }
 20 }
 21 
 22 void    kwsnake_correct_maxstep(Kwsnake * kwsnake, double *dx, double *dy, double maxstep)
 23 {
 24     int     i;
 25     double  step = 0.0, k;
 26 
 27     for (i = 0; i < kwsnake->n; i++)
 28     {
 29         step = MAX(step, fabs(dx[i]));
 30         step = MAX(step, fabs(dy[i]));
 31     }
 32     if (step == 0.0)
 33         k = 0.0;
 34     else
 35         k = maxstep / step;
 36 
 37     kwsnake_correct(kwsnake, dx, dy, k);
 38 }
 39 
 40 void    kwsnake_correct_fullstep(Kwsnake * kwsnake, double *dx, double *dy, double fullstep)
 41 {
 42     int     i;
 43 
 44     for (i = 0; i < kwsnake->n; i++)
 45     {
 46         dx[i] = (dx[i] == 0) ? 0 : SGN(dx[i]);
 47         dy[i] = (dy[i] == 0) ? 0 : SGN(dy[i]);
 48     }
 49     kwsnake_correct(kwsnake, dx, dy, fullstep);
 50 }
 51 
 52 static Vec2 grad_orth(float *rast_orth, Vec2 p, Vec2 v, Vec2 q, int n1, int n2)
 53 {
 54     double  d, g;
 55     int     i;
 56 
 57     d = vec2_dot(v, vec2_diff(q, p));
 58     i = (int)floor(d);
 59 
 60     if (i < n1 || i > n2)
 61         return (vec2(0.0, 0.0));
 62 
 63     if (i > n1 && i < n2)
 64         g = (rast_orth[i + 1] - rast_orth[i - 1]) * 0.5;
 65     else if (i == n1)
 66         g = rast_orth[i + 1] - rast_orth[i];
 67     else
 68         g = rast_orth[i] - rast_orth[i - 1];
 69     return (vec2_times(g, v));
 70 }
 71 
 72 Vec2    kwsnake_orth(Kwsnake * kwsnake, int i)
 73 {
 74     int     n = kwsnake->n;
 75     double *x = kwsnake->x, *y = kwsnake->y;
 76     Vec2    v = {Vec2_id};
 77 
 78     if (i != 0 && i != n - 1)
 79         v = vec2_unit(vec2_diff(vec2(x[i + 1], y[i + 1]), vec2(x[i - 1], y[i - 1
 80 ])));
 81     else if (i == 0)
 82     {
 83         if (kwsnake->type == LOOP)
 84             v = vec2_unit(vec2_diff(vec2(x[1], y[1]), vec2(x[n - 1], y[n - 1])))
 85 ;
 86         else
 87             v = vec2_unit(vec2_diff(vec2(x[1], y[1]), vec2(x[0], y[0])));
 88     } else
 89     {
 90         if (kwsnake->type == LOOP)
 91             v = vec2_unit(vec2_diff(vec2(x[0], y[0]), vec2(x[n - 2], y[n - 2])))
 92 ;
 93         else
 94             v = vec2_unit(vec2_diff(vec2(x[n - 1], y[n - 1]), vec2(x[n - 2], y[n
 95  - 2])));
 96     }
 97 
 98     return (vec2(-vec2_y(v), vec2_x(v)));
 99 }
100 
101 
102 void    kwsnake_external_step_orth(Kwsnake * kwsnake, float **im_orth, Vec2 * p, Vec2 * v, int n1, int n2, double step, int type)
103 {
104     static int n = 0;
105     double *x = kwsnake->x;
106     double *y = kwsnake->y;
107     static double *fx = NULL, *fy = NULL;
108     int     i;
109 
110     if (kwsnake == NULL)
111         return;
112 
113     if (n != kwsnake->n)
114     {
115         n = kwsnake->n;
116         /* BUG  dvector_free(fx, 0); dvector_free(fy, 0); */
117         fx = dvector_alloc(0, n);
118         fy = dvector_alloc(0, n);
119     }
120     for (i = 0; i < n; i++)
121     {
122         Vec2    grad = {Vec2_id};
123 
124         grad = grad_orth(im_orth[i], p[i], v[i], vec2(x[i], y[i]), n1, n2);
125         fx[i] = vec2_x(grad);
126         fy[i] = vec2_y(grad);
127     }
128 
129     switch (type)
130     {
131     case KW_STEP:
132         kwsnake_correct(kwsnake, fx, fy, step);
133         break;
134     case KW_MAXSTEP:
135         kwsnake_correct_maxstep(kwsnake, fx, fy, step);
136         break;
137     case KW_FULLSTEP:
138         kwsnake_correct_fullstep(kwsnake, fx, fy, step);
139         break;
140     }
141 }
142 
143 static void im_gradsubpixf(Imrect * im, double y, double x, double *gy, double *gx)
144 {
145     double  g1, g2;
146 
147     g1 = im_sub_pixf(im, y - 1, x);
148     g2 = im_sub_pixf(im, y + 1, x);
149     *gy = 0.5 * (g2 - g1);
150     g1 = im_sub_pixf(im, y, x - 1);
151     g2 = im_sub_pixf(im, y, x + 1);
152     *gx = 0.5 * (g2 - g1);
153 }
154 
155 static void im_d2subpixf(Imrect * im, double y, double x, double *ixx, double *ixy, double *iyy)
156 {
157     *ixx = 0.5 * (im_sub_pixf(im, y, x + 1)
158                   - 2.0 * im_sub_pixf(im, y, x)
159                   + im_sub_pixf(im, y, x - 1));
160     *iyy = 0.5 * (im_sub_pixf(im, y + 1, x)
161                   - 2.0 * im_sub_pixf(im, y, x)
162                   + im_sub_pixf(im, y - 1, x));
163     *ixy = 0.25 * (im_sub_pixf(im, y + 1, x + 1)
164                    - im_sub_pixf(im, y + 1, x - 1)
165                    - im_sub_pixf(im, y - 1, x + 1)
166                    + im_sub_pixf(im, y - 1, x - 1));
167 }
168 
169 void    kwsnake_external_step_from_pot(Kwsnake * kwsnake, Imrect * pot, double step, int type)
170 {
171     static int n = 0;
172     double *x = kwsnake->x;
173     double *y = kwsnake->y;
174     static double *fx = NULL, *fy = NULL;
175     int     i;
176 
177     if (kwsnake == NULL)
178         return;
179 
180     if (n != kwsnake->n)
181     {
182         n = kwsnake->n;
183         dvector_free((void *) fx, 0);
184         dvector_free((void *) fy, 0);
185         fx = dvector_alloc(0, n);
186         fy = dvector_alloc(0, n);
187     }
188     for (i = 0; i < n; i++)
189     {
190         double  gx, gy;
191 
192         im_gradsubpixf(pot, y[i], x[i], &gy, &gx);
193         fx[i] = gx;
194         fy[i] = gy;
195     }
196     switch (type)
197     {
198     case KW_STEP:
199         kwsnake_correct(kwsnake, fx, fy, step);
200         break;
201     case KW_MAXSTEP:
202         kwsnake_correct_maxstep(kwsnake, fx, fy, step);
203         break;
204     case KW_FULLSTEP:
205         kwsnake_correct_fullstep(kwsnake, fx, fy, step);
206         break;
207     }
208 }
209 
210 static void pentadiag0(int n_call, double a0, double b0, double c0, double d0, double e0, double *x, int cyclic)
211 {
212     static double *a, *b, *c, *d, *e;
213     static int n = 0;
214     int     i;
215 
216     if (n != n_call)
217     {
218         n = n_call;
219         dvector_free((void *) a, 0);
220         dvector_free((void *) b, 0);
221         dvector_free((void *) c, 0);
222         dvector_free((void *) d, 0);
223         dvector_free((void *) e, 0);
224         a = dvector_alloc(0, n);
225         b = dvector_alloc(0, n);
226         c = dvector_alloc(0, n);
227         d = dvector_alloc(0, n);
228         e = dvector_alloc(0, n);
229     }
230     for (i = 0; i < n; i++)
231     {
232         a[i] = a0;
233         b[i] = b0;
234         c[i] = c0;
235         d[i] = d0;
236         e[i] = e0;
237     }
238     if (!cyclic)
239     {
240         a[0] = a[1] = e[n - 1] = e[n - 2] = b[0] = d[n - 1] = 0;
241         c[0] += a0 + b0;
242         c[1] += a0;
243         c[n - 1] += e0 + d0;
244         c[n - 2] += e0;
245     }
246     pentadiag(n, a, b, c, d, e, x);
247 }
248 
249 void    kwsnake_internal_step_orth(Kwsnake * kwsnake, Vec2 * p, Vec2 * v, double alpha, double beta)
250 {
251     double *x = kwsnake->x;
252     double *y = kwsnake->y;
253     static double *o;
254     double  a, b, c, d, e;
255     static int n = 0;
256     int     i;
257 
258     if (n != kwsnake->n)
259     {
260         n = kwsnake->n;
261         dvector_free((void *) o, 0);
262         o = dvector_alloc(0, n);
263     }
264     for (i = 0; i < n; ++i)
265         o[i] = vec2_dot(vec2_diff(vec2(x[i], y[i]), p[i]), v[i]);
266 
267     e = a = beta;
268     d = b = -alpha - 4.0 * beta;
269     c = 1.0 + 2.0 * alpha + 6.0 * beta;
270 
271     pentadiag0(n, a, b, c, d, e, o, kwsnake->type == LOOP);
272 
273     for (i = 0; i < n; ++i)
274     {
275         Vec2    pi = {Vec2_id};
276 
277         pi = vec2_sum(p[i], vec2_times(o[i], v[i]));
278         x[i] = vec2_x(pi);
279         y[i] = vec2_y(pi);
280     }
281 }
282 
283 void    kwsnake_dt_step(Kwsnake * kwsnake, Imrect * pot, double alpha, double beta, double trans_dt, double shape_dt)
284 {
285     static int n = -1;
286     static double *fx = NULL, *fy = NULL, fxmean, fymean;
287     double *x = kwsnake->x;
288     double *y = kwsnake->y;
289     double  a, b, c, d, e, fmax;
290     int     i;
291 
292     if (kwsnake == NULL)
293         return;
294 
295     if (n != kwsnake->n)
296     {
297         n = kwsnake->n;
298         /* BUG  dvector_alloc(fx, 0); dvector_alloc(fy, 0); */
299         fx = dvector_alloc(0, n);
300         fy = dvector_alloc(0, n);
301     }
302     fmax = 0.0;
303     for (i = 0; i < n; i++)
304     {
305         double  gx, gy, k;
306         Vec2    n = {Vec2_id};
307         Vec2    kwsnake_orth();
308 
309         im_gradsubpixf(pot, y[i], x[i], &gy, &gx);
310 
311         n = kwsnake_orth(kwsnake, i);
312         k = fabs(vec2_dot(n, vec2_unit(vec2(gx, gy))));
313         gx *= k;
314         gy *= k;
315 
316         fx[i] = -gx;
317         fxmean += fx[i];
318         fmax = MAX(fmax, fabs(gx));
319         fy[i] = -gy;
320         fymean += fy[i];
321         fmax = MAX(fmax, fabs(gy));
322     }
323     fxmean /= n;
324     fymean /= n;
325 
326     if (fmax == 0.0)
327         fmax = 1.0;
328     for (i = 0; i < n; i++)
329     {
330         fx[i] -= fxmean;
331         fx[i] /= fmax;
332         fy[i] -= fymean;
333         fy[i] /= fmax;
334     }
335 
336     fmax = MAX(fabs(fxmean), fabs(fymean));
337     if (fmax == 0.0)
338         fmax = 1.0;
339     fxmean /= fmax;
340     fymean /= fmax;
341 
342     for (i = 0; i < n; i++)
343     {
344         x[i] += trans_dt * fxmean + shape_dt * fx[i];
345         y[i] += trans_dt * fymean + shape_dt * fy[i];
346     }
347 
348     e = a = shape_dt * beta;
349     d = b = -shape_dt * (alpha + 4.0 * beta);
350     c = 1.0 + shape_dt * (2.0 * alpha + 6.0 * beta);
351 
352     pentadiag0(n, a, b, c, d, e, x, kwsnake->type == LOOP);
353     pentadiag0(n, a, b, c, d, e, y, kwsnake->type == LOOP);
354 }
355 
356 void    kwsnake_dt_step2(Kwsnake * kwsnake, Imrect * pot, double alpha, double beta, double trans_dt, double shape_dt)
357 {
358     static int n = -1;
359     static double *fx = NULL, *fy = NULL, fxmean, fymean;
360     double *x = kwsnake->x;
361     double *y = kwsnake->y;
362     double  a, b, c, d, e, fmax;
363     int     i;
364 
365     if (kwsnake == NULL)
366         return;
367 
368     if (n != kwsnake->n)
369     {
370         n = kwsnake->n;
371         /* BUG  dvector_alloc(fx, 0); dvector_alloc(fy, 0); */
372         fx = dvector_alloc(0, n);
373         fy = dvector_alloc(0, n);
374     }
375     fmax = 0.0;
376     for (i = 0; i < n; i++)
377     {
378         double  ixx, ixy, iyy, gx, gy;
379         Vec2    normal = {Vec2_id};
380         Vec2    kwsnake_orth();
381 
382         im_d2subpixf((Imrect *) pot, *y, *x, &ixx, &ixy, &iyy);
383         normal = kwsnake_orth(kwsnake, i);
384         gx = ixx * vec2_x(normal) + ixy * vec2_y(normal);
385         gy = ixy * vec2_x(normal) + iyy * vec2_y(normal);
386 
387         fx[i] = -gx;
388         fxmean += fx[i];
389         fmax = MAX(fmax, fabs(gx));
390         fy[i] = -gy;
391         fymean += fy[i];
392         fmax = MAX(fmax, fabs(gy));
393     }
394     fxmean /= n;
395     fymean /= n;
396 
397     if (fmax == 0.0)
398         fmax = 1.0;
399     for (i = 0; i < n; i++)
400     {
401         fx[i] -= fxmean;
402         fx[i] /= fmax;
403         fy[i] -= fymean;
404         fy[i] /= fmax;
405     }
406 
407     fmax = MAX(fabs(fxmean), fabs(fymean));
408     if (fmax == 0.0)
409         fmax = 1.0;
410     fxmean /= fmax;
411     fymean /= fmax;
412 
413     for (i = 0; i < n; i++)
414     {
415         x[i] += trans_dt * fxmean + shape_dt * fx[i];
416         y[i] += trans_dt * fymean + shape_dt * fy[i];
417     }
418 
419     e = a = shape_dt * beta;
420     d = b = -shape_dt * (alpha + 4.0 * beta);
421     c = 1.0 + shape_dt * (2.0 * alpha + 6.0 * beta);
422 
423     pentadiag0(n, a, b, c, d, e, x, kwsnake->type == LOOP);
424     pentadiag0(n, a, b, c, d, e, y, kwsnake->type == LOOP);
425 }
426 
427 double  kwsnake_dist(Kwsnake * kws1, Kwsnake * kws2)
428 {
429     int     n = kws1->n;
430     double *x1 = kws1->x, *x2 = kws2->x;
431     double *y1 = kws1->y, *y2 = kws2->y;
432     double  d, dmax = 0.0;
433     int     i;
434 
435     for (i = 0; i < n; i++)
436     {
437         d = fabs(x1[i] - x2[i]) + fabs(y1[i] - y2[i]);
438         if (dmax < d)
439             dmax = d;
440     }
441     return (dmax);
442 }
443 
444 double  kwsnake_dt_run(Kwsnake * kwsnake, Imrect * pot, double alpha, double beta, double trans_dt, double shape_dt, int steps)
445 {
446     int     i;
447     double  d;
448     Kwsnake *copy = kwsnake_copy(kwsnake);
449 
450     for (i = 0; i < steps; i++)
451         kwsnake_dt_step(kwsnake, pot, alpha, beta, trans_dt, shape_dt);
452     d = kwsnake_dist(kwsnake, copy);
453     kwsnake_free(copy);
454     return (d);
455 }
456 
457 double  kwsnake_dt_run2(Kwsnake * kwsnake, Imrect * pot, double alpha, double beta, double trans_dt, double shape_dt, int steps)
458 {
459     int     i;
460     double  d;
461     Kwsnake *copy = kwsnake_copy(kwsnake);
462 
463     for (i = 0; i < steps; i++)
464         kwsnake_dt_step2(kwsnake, pot, alpha, beta, trans_dt, shape_dt);
465     d = kwsnake_dist(kwsnake, copy);
466     kwsnake_free(copy);
467     return (d);
468 }
469 
470 void    kwsnake_internal_step(Kwsnake * kwsnake, double alpha, double beta)
471 {
472     double *x = kwsnake->x;
473     double *y = kwsnake->y;
474     double  a, b, c, d, e;
475     int     n = kwsnake->n;
476 
477     e = a = beta;
478     d = b = -alpha - 4.0 * beta;
479     c = 1.0 + 2.0 * alpha + 6.0 * beta;
480     pentadiag0(n, a, b, c, d, e, x, kwsnake->type == LOOP);
481     pentadiag0(n, a, b, c, d, e, y, kwsnake->type == LOOP);
482 }
483 
484 void    kwsnake_step_from_pot(Kwsnake * kwsnake, Imrect * pot, double alpha, double beta, double step, int type)
485 {
486     kwsnake_external_step_from_pot(kwsnake, pot, step, type);
487     kwsnake_internal_step(kwsnake, alpha, beta);
488 }
489 
490 void    kwsnake_reorth(Kwsnake * kwsnake, Vec2 * p, Vec2 * v)
491 {
492     double *x, *y;
493     int     i, n;
494 
495     if (kwsnake == NULL)
496         return;
497 
498     x = kwsnake->x;
499     y = kwsnake->y;
500     n = kwsnake->n;
501 
502     for (i = 0; i < n; ++i)
503     {
504         double  dp = vec2_dot(v[i], vec2_diff(vec2(x[i], y[i]), p[i]));
505         Vec2    new = {Vec2_id};
506 
507         new = vec2_sum(p[i], vec2_times(dp, v[i]));
508         x[i] = vec2_x(new);
509         y[i] = vec2_y(new);
510     }
511 }
512 
513 void    kwsnake_orth_step(Kwsnake * kwsnake, float **im_orth, Vec2 * p, Vec2 * v, int n1, int n2, double alpha, double beta, double step, int type)
514 {
515     kwsnake_external_step_orth(kwsnake, im_orth, p, v, n1, n2, step, type);
516     kwsnake_internal_step(kwsnake, alpha, beta);
517 }
518 
519 void    kwsnake_step_orth(Kwsnake * kwsnake, float **im_orth, Vec2 * p, Vec2 * v, int n1, int n2, double alpha, double beta, double step, int type)
520 {
521     kwsnake_external_step_orth(kwsnake, im_orth, p, v, n1, n2, step, type);
522     kwsnake_internal_step(kwsnake, alpha, beta);
523     kwsnake_reorth(kwsnake, p, v);
524 }
525 
526 void    kwsnake_inflate(Kwsnake * kwsnake, Vec2 * p, Vec2 * v, double step)
527 {
528     double *x, *y;
529     int     i, n;
530 
531     if (kwsnake == NULL)
532         return;
533 
534     x = kwsnake->x;
535     y = kwsnake->y;
536     n = kwsnake->n;
537 
538     for (i = 0; i < n; ++i)
539     {
540         Vec2    pi = {Vec2_id};
541 
542         pi = vec2_sum(p[i], vec2_times(step, v[i]));
543         x[i] = vec2_x(pi);
544         y[i] = vec2_y(pi);
545     }
546 }
547 
548 void    kwsnake_grow(Kwsnake * kwsnake, Vec2 * p, Vec2 * v, Imrect * im, double mean, double sd, double step)
549 {
550     double *x, *y;
551     double  gl, gl_low, gl_up;
552     int     i, n;
553 
554     if (kwsnake == NULL)
555         return;
556 
557     x = kwsnake->x;
558     y = kwsnake->y;
559     n = kwsnake->n;
560 
561     gl_low = mean - 2 * sd;
562     gl_up = mean + 2 * sd;
563 
564     for (i = 0; i < n; ++i)
565     {
566         Vec2    pi = {Vec2_id};
567 
568         gl = im_sub_pixf(im, y[i], x[i]);
569         if (gl < gl_low || gl > gl_up)
570         {
571             pi = vec2_sum(p[i], vec2_times(-step, v[i]));
572             x[i] = vec2_x(pi);
573             y[i] = vec2_y(pi);
574             continue;
575         }
576         pi = vec2_sum(p[i], vec2_times(step, v[i]));
577         gl = im_sub_pixf(im, vec2_y(pi), vec2_x(pi));
578         if (gl > gl_low && gl < gl_up)
579         {
580             x[i] = vec2_x(pi);
581             y[i] = vec2_y(pi);
582         }
583     }
584 }
585 
586 /* ARGSUSED quieten lint */
587 void    kwsnake_region(Kwsnake * kwsnake, Imrect * im, double mean, double sd, Vec2 * p, Vec2 * v, double alpha, double beta, double step, int type)
588 
589 
590 
591 
592 
593 /* unused */
594 {
595     kwsnake_grow(kwsnake, p, v, im, mean, sd, step);
596     kwsnake_internal_step(kwsnake, alpha, beta);
597 }
598 

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