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

Linux Cross Reference
Tina4/src/vision/conic/con_util.c

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

  1 /**@(#)
  2 **/
  3 /**
  4 * con_util.c
  5 * utility functions for conics
  6 *
  7 **/
  8 
  9 #include <math.h>
 10 #include <tina/sys.h>
 11 #include <tina/sysfuncs.h>
 12 #include <tina/math.h>
 13 #include <tina/mathfuncs.h>
 14 #include <tina/vision.h>
 15 #include <tina/visionfuncs.h>
 16 
 17 /**
 18 scale coefficients so  a+c=1
 19 **/
 20 void    conic_normalise(Conic * conic)
 21 {
 22     double  k;
 23 
 24     if (conic == NULL)
 25         return;
 26 
 27     k = 1.0 / (conic->a + conic->c);
 28 
 29     conic->a *= k;
 30     conic->b *= k;
 31     conic->c *= k;
 32     conic->d *= k;
 33     conic->e *= k;
 34     conic->f *= k;
 35 }
 36 
 37 /**
 38 
 39 evaluate conic quadratic form at point (x y)
 40 **/
 41 double  conic_eval(Conic * conic, Vec2 p)
 42 {
 43     double  a = conic->a, b = conic->b, c = conic->c;
 44     double  d = conic->d, e = conic->e, f = conic->f;
 45     double  x = vec2_x(p), y = vec2_y(p);
 46 
 47     return (a * x * x + 2.0 * b * x * y + c * y * y + 2.0 * d * x + 2.0 * e * y + f);
 48 }
 49 
 50 /**
 51 
 52 evaluate conic gradient vector at point (x y)
 53 **/
 54 Vec2    conic_grad(Conic * conic, Vec2 p)
 55 {
 56     double  x = vec2_x(p), y = vec2_y(p);
 57 
 58     return (vec2(2.0 * (conic->a * x + conic->b * y + conic->d),
 59                  2.0 * (conic->b * x + conic->c * y + conic->e)));
 60 }
 61 
 62 /**
 63 discriminant of conic - positive for ellipse, negative for hyperbola
 64 **/
 65 double  conic_discrim(Conic * conic)
 66 {
 67     double  a = conic->a, b = conic->b, c = conic->c;
 68 
 69     return (a * c - b * b);
 70 }
 71 
 72 /**
 73 eigendirection and eigenvalues of symmetric 2by2 matrix
 74 |a b|
 75 |b c|
 76 **/
 77 void    conic_eigen(double a, double b, double c, double *theta, double *lambda1, double *lambda2)
 78 {
 79     double  p = (a + c) / 2.0, q = (a - c) / 2.0, r;
 80 
 81     r = sqrt(q * q + b * b);
 82     if (b == 0.0 && q == 0.0)
 83         *theta = 0.0;
 84     else
 85         *theta = 0.5 * atan2(b, q);
 86     *lambda1 = p + r;
 87     *lambda2 = p - r;
 88 }
 89 
 90 /**
 91 
 92 value of  lambda  such that  lambda*C0+(1-lambda)*C1 = 0  at  (x y)
 93 **/
 94 double  conic_lambda(Conic * conic0, Conic * conic1, Vec2 p)
 95 {
 96     double  c0 = conic_eval(conic0, p);
 97     double  c1 = conic_eval(conic1, p);
 98 
 99     return (c1 / (c1 - c0));
100 }
101 
102 /**
103 point on ellipse with angular parameter t
104 **/
105 Vec2    ellipse_point(Conic * conic, double t)
106 {
107     double  c, s, x, y;
108 
109     c = cos(conic->theta);
110     s = sin(conic->theta);
111     x = conic->alpha * cos(t);
112     y = conic->beta * sin(t);
113     return (vec2_sum(conic->center, vec2(x * c - y * s, x * s + y * c)));
114 }
115 
116 /**
117 point on hyperbola with angular parameter t
118 **/
119 Vec2    hyperbola_point(Conic * conic, double t)
120 {
121     double  c, s, x, y;
122 
123     c = cos(conic->theta);
124     s = sin(conic->theta);
125     x = conic->branch * conic->alpha * cosh(t);
126     y = conic->beta * sinh(t);
127     return (vec2_sum(conic->center, vec2(x * c - y * s, x * s + y * c)));
128 }
129 
130 /**
131 angular parameter of point near ellipse
132 
133 return values in [0, twopi)
134 **/
135 double  ellipse_param(Conic * conic, Vec2 p)
136 {
137     double  c, s, x, y;
138     double  t;
139 
140     p = vec2_diff(p, conic->center);
141     c = cos(conic->theta);
142     s = sin(conic->theta);
143     x = vec2_x(p) * c + vec2_y(p) * s;
144     y = -vec2_x(p) * s + vec2_y(p) * c;
145 
146     t = atan2(conic->alpha * y, conic->beta * x);
147     if (t < 0.0)
148         t += TWOPI;
149     return (t);
150 }
151 
152 /**
153 angular parameter of point near hyperbola
154 **/
155 double  hyperbola_param(Conic * conic, Vec2 p)
156 {
157     double  c, s;
158 
159 
160     p = vec2_diff(p, conic->center);
161     c = cos(conic->theta);
162     s = sin(conic->theta);
163     return (asinh((-vec2_x(p) * s + vec2_y(p) * c) / conic->beta));
164 }
165 
166 int     hyperbola_branch(Conic * conic, Vec2 p)
167 {
168     double  c, s;
169 
170     p = vec2_diff(p, conic->center);
171     c = cos(conic->theta);
172     s = sin(conic->theta);
173     return (((vec2_x(p) * c + vec2_y(p) * s) > 0.0) ? 1 : -1);
174 }
175 
176 /**
177 approximate squared distance of point from conic
178 **/
179 double  conic_approx_sqrdist(Conic * conic, Vec2 p)
180 {
181     double  a = conic->a, b = conic->b, c = conic->c;
182     double  d = conic->d, e = conic->e, f = conic->f;
183     double  q0, qx, qy;
184     double  x = vec2_x(p), y = vec2_y(p);
185 
186     q0 = a * x * x + 2.0 * b * x * y + c * y * y + 2.0 * d * x + 2.0 * e * y + f;
187     qx = 2.0 * (a * x + b * y + d);
188     qy = 2.0 * (b * x + c * y + e);
189     return (q0 * q0 / (qx * qx + qy * qy));
190 }
191 
192 /**
193 set type, orientation and semiaxes of conic and return center
194 **/
195 void    conic_setup(Conic * conic)
196 {
197     double  a, b, c, d, e;
198     double  lambda1, lambda2;
199     double  q0, disc;
200 
201     if (conic == NULL)
202         return;
203 
204     a = conic->a;
205     b = conic->b;
206     c = conic->c;
207     d = conic->d;
208     e = conic->e;
209 
210     disc = conic_discrim(conic);
211     if (disc > 0.0)
212     {
213         conic->type = ELLIPSE;
214         conic->t1 = 0.0;
215         conic->t2 = TWOPI;
216     } else if (disc < 0.0)
217     {
218         conic->type = HYPERBOLA;
219         conic->t1 = -2.0;
220         conic->t2 = 2.0;
221         conic->branch = 1;
222     } else
223     {
224         conic->type = DEGENERATE;
225         return;
226     }
227     conic->center = vec2(-(c * d - b * e) / disc, -(a * e - b * d) / disc);
228 
229     q0 = conic_eval(conic, conic->center);
230     conic_eigen(a, b, c, &conic->theta, &lambda1, &lambda2);
231     if (lambda1 == 0.0 || lambda2 == 0.0 || q0 == 0.0)
232     {
233         conic->type = DEGENERATE;
234         return;
235     }
236     lambda1 /= -q0;
237     lambda2 /= -q0;
238     conic->alpha = 1.0 / sqrt(fabs(lambda1));
239     conic->beta = 1.0 / sqrt(fabs(lambda2));
240 }
241 
242 Vec2    conic_point(Conic * conic, double t)
243 {
244     Vec2    vec_2 = {Vec2_id};
245 
246     switch (conic->type)
247     {
248     case ELLIPSE:
249         vec_2 = (ellipse_point(conic, t));
250         break;
251     case HYPERBOLA:
252         vec_2 = (hyperbola_point(conic, t));
253         break;
254     case DEGENERATE:
255     default:
256         vec_2 = (vec2_zero());
257     }
258     return vec_2;
259 }
260 
261 double  conic_param(Conic * conic, Vec2 p)
262 {
263     switch (conic->type)
264     {
265         case ELLIPSE:
266         return (ellipse_param(conic, p));
267     case HYPERBOLA:
268         return (hyperbola_param(conic, p));
269     case DEGENERATE:
270     default:
271         return (0.0);
272     }
273 }
274 
275 static int ordered(double t1, double t2, double t3)
276 {
277     int     o = ((t1 < t2) && (t2 < t3)) ||
278     ((t2 < t3) && (t3 < t1)) ||
279     ((t3 < t1) && (t1 < t2));
280 
281     return (o);
282 }
283 
284 void    conic_set_ends(Conic * conic, Vec2 p1, Vec2 p2, Vec2 pmid)
285 
286 /* set end points to be from p1 to p2 */
287 /* another point defining the sence of the curve */
288 {
289     double  t1, t2, tmid;
290 
291     if (conic == NULL || conic->type == DEGENERATE)
292         return;
293 
294     t1 = conic_param(conic, p1);
295     t2 = conic_param(conic, p2);
296 
297     if (conic->type == HYPERBOLA)
298     {
299         conic->branch = hyperbola_branch(conic, p1);
300         conic->t1 = t1;
301         conic->t2 = t2;
302         return;
303     }
304     tmid = conic_param(conic, pmid);
305 
306     if (ordered(t1, tmid, t2))
307     {
308         if (t1 > t2)
309             t2 += TWOPI;
310         conic->t1 = t1;
311         conic->t2 = t2;
312     } else
313     {
314         if (t2 >= t1)
315             t1 += TWOPI;
316         conic->t1 = t2;
317         conic->t2 = t1;
318     }
319 }
320 
321 double  conic_approx_length(Conic * conic, int n)
322 {
323     int     i;
324     Vec2    p = {Vec2_id};
325     Vec2    q = {Vec2_id};
326     double  dt = (conic->t2 - conic->t1) / n, len = 0.0;
327 
328     p = conic_point(conic, conic->t1);
329     for (i = 1; i <= n; i++)
330     {
331         q = conic_point(conic, conic->t1 + i * dt);
332         len += vec2_dist(p, q);
333         p = q;
334     }
335     return (len);
336 }
337 
338 double  conic_param_length(Conic * conic, Vec2 p1, Vec2 p2, Vec2 pmid)
339 {
340     double  t1, t2, tmid;
341 
342     if (conic == NULL || conic->type == DEGENERATE)
343         return (0.0);
344 
345     t1 = conic_param(conic, p1);
346     t2 = conic_param(conic, p2);
347 
348     if (conic->type == HYPERBOLA)
349         return (fabs(t2 - t1));
350 
351     tmid = conic_param(conic, pmid);
352     if (ordered(t1, tmid, t2))
353     {
354         if (t1 > t2)
355             t2 += TWOPI;
356     } else
357     {
358         if (t2 > t1)
359             t1 += TWOPI;
360     }
361     return (fabs(t2 - t1));
362 }
363 
364 
365 void    ellipse_format(Conic * conic)
366 {
367     Vec2    center = {Vec2_id};
368 
369     if (conic == NULL)
370         return;
371 
372     center = conic->center;
373     format("ellipse  : %15d\n", conic->label);
374     format("center   : %15.6f %15.6f\n", vec2_x(center), vec2_y(center));
375     format("theta    : %15.6f\n", conic->theta);
376     format("alpha    : %15.6f\n", conic->alpha);
377     format("beta     : %15.6f\n", conic->beta);
378 }
379 
380 void    hyperbola_format(Conic * conic)
381 {
382     Vec2    center = {Vec2_id};
383 
384     if (conic == NULL)
385         return;
386 
387     center = conic->center;
388     format("hyperbola: %15d\n", conic->label);
389     format("branch   : %3d\n", conic->branch);
390     format("center   : %15.6f %15.6f\n", vec2_x(center), vec2_y(center));
391     format("theta    : %15.6f\n", conic->theta);
392     format("alpha    : %15.6f\n", conic->alpha);
393     format("beta     : %15.6f\n", conic->beta);
394 }
395 
396 void    conic_format(Conic * conic)
397 {
398     Vec2    p = {Vec2_id};
399 
400     if (conic == NULL)
401         return;
402     switch (conic->type)
403     {
404     case ELLIPSE:
405         ellipse_format(conic);
406         break;
407     case HYPERBOLA:
408         hyperbola_format(conic);
409         break;
410     case DEGENERATE:
411     default:
412         return;
413     }
414     p = conic_point(conic, conic->t1);
415     format("p1       : %15.6f %15.6f\n", vec2_x(p), vec2_y(p));
416     p = conic_point(conic, conic->t2);
417     format("p2       : %15.6f %15.6f\n", vec2_x(p), vec2_y(p));
418     format("\n");
419 }
420 
421 /** ratio of minor to major axes (< 1.0) **/
422 double  conic_aratio(Conic * conic)
423 {
424     double  aratio;
425 
426     aratio = conic->alpha / conic->beta;
427     aratio = MIN(aratio, 1.0 / aratio);
428     return (aratio);
429 }
430 

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