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

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

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