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

Linux Cross Reference
Tina4/src/vision/curve3/conic3.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 #ifdef _PCC
 11 #include <memory.h>
 12 #endif
 13 
 14 Conic3 *conic3_alloc(unsigned int type)
 15 {
 16     Conic3 *con3 = ts_ralloc(Conic3);
 17 
 18     con3->type = type;
 19     return (con3);
 20 }
 21 
 22 Conic3 *conic3_copy(Conic3 * con3)
 23 {
 24     Conic3 *copy;
 25 
 26     if (con3 == NULL)
 27         return (NULL);
 28 
 29     copy = ts_ralloc(Conic3);
 30     (void) memcpy((char *) copy, (char *) con3, sizeof(Conic3));
 31     copy->ts_id = Conic3_id;    /* ANITISE */
 32     copy->conic = conic_copy(con3->conic);
 33     return (copy);
 34 }
 35 
 36 void    conic3_free(Conic3 * con3)
 37 {
 38     if (con3 == NULL)
 39         return;
 40     conic_free(con3->conic);
 41     rfree((void *) con3);
 42 }
 43 
 44 Conic3 *conic3_make(Conic * con2, Vec3 o, Vec3 ex, Vec3 ey, Vec3 ez)
 45 {
 46     Conic3 *con3;
 47 
 48     if (con2 == NULL)
 49         return (NULL);
 50 
 51     con3 = conic3_alloc(con2->type);
 52     con3->conic = con2;
 53     con3->origin = o;
 54     con3->ex = ex;
 55     con3->ey = ey;
 56     con3->ez = ez;
 57     return (con3);
 58 }
 59 
 60 void    conic3_transform(Conic3 * conic, Transform3 trans)
 61 {
 62     if (conic == NULL)
 63         return;
 64 
 65     conic->origin = trans3_pos(trans, conic->origin);
 66     conic->ex = trans3_vec(trans, conic->ex);
 67     conic->ey = trans3_vec(trans, conic->ey);
 68     conic->ez = trans3_vec(trans, conic->ez);
 69 }
 70 
 71 /* find point in 3 space correspondint to param */
 72 
 73 Vec3    conic3_point(Conic3 * con3, double t)
 74 {
 75     Vec2    p2 = {Vec2_id};
 76     Vec3    p3 = {Vec3_id};
 77 
 78     p2 = conic_point(con3->conic, t);   /* point in plane */
 79     p3 = vec3_sum(con3->origin, vec3_times(vec2_x(p2), con3->ex));
 80     return (vec3_sum(p3, vec3_times(vec2_y(p2), con3->ey)));
 81 }
 82 
 83 /* find approx parameter of point in 3 space  */
 84 
 85 double  conic3_param(Conic3 * con3, Vec3 p3)
 86 {
 87     Vec2    p2 = {Vec2_id};     /* point in 2D conic coords */
 88 
 89     p3 = vec3_diff(p3, con3->origin);
 90     p2 = vec2(vec3_dot(p3, con3->ex), vec3_dot(p3, con3->ey));
 91 
 92     return (conic_param(con3->conic, p2));
 93 }
 94 
 95 /* Find the parameter offset of c2 with respect to c1.
 96  * 
 97  * Find the vector from parameter point zero on c1 to its origin. Add this
 98  * to the origin on c2 and find the approximate parameter value. */
 99 double  conic3_parameter_offset(Conic3 * c1, Conic3 * c2)
