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

Linux Cross Reference
Tina5/tina-libs/tina/geometry/geomCurve_conic3.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 Lesser 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 Lesser General Public License for more details.
 14  *
 15  * You should have received a copy of the GNU Lesser 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  **********
 20  * 
 21  * Program :    TINA
 22  * File    :  $Source: /home/tina/cvs/tina-libs/tina/geometry/geomCurve_conic3.c,v $
 23  * Date    :  $Date: 2008/12/16 11:34:54 $
 24  * Version :  $Revision: 1.5 $
 25  * CVS Id  :  $Id: geomCurve_conic3.c,v 1.5 2008/12/16 11:34:54 neil Exp $
 26  *
 27  * Author  : Legacy TINA
 28  *
 29  * Notes :
 30  *
 31  *********
 32 */
 33 
 34 #include "geomCurve_conic3.h"
 35 
 36 #if HAVE_CONFIG_H
 37   #include <config.h>
 38 #endif
 39 
 40 #include <math.h>
 41 #include <string.h>
 42 #include <tina/sys/sysDef.h>
 43 #include <tina/sys/sysPro.h>
 44 #include <tina/math/mathDef.h>
 45 #include <tina/math/mathPro.h>
 46 #include <tina/geometry/geomDef.h>
 47 #include <tina/geometry/geom_CamPro.h>
 48 #include <tina/geometry/geom_CamDef.h>
 49 #include <tina/geometry/geom_PlanePro.h>
 50 #include <tina/geometry/geom_PlaneDef.h>
 51 #include <tina/geometry/geom_CurveDef.h>
 52 #include <tina/geometry/geom_TransDef.h>
 53 #include <tina/geometry/geomCurve_conic.h>
 54 #include <tina/geometry/geomCurve_conic2.h>
 55 #include <tina/geometry/geomCurve_con_util.h>
 56 #include <tina/geometry/geomCurve_conic_5pt.h>
 57 #include <tina/geometry/geomCurve_affine_cv.h>
 58 #include <tina/geometry/geomCurve_es_string3.h>
 59 
 60 #ifdef _PCC
 61 #include <memory.h>
 62 #endif
 63 
 64 #define HORIZONTAL_THRES 4
 65 #define EXTRADATA 2001
 66 
 67 Conic3 *conic3_alloc(unsigned int type)
 68 {
 69     Conic3 *con3 = ts_ralloc(Conic3);
 70 
 71     con3->type = type;
 72     return (con3);
 73 }
 74 
 75 Conic3 *conic3_copy(Conic3 * con3)
 76 {
 77     Conic3 *copy;
 78 
 79     if (con3 == NULL)
 80         return (NULL);
 81 
 82     copy = ts_ralloc(Conic3);
 83     (void) memcpy((char *) copy, (char *) con3, sizeof(Conic3));
 84     copy->ts_id = Conic3_id;    /* ANITISE */
 85     copy->conic = conic_copy(con3->conic);
 86     return (copy);
 87 }
 88 
 89 void    conic3_free(Conic3 * con3)
 90 {
 91     if (con3 == NULL)
 92         return;
 93     conic_free(con3->conic);
 94     rfree((void *) con3);
 95 }
 96 
 97 Conic3 *conic3_make(Conic * con2, Vec3 o, Vec3 ex, Vec3 ey, Vec3 ez)
 98 {
 99     Conic3 *con3;
100 
101     if (con2 == NULL)
102         return (NULL);
103 
104     con3 = conic3_alloc(con2->type);
105     con3->conic = con2;
106     con3->origin = o;
107     con3->ex = ex;
108     con3->ey = ey;
109     con3->ez = ez;
110     return (con3);
111 }
112 
113 void    conic3_transform(Conic3 * conic, Transform3 trans)
114 {
115     if (conic == NULL)
116         return;
117 
118     conic->origin = trans3_pos(trans, conic->origin);
119     conic->ex = trans3_vec(trans, conic->ex);
120     conic->ey = trans3_vec(trans, conic->ey);
121     conic->ez = trans3_vec(trans, conic->ez);
122 }
123 
124 /* find point in 3 space correspondint to param */
125 
126 Vec3    conic3_point(Conic3 * con3, double t)
127 {
128     Vec2    p2 = {Vec2_id};
129     Vec3    p3 = {Vec3_id};
130 
131     p2 = conic_point(con3->conic, t);   /* point in plane */
132     p3 = vec3_sum(con3->origin, vec3_times(vec2_x(p2), con3->ex));
133     return (vec3_sum(p3, vec3_times(vec2_y(p2), con3->ey)));
134 }
135 
136 /* find approx parameter of point in 3 space  */
137 
138 double  conic3_param(Conic3 * con3, Vec3 p3)
139 {
140     Vec2    p2 = {Vec2_id};     /* point in 2D conic coords */
141 
142     p3 = vec3_diff(p3, con3->origin);
143     p2 = vec2(vec3_dot(p3, con3->ex), vec3_dot(p3, con3->ey));
144 
145     return (conic_param(con3->conic, p2));
146 }
147 
148 /* Find the parameter offset of c2 with respect to c1.
149  * 
150  * Find the vector from parameter point zero on c1 to its origin. Add this
151  * to the origin on c2 and find the approximate parameter value. */
152 double  conic3_parameter_offset(Conic3 * c1, Conic3 * c2)
153 {
154     Vec3    p = {Vec3_id};
155 
156     p = vec3_diff(conic3_point(c1, 0.0), c1->origin);
157     return (conic3_param(c2, vec3_sum(c2->origin, p)));
158 }
159 
160 Bool    conic3_overlap(Conic3 * c1, Conic3 * c2, float *t1, float *t2)
161 
162 /* overlap in coords of c1 */
163 {
164     double  t11, t12, t21, t22; /* all params in coords of c1 */
165     Vec3    p = {Vec3_id};
166 
167     t11 = c1->conic->t1;
168     t12 = c1->conic->t2;
169     p = conic3_point(c2, c2->conic->t1);
170     t21 = conic3_param(c1, p);
171     t22 = t21 + c2->conic->t2 - c2->conic->t1;
172 
173     if (t12 < t21)              /* possible non overlap */
174     {
175         if (t22 < TWOPI || t11 + TWOPI > t22)
176             return (false);
177         t11 += TWOPI;
178         t12 += TWOPI;
179     } else if (t11 > t22)       /* possible non overlap */
180     {
181         if (t12 < TWOPI || t21 + TWOPI > t12)
182             return (false);
183         t21 += TWOPI;
184         t22 += TWOPI;
185     }
186     *t1 = (float)MAX(t11, t21);
187     *t2 = (float)MIN(t12, t22);
188 
189     if (*t1 > TWOPI)
190     {
191         *t1 -= (float)TWOPI;
192         *t2 -= (float)TWOPI;
193     }
194     return (true);
195 }
196 
197 Bool    conic3_coincident(Conic3 * c1, Conic3 * c2, double doterror, double poserror)
198 
199 /* BUG */
200 
201 {
202     float   t1, t2, t, i;       /* overlap in coords of c1 */
203     Vec3    p1 = {Vec3_id};
204     Vec3    p2 = {Vec3_id};
205 
206     if (c1 == NULL || c2 == NULL)
207         return (false);
208 
209     if (vec3_dot(c1->ez, c2->ez) < 0.0) /* oposite sign */
210         return (false);
211 
212     /* if (vec3_dist(c1->origin, c2->origin) > poserror) return
213      * (false); */
214 
215     if (conic3_overlap(c1, c2, &t1, &t2) == false)
216         return (false);
217 
218     for (i = (float)0.25; i < 0.8; i += (float)0.25)/* i = {0.27, 0.5, 0.75} */
219     {
220         t = (t1 + t2) * i;
221         p1 = conic3_point(c1, t);
222         t = (float)conic3_param(c2, p1);
223         p2 = conic3_point(c2, t);
224         if (vec3_dist(p1, p2) > poserror)
225             return (false);
226     }
227 
228     return (true);
229 }
230 
231 Bool    conic3_within_error(Conic3 * c1, Conic3 * c2)
232 {
233     Iso_error *iso1;
234     Iso_error *iso2;
235 
236     if (c1 == NULL || c2 == NULL)
237         return (false);
238 
239     iso1 = (Iso_error *) prop_get(c1->conic->props, ISO_ERROR);
240     iso2 = (Iso_error *) prop_get(c2->conic->props, ISO_ERROR);
241 
242     if (iso1 == NULL || iso2 == NULL)
243         return (false);
244 
245     return (conic3_coincident(c1, c2, iso1->dot * iso2->dot, iso1->pos + iso2->pos));
246 }
247 
248 void    conic3_negate(Conic3 * con3)
249 {
250     double  t1, t2;
251     Conic  *conic;
252 
253     if (con3 == NULL)
254         return;
255 
256     conic = con3->conic;
257     con3->ez = vec3_minus(con3->ez);
258     con3->ey = vec3_minus(con3->ey);
259     conic->theta = -conic->theta;
260 
261     t1 = TWOPI - conic->t2;
262     if (t1 < 0)
263         t1 += TWOPI;
264 
265     t2 = t1 + conic->t2 - conic->t1;
266 
267     conic->t1 = t1;
268     conic->t2 = t2;
269 }
270 
271 Conic3 *conic3_negative(Conic3 * con3)
272 {
273     con3 = conic3_copy(con3);
274     conic3_negate(con3);
275     return (con3);
276 }
277 
278 /* test for poorly oriented and badly approximated data  NAT 25/10/08 */
279 static int test_geom3(Vec3 p1, Vec3 p2, Vec3 p3, Vec3 t1, Vec3 t2, Vec3 t3)
280 {
281     float   z , xy;
282     float  length;
283 
284 
285     z = fabs(vec3_z(p2)-vec3_z(p1)) + fabs(vec3_z(p3)-vec3_z(p2));
286     xy = fabs(vec3_x(p2)-vec3_x(p1))+ fabs(vec3_y(p2)-vec3_y(p1))
287        + fabs(vec3_x(p3)-vec3_x(p2))+ fabs(vec3_y(p3)-vec3_y(p2));
288 
289     if (fabs(z / xy) > 2.5) /* unsuitable geometry */
290        return(0);
291 
292     length = vec3_mod(vec3_diff(p1,p2));
293     if ( (vec3_mod(vec3_diff(p1,t1))/length > 0.5 && vec3_mod(vec3_diff(p1,t3))/length > 0.5)
294       || (vec3_mod(vec3_diff(p3,t1))/length > 0.5 && vec3_mod(vec3_diff(p3,t3))/length > 0.5)
295       || (vec3_mod(vec3_diff(p2,t2))/length > 0.5) ) /* non matching 3D fit */
296        return(0);
297 
298 
299     return(1);
300 }
301 
302 Conic3 *conic_par_proj_to_plane(Conic * conic, Plane * plane)
303 {
304     Vec3    p = {Vec3_id};
305     Vec3    n = {Vec3_id};
306     Vec3    c = {Vec3_id};
307     Vec3    ex = {Vec3_id};
308     Vec3    ey = {Vec3_id};
309     Vec2    p2 = {Vec2_id};
310     Vec2    p_array[5] = {{Vec2_id}, {Vec2_id}, {Vec2_id}, {Vec2_id}, {Vec2_id}};
311     Vec3    p3_array[3] = {{Vec3_id}, {Vec3_id}, {Vec3_id}};
312     Vec3    p3_test[3] = {{Vec3_id}, {Vec3_id}, {Vec3_id}};
313     Vec3    p3 = {Vec3_id};
314     Vec3    v = {Vec3_id};
315     Vec3    o = {Vec3_id};
316     double  t, t1, t2, step = TWOPI / 6;
317     Conic  *conic_new;
318     Conic3 *conic3_new;
319     int     i;
320 
321     if (conic == NULL || plane == NULL)
322         return (NULL);
323 
324     p = plane->p;
325     n = plane->n;
326 
327     t1 = conic->t1;
328     t2 = conic->t2;
329 
330     par_proj_ray(conic->center, &o, &v);
331     c = vec3_inter_line_plane(o, v, p, n);
332 /* possible numerical instability.. boost shift of axis NAT 20/10/08 */
333     p2 = vec2_sum(conic->center, vec2(20.0*cos(conic->theta), 20.0*sin(conic->theta)));
334     par_proj_ray(p2, &o, &v);
335     p3 = vec3_inter_line_plane(o, v, p, n);
336     ex = vec3_unit(vec3_diff(p3, c));
337     ey = vec3_cross(n, ex);
338 
339     if ((t2-t1)< 1.666*PI) step = (t2-t1)/5.0;
340     for (i = 0, t = t1; i < 5; ++i, t += step)
341     {
342         p2 = conic_point(conic, t);
343         par_proj_ray(p2, &o, &v);
344         p3 = vec3_inter_line_plane(o, v, p, n);
345         p3 = vec3_diff(p3, c);
346         vec2_x(p2) = (float)vec3_dot(p3, ex);
347         vec2_y(p2) = (float)vec3_dot(p3, ey);
348         p_array[i] = p2;
349     }
350 
351     conic_new = conic_5pt(p_array[0], p_array[1], p_array[2], p_array[3], p_array[4]);
352 
353 /* store required 3D points for later test NAT 25/10/08 */
354     step = (t2 - t1) / 2.0;
355     for (i = 0, t = t1; i < 3; ++i, t += step)
356     {
357         p2 = conic_point(conic, t);
358         par_proj_ray(p2, &o, &v);
359         p3 = vec3_inter_line_plane(o, v, p, n);
360         p3_test[i] = p3;
361         p3 = vec3_diff(p3, c);
362         vec2_x(p2) = (float)vec3_dot(p3, ex);
363         vec2_y(p2) = (float)vec3_dot(p3, ey);
364         p_array[i] = p2;
365     }
366 
367     conic_set_ends(conic_new, p_array[0], p_array[2], p_array[1]);
368     conic3_new = conic3_make(conic_new, c, ex, ey, n);
369 
370     p3_array[0] = conic3_point(conic3_new, conic3_new->conic->t1);
371     p3_array[1] = conic3_point(conic3_new, (conic3_new->conic->t1+conic3_new->conic->t2)/2.0);
372     p3_array[2] = conic3_point(conic3_new, conic3_new->conic->t2);
373 /* use 3D test to reject poor fits rather than earlier disparity gradient cut. NAT 25/10/08 */
374     if (!test_geom3(p3_array[0], p3_array[1], p3_array[2], p3_test[0], p3_test[1], p3_test[2]))
375     {
376         conic3_free(conic3_new);
377         return(NULL);
378     }
379     else
380         return (conic3_new);
381 }
382 
383 void    conic3_shift_origin_to_center(Conic3 * con3)
384 {
385     Conic  *conic;
386     Vec3    shift = {Vec3_id};
387 
388     if (con3 == NULL || con3->conic == NULL)
389         return;
390 
391     conic = con3->conic;
392     shift = vec3_times(vec2_x(conic->center), con3->ex);
393     shift = vec3_sum(shift, vec3_times(vec2_y(conic->center), con3->ey));
394     con3->origin = vec3_sum(con3->origin, shift);
395     conic->center = vec2_zero();
396 }
397 
398 static double y_range(Tstring * str3)
399 {
400     List *ptr;
401     double  ly, uy;
402 
403     ly = uy = vec3_y(*(Vec3 *) str3->start->to);
404     for (ptr = str3->start;; ptr = ptr->next)
405     {
406         double  y = vec3_y(*(Vec3 *) ptr->to);
407 
408         if (y < ly)
409             ly = y;
410         if (y > uy)
411             uy = y;
412         if (ptr == str3->end)
413             break;
414     }
415     return (uy - ly);
416 }
417 
418 Conic3 *conic3_from_conic2(Conic * conic, double fit_thres)
419 {
420     Tstring *curve, *str, *string3d;
421     Conic   copy = {Conic_id};
422     Conic3 *con3;
423     Plane  *plane;
424     Vec2    p1 = {Vec2_id};
425     Vec2    p2 = {Vec2_id};
426     Vec2    pm = {Vec2_id};
427 
428     str = (Tstring *) prop_get(conic->props, STRING);
429     if (str == NULL)
430         return (NULL);
431     curve = conic2_string(conic);
432 
433     string3d = es_build_string3(curve, str);
434     if (string3d == NULL || y_range(string3d) < HORIZONTAL_THRES)
435     {
436         str_rm(string3d, rfree);
437         return (NULL);
438     }
439 
440     plane = plane_curve_ls(curve, string3d, fit_thres, &p1, &p2, &pm);
441     str_rm(string3d, rfree);
442     str_rm(curve, rfree);
443     copy = *conic;
444     conic_set_ends(&copy, p1, p2, pm);
445     con3 = conic_par_proj_to_plane(&copy, plane);
446     conic3_shift_origin_to_center(con3);
447 
448     return (con3);
449 }
450 
451 static void split_string3z( Tstring *string3d, Tstring **stringa, Tstring **stringb)
452 {
453     List *start, *ptr;
454     List *end;
455     Vec3 *p, *pold;
456     int count = 0, split = 0;
457     float diff, maxdiff=0.0;
458     List *rem = NULL, *mer = NULL;
459 
460     start = string3d->start;
461     end = string3d->end;
462 
463     for (ptr = start; ptr != end; ptr = ptr->next)
464     {
465         p = ptr->to; 
466         if (count >0)
467         {
468             diff = pold->el[2] - p->el[2];
469             if (fabs(diff)>maxdiff)
470             {
471                 maxdiff = fabs(diff);
472                 split = count;
473             }
474         }
475         pold = p;
476         count++;
477     }
478 
479     count = 0;
480     for (ptr =start; ptr != end; ptr = ptr->next)
481     {
482        pold = ptr->to;;
483        p = vec3_copy(pold);
484         
485        if (count < split) rem = dd_ref_addtostart(rem, (void *) p, VEC3);
486        if (count >= split) mer = dd_ref_addtostart(mer, (void *) p, VEC3);
487        count++;
488     }
489     *stringa = str_make(STRING, rem, dd_get_end(rem));
490     *stringb = str_make(STRING, mer, dd_get_end(mer)); 
491 } 
492 
493 static Conic3 *conic_proj_to_plane(Conic *conic, Tstring *curve, Tstring *string3d, double fit_thres)
494 {
495     Conic   copy = {Conic_id};
496     Conic3 *con3=NULL;
497     Plane  *plane;
498     Vec2    p1 = {Vec2_id};
499     Vec2    p2 = {Vec2_id};
500     Vec2    pm = {Vec2_id};
501 
502     if (string3d ==NULL) return(NULL);
503     plane = plane_curve_ls(curve, string3d, fit_thres, &p1, &p2, &pm);
504     if (plane==NULL) return(NULL);
505     copy = *conic;
506     conic_set_ends(&copy, p1, p2, pm);
507     con3 = conic_par_proj_to_plane(&copy, plane);
508     conic3_shift_origin_to_center(con3);
509     return(con3);
510 }
511 
512 /* data is filtered to extract up to two seperate strings which conform to the DG limit */
513 /* failed conic fits are re-tried following a split at the largest z gap.               */
514 /* This strategy recovers approx 95%  of fit-able (>6 = pts) 3D data NAT 27/10/08       */ 
515 List *conic3_from_conic2_rect(Conic * conic, double fit_thres)
516 {
517     Tstring *curve, *str, *string3d=NULL, *string3d2=NULL, *string3da=NULL, *string3db=NULL;
518     Conic3 *con3=NULL;
519     List   *conics=NULL;
520 
521     str = (Tstring *) prop_get(conic->props, STRING);
522     if (str == NULL)
523         return (NULL);
524 
525     curve = conic2_string(conic);
526     string3d = es_build_string3_rect(curve, str);
527     if (string3d == NULL) 
528         return(NULL);
529 
530     string3d2 = prop_get(string3d->props, EXTRADATA);
531     string3d->props = proplist_rm(string3d->props, EXTRADATA);
532 
533     con3 = conic_proj_to_plane(conic, curve, string3d, fit_thres);
534     if (con3 != NULL)
535         conics = ref_addtostart((List *) conics, (void *) con3, CONIC3);
536     else
537     {
538         split_string3z(string3d, &string3da, &string3db);
539         con3 = conic_proj_to_plane(conic, curve, string3da, fit_thres);
540         if (con3 != NULL)
541              conics = ref_addtostart((List *) conics, (void *) con3, CONIC3);
542         con3 = conic_proj_to_plane(conic, curve, string3db, fit_thres);
543         if (con3 != NULL)
544              conics = ref_addtostart((List *) conics, (void *) con3, CONIC3);
545         if (string3da != NULL) str_rm(string3da, rfree);
546         if (string3db != NULL) str_rm(string3db, rfree);
547     }
548     str_rm(string3d, rfree);
549 
550     if (string3d2 != NULL)
551     {
552         con3 = conic_proj_to_plane(conic, curve, string3d2, fit_thres);
553         if (con3 != NULL)
554             conics = ref_addtostart((List *) conics, (void *) con3, CONIC3);
555         else
556         {
557             split_string3z(string3d2, &string3da, &string3db);
558             con3 = conic_proj_to_plane(conic, curve, string3da, fit_thres);
559             if (con3 != NULL)
560                  conics = ref_addtostart((List *) conics, (void *) con3, CONIC3);
561             con3 = conic_proj_to_plane(conic, curve, string3db, fit_thres);
562             if (con3 != NULL)
563                  conics = ref_addtostart((List *) conics, (void *) con3, CONIC3);
564             if (string3da != NULL) str_rm(string3da, rfree);
565             if (string3db != NULL) str_rm(string3db, rfree);
566         }
567         str_rm(string3d2, rfree);
568     }
569     str_rm(curve, rfree);
570 
571     return (conics);
572 }
573 
574 void    conic3_format(Conic3 * conic)
575 {
576     Vec3    p = {Vec3_id};
577 
578     if (conic == NULL)
579         return;
580 
581     format("conic3   :\n");
582     p = conic->origin;
583     format("origin   : %15.6f %15.6f %15.6f\n", vec3_x(p), vec3_y(p), vec3_z(p));
584     p = conic->ex;
585     format("x axis   : %15.6f %15.6f %15.6f\n", vec3_x(p), vec3_y(p), vec3_z(p));
586     p = conic->ey;
587     format("y axis   : %15.6f %15.6f %15.6f\n", vec3_x(p), vec3_y(p), vec3_z(p));
588     p = conic->ez;
589     format("z axis   : %15.6f %15.6f %15.6f\n", vec3_x(p), vec3_y(p), vec3_z(p));
590     conic_format(conic->conic);
591     p = conic3_point(conic, conic->conic->t1);
592     format("p1       : %15.6f %15.6f %15.6f\n", vec3_x(p), vec3_y(p), vec3_z(p));
593     p = conic3_point(conic, conic->conic->t2);
594     format("p2       : %15.6f %15.6f %15.6f\n", vec3_x(p), vec3_y(p), vec3_z(p));
595     format("\n");
596 }
597 

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