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

# Linux Cross ReferenceTina4/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 ] ~