100 {
101     Vec3    p = {Vec3_id};
102 
103     p = vec3_diff(conic3_point(c1, 0.0), c1->origin);
104     return (conic3_param(c2, vec3_sum(c2->origin, p)));
105 }
106 
107 Bool    conic3_overlap(Conic3 * c1, Conic3 * c2, float *t1, float *t2)
108 
109 /* overlap in coords of c1 */
110 {
111     double  t11, t12, t21, t22; /* all params in coords of c1 */
112     Vec3    p = {Vec3_id};
113 
114     t11 = c1->conic->t1;
115     t12 = c1->conic->t2;
116     p = conic3_point(c2, c2->conic->t1);
117     t21 = conic3_param(c1, p);
118     t22 = t21 + c2->conic->t2 - c2->conic->t1;
119 
120     if (t12 < t21)              /* possible non overlap */
121     {
122         if (t22 < TWOPI || t11 + TWOPI > t22)
123             return (false);
124         t11 += TWOPI;
125         t12 += TWOPI;
126     } else if (t11 > t22)       /* possible non overlap */
127     {
128         if (t12 < TWOPI || t21 + TWOPI > t12)
129             return (false);
130         t21 += TWOPI;
131         t22 += TWOPI;
132     }
133     *t1 = (float)MAX(t11, t21);
134     *t2 = (float)MIN(t12, t22);
135 
136     if (*t1 > TWOPI)
137     {
138         *t1 -= (float)TWOPI;
139         *t2 -= (float)TWOPI;
140     }
141     return (true);
142 }
143 
144 Bool    conic3_coincident(Conic3 * c1, Conic3 * c2, double doterror, double poserror)
145 
146 /* BUG */
147 
148 {
149     float   t1, t2, t, i;       /* overlap in coords of c1 */
150     Vec3    p1 = {Vec3_id};
151     Vec3    p2 = {Vec3_id};
152 
153     if (c1 == NULL || c2 == NULL)
154         return (false);
155 
156     if (vec3_dot(c1->ez, c2->ez) < 0.0) /* oposite sign */
157         return (false);
158 
159     /* if (vec3_dist(c1->origin, c2->origin) > poserror) return
160      * (false); */
161 
162     if (conic3_overlap(c1, c2, &t1, &t2) == false)
163         return (false);
164 
165     for (i = (float)0.25; i < 0.8; i += (float)0.25)/* i = {0.27, 0.5, 0.75} */
166     {
167         t = (t1 + t2) * i;
168         p1 = conic3_point(c1, t);
169         t = (float)conic3_param(c2, p1);
170         p2 = conic3_point(c2, t);
171         if (vec3_dist(p1, p2) > poserror)
172             return (false);
173     }
174 
175     return (true);
176 }
177 
178 Bool    conic3_within_error(Conic3 * c1, Conic3 * c2)
179 {
180     Iso_error *iso1;
181     Iso_error *iso2;
182 
183     if (c1 == NULL || c2 == NULL)
184         return (false);
185 
186     iso1 = (Iso_error *) prop_get(c1->conic->props, ISO_ERROR);
187     iso2 = (Iso_error *) prop_get(c2->conic->props, ISO_ERROR);
188 
189     if (iso1 == NULL || iso2 == NULL)
190         return (false);
191 
192     return (conic3_coincident(c1, c2, iso1->dot * iso2->dot, iso1->pos + iso2->pos));
193 }
194 
195 void    conic3_negate(Conic3 * con3)
196 {
197     double  t1, t2;
198     Conic  *conic;
199 
200     if (con3 == NULL)
201         return;
202 
203     conic = con3->conic;
204     con3->ez = vec3_minus(con3->ez);
205     con3->ey = vec3_minus(con3->ey);
206     conic->theta = -conic->theta;
207 
208     t1 = TWOPI - conic->t2;
209     if (t1 < 0)
210         t1 += TWOPI;
211 
212     t2 = t1 + conic->t2 - conic->t1;
213 
214     conic->t1 = t1;
215     conic->t2 = t2;
216 }
217 
218 Conic3 *conic3_negative(Conic3 * con3)
219 {
220     con3 = conic3_copy(con3);
221     conic3_negate(con3);
222     return (con3);
223 }
224 
225 Conic3 *conic_par_proj_to_plane(Conic * conic, Plane * plane)
226 {
227     Vec3    p = {Vec3_id};
228     Vec3    n = {Vec3_id};
229     Vec3    c = {Vec3_id};
230     Vec3    ex = {Vec3_id};
231     Vec3    ey = {Vec3_id};
232     Vec2    p2 = {Vec2_id};
233     Vec2    p_array[5] = {{Vec2_id}, {Vec2_id}, {Vec2_id}, {Vec2_id}, {Vec2_id}};
234     Vec3    p3 = {Vec3_id};
235     Vec3    v = {Vec3_id};
236     Vec3    o = {Vec3_id};
237     double  t, t1, t2, step = TWOPI / 6;
238     Conic  *conic_new;
239     int     i;
240 
241     if (conic == NULL || plane == NULL)
242         return (NULL);
243 
244     p = plane->p;
245     n = plane->n;
246 
247     t1 = conic->t1;
248     t2 = conic->t2;
249 
250     par_proj_ray(conic->center, &o, &v);
251     c = vec3_inter_line_plane(o, v, p, n);
252     p2 = vec2_sum(conic->center, vec2(cos(conic->theta), sin(conic->theta)));
253     par_proj_ray(p2, &o, &v);
254     p3 = vec3_inter_line_plane(o, v, p, n);
255     ex = vec3_unit(vec3_diff(p3, c));
256     ey = vec3_cross(n, ex);
257 
258     for (i = 0, t = 0; i < 5; ++i, t += step)
259     {
260         p2 = conic_point(conic, t);
261         par_proj_ray(p2, &o, &v);
262         p3 = vec3_inter_line_plane(o, v, p, n);
263         p3 = vec3_diff(p3, c);
264         vec2_x(p2) = (float)vec3_dot(p3, ex);
265         vec2_y(p2) = (float)vec3_dot(p3, ey);
266         p_array[i] = p2;
267     }
268 
269     conic_new = conic_5pt(p_array[0], p_array[1], p_array[2], p_array[3], p_array[4]);
270 
271     step = (t2 - t1) / 2.0;
272 
273     for (i = 0, t = t1; i < 3; ++i, t += step)
274     {
275         p2 = conic_point(conic, t);
276         par_proj_ray(p2, &o, &v);
277         p3 = vec3_inter_line_plane(o, v, p, n);
278         p3 = vec3_diff(p3, c);
279         vec2_x(p2) = (float)vec3_dot(p3, ex);
280         vec2_y(p2) = (float)vec3_dot(p3, ey);
281         p_array[i] = p2;
282     }
283 
284     conic_set_ends(conic_new, p_array[0], p_array[2], p_array[1]);
285     return (conic3_make(conic_new, c, ex, ey, n));
286 }
287 
288 void    conic3_shift_origin_to_center(Conic3 * con3)
289 {
290     Conic  *conic;
291     Vec3    shift = {Vec3_id};
292 
293     if (con3 == NULL || con3->conic == NULL)
294         return;
295 
296     conic = con3->conic;
297     shift = vec3_times(vec2_x(conic->center), con3->ex);
298     shift = vec3_sum(shift, vec3_times(vec2_y(conic->center), con3->ey));
299     con3->origin = vec3_sum(con3->origin, shift);
300     conic->center = vec2_zero();
301 }
302 
303 Conic3 *conic3_from_conic2(Conic * conic, double fit_thres)
304 {
305     Tstring *curve, *str;
306     Conic   copy = {Conic_id};
307     Conic3 *con3;
308     Plane  *plane;
309     Vec2    p1 = {Vec2_id};
310     Vec2    p2 = {Vec2_id};
311     Vec2    pm = {Vec2_id};
312 
313     str = (Tstring *) prop_get(conic->props, STRING);
314     if (str == NULL)
315         return (NULL);
316     curve = conic2_string(conic);
317     plane = plane_curve_ls(curve, str, fit_thres, &p1, &p2, &pm);
318     str_rm(curve, rfree);
319     copy = *conic;
320     conic_set_ends(&copy, p1, p2, pm);
321     con3 = conic_par_proj_to_plane(&copy, plane);
322     conic3_shift_origin_to_center(con3);
323     return (con3);
324 }
325 
326 void    conic3_format(Conic3 * conic)
327 {
328     Vec3    p = {Vec3_id};
329 
330     if (conic == NULL)
331         return;
332 
333     format("conic3   :\n");
334     p = conic->origin;
335     format("origin   : %15.6f %15.6f %15.6f\n", vec3_x(p), vec3_y(p), vec3_z(p));
336     p = conic->ex;
337     format("x axis   : %15.6f %15.6f %15.6f\n", vec3_x(p), vec3_y(p), vec3_z(p));
338     p = conic->ey;
339     format("y axis   : %15.6f %15.6f %15.6f\n", vec3_x(p), vec3_y(p), vec3_z(p));
340     p = conic->ez;
341     format("z axis   : %15.6f %15.6f %15.6f\n", vec3_x(p), vec3_y(p), vec3_z(p));
342     conic_format(conic->conic);
343     p = conic3_point(conic, conic->conic->t1);
344     format("p1       : %15.6f %15.6f %15.6f\n", vec3_x(p), vec3_y(p), vec3_z(p));
345     p = conic3_point(conic, conic->conic->t2);
346     format("p2       : %15.6f %15.6f %15.6f\n", vec3_x(p), vec3_y(p), vec3_z(p));
347     format("\n");
348 }
349 

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