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

Linux Cross Reference
Tina4/src/vision/curve3/es_string3.c

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

  1 /**@(#)
  2 **/
  3 /* es_string3.c
  4  * 
  5  * build a string of 3d disparity points from an approximation to a curve
  6  * and a string of edges
  7  * 
  8  * the curve is a suitable approx to the edge string found by a spline or
  9  * conic fitter say
 10  * 
 11  */
 12 
 13 #include <math.h>
 14 #include <tina/sys.h>
 15 #include <tina/sysfuncs.h>
 16 #include <tina/math.h>
 17 #include <tina/mathfuncs.h>
 18 #include <tina/vision.h>
 19 #include <tina/visionfuncs.h>
 20 
 21 Tstring *es_par_proj_to_plane(Tstring * es, Plane * plane)
 22 {
 23     Tstring *str3;
 24     List *ptr;
 25     List *end;
 26     Vec3    p = {Vec3_id};
 27     Vec3    n = {Vec3_id};
 28 
 29     if (es == NULL || plane == NULL)
 30         return (NULL);
 31 
 32     p = plane->p;
 33     n = plane->n;
 34     str3 = str_clone(es);
 35     ptr = str3->start;
 36     end = str3->end;
 37 
 38     do
 39     {
 40         Vec3    v = {Vec3_id};
 41         Vec3    o = {Vec3_id};
 42         Vec2    pos = {Vec2_id};
 43 
 44         DD_GET_POS2(ptr, pos);
 45         par_proj_ray(pos, &o, &v);
 46         ptr->to = vec3_make(vec3_inter_line_plane(o, v, p, n));
 47         ptr->type = VEC3;
 48         if (ptr == end)
 49             break;
 50         ptr = ptr->next;
 51     } while (1);
 52     return (str3);
 53 }
 54 
 55 static Vec3 *disp_pos3(Vec2 p, Vec2 p1, Vec2 p2)
 56 {
 57     Vec2    v = {Vec2_id};
 58     float   x;
 59 
 60     v = vec2_unit(vec2_diff(p2, p1));
 61     x = vec2_x(p1) + (vec2_y(p) - vec2_y(p1)) * vec2_x(v) / vec2_y(v);
 62     return (vec3_make(vec3(x, vec2_y(p), vec2_x(p) - x)));
 63 }
 64 
 65 static Vec3 *cv_approx3_next(List * curv, Vec2 pos, List * end)
 66 {
 67     Vec2    cpos = {Vec2_id};
 68     Vec2    npos = {Vec2_id};
 69     List *ptr;
 70 
 71     DD_GET_POS2(curv, cpos);
 72 
 73     for (ptr = curv;; ptr = ptr->next)
 74     {
 75         DD_GET_POS2(ptr, npos);
 76         if (BETWEEN(vec2_y(pos), vec2_y(cpos), vec2_y(npos)))
 77             return (disp_pos3(pos, cpos, npos));
 78 
 79         if (ptr == end)
 80             break;
 81     }
 82     return (NULL);
 83 }
 84 
 85 static Vec3 *cv_approx3_last(List * curv, Vec2 pos, List * start)
 86 {
 87     Vec2    cpos = {Vec2_id};
 88     Vec2    lpos = {Vec2_id};
 89     List *ptr;
 90 
 91     DD_GET_POS2(curv, cpos);
 92 
 93     for (ptr = curv;; ptr = ptr->last)
 94     {
 95         DD_GET_POS2(ptr, lpos);
 96         if (BETWEEN(vec2_y(pos), vec2_y(cpos), vec2_y(lpos)))
 97             return (disp_pos3(pos, cpos, lpos));
 98 
 99         if (ptr == start)
100             break;
101     }
102     return (NULL);
103 }
104 
105 Vec3   *cv_approx3(List * curv, Vec2 pos, List * start, List * end)
106 {
107     Vec2    cpos = {Vec2_id};
108     Vec2    npos = {Vec2_id};
109     Vec2    lpos = {Vec2_id};
110 
111     if (curv == NULL || (curv->last == NULL && curv->next == NULL))
112         return (NULL);
113 
114     if (curv == start)
115         return (cv_approx3_next(curv, pos, end));
116     if (curv == end)
117         return (cv_approx3_last(curv, pos, start));
118 
119     DD_GET_POS2(curv, cpos);
120     DD_GET_POS2(curv->last, lpos);
121     DD_GET_POS2(curv->next, npos);
122 
123     if ((vec2_y(cpos) < vec2_y(lpos)) == (vec2_y(cpos) < vec2_y(npos))) /* ambiguous */
124         return (NULL);
125 
126     if ((vec2_y(cpos) < vec2_y(pos)) == (vec2_y(cpos) < vec2_y(npos)))  /* go next */
127         return (cv_approx3_next(curv, pos, end));
128     if ((vec2_y(cpos) < vec2_y(pos)) == (vec2_y(cpos) < vec2_y(lpos)))  /* go last */
129             return (cv_approx3_last(curv, pos, start));
130     return (NULL);              /* strange occurance */
131 }
132 
133 /* build a fast index into the string structure */
134 static Rindex *string_index_get(Tstring * es)
135 {
136     Imregion *roi = es_bounding_region(es);
137     List *end;
138     List *ptr;
139     Rindex *rx;
140     List  **index;
141 
142     if (roi == NULL)
143         return (NULL);
144 
145     rx = rx_alloc(roi, STRING);
146     rfree((void *) roi);
147     index = (List **) rx->index;
148     ptr = es->start;
149     end = es->end;
150 
151     do
152     {
153         Vec2    p = {Vec2_id};
154         int     i;
155 
156         DD_GET_POS2(ptr, p);
157         i = (int)floor(vec2_y(p));
158         index[i] = ref_addtostart(index[i], (void *) ptr, LIST);
159         if (ptr == end)
160             break;
161         ptr = ptr->next;
162     } while (1);
163     return (rx);
164 }
165 
166 #define SEARCH_WINDOW  3
167 #define MAX_ALLOW_DIFF 5.0
168 
169 static List *closest_in_index(Rindex * rx, Vec2 pos)
170 {
171     int     r, i;
172     List *closest = NULL;
173     Imregion *roi;
174     List   *ptr;
175     Vec2    p = {Vec2_id};
176     float   dist;
177     float   min_dist = (float)0.0;
178 
179     if (rx == NULL)
180         return (NULL);
181 
182     roi = rx->region;
183 
184     r = (int)floor(vec2_y(p));
185     i = r - SEARCH_WINDOW;
186     if (i < roi->ly)
187         i = roi->ly;
188     r += SEARCH_WINDOW;
189     if (r > roi->uy)
190         r = roi->uy;
191 
192     while (i < r)
193     {
194         for (ptr = rx->index[i]; ptr != NULL; ptr = ptr->next)
195         {
196             List *dd = (List *) ptr->to;
197 
198             DD_GET_POS2(dd, p);
199 
200             dist = (float)vec2_mod(vec2_diff(pos, p));
201             if (dist < MAX_ALLOW_DIFF && (closest == NULL || dist < min_dist))
202             {
203                 min_dist = dist;
204                 closest = dd;
205             }
206         }
207         ++i;
208     }
209     return (closest);
210 }
211 
212 Tstring *es_build_string3(Tstring * curv, Tstring * es)
213 /* approximated geometrical string (not always edge) */
214 /* associated edge string */
215 {
216     List   *mlist;
217     List   *mptr;
218     List *dispstr = NULL;
219     List *start;
220     List *end;
221     Rindex *rx;
222 
223     if (curv == NULL || es == NULL)
224         return (NULL);
225 
226     /* get list of matches unique in ->to2  ordered along list */
227     mlist = es_get_list_of_matches(es);
228     rx = string_index_get(curv);/* index string */
229 
230     start = curv->start;
231     end = curv->end;
232 
233     for (mptr = mlist; mptr != NULL; mptr = mptr->next)
234     {
235         List *dptr;
236         List *closest;
237         Tstring *s1, *s2;
238         Vec2    p = {Vec2_id};
239 
240         s1 = (Tstring *) ((Match *) mptr->to)->to1;
241         s2 = (Tstring *) ((Match *) mptr->to)->to2;
242 
243         p = DD_EDGE_POS(s1->start);
244         closest = closest_in_index(rx, p);
245         if (closest == NULL)
246             closest = str_get_min(curv, dist_to_pos2, (void *) &p);     /* slow */
247 
248         for (dptr = s2->start;; dptr = dptr->next)
249         {
250             Vec3   *p;
251 
252             p = cv_approx3(closest, DD_EDGE_POS(dptr), start, end);
253             if (p != NULL)
254                 dispstr = dd_ref_addtostart(dispstr, (void *) p, VEC3);
255             if (dptr == s2->end)
256                 break;
257         }
258     }
259     rx_free_links(rx);          /* free sting index */
260     if (dispstr == NULL)
261         return (NULL);
262     return (str_make(STRING, dispstr, dd_get_end(dispstr)));
263 }
264 
265 Tstring *es_build_string4(Tstring * curv, Tstring * es)
266 /* approximated geometrical string (not always edge) */
267 /* associated edge string */
268 {
269     List   *mlist;
270     List   *mptr;
271     List *dispstr = NULL;
272     List *start;
273     List *end;
274     Rindex *rx;
275 
276     if (curv == NULL || es == NULL)
277         return (NULL);
278 
279     /* get list of matches unique in ->to2  ordered along list */
280     mlist = es_get_list_of_matches(es);
281     rx = string_index_get(curv);/* index string */
282 
283     start = curv->start;
284     end = curv->end;
285 
286     for (mptr = mlist; mptr != NULL; mptr = mptr->next)
287     {
288         List *dptr;
289         List *closest;
290         Tstring *s1, *s2;
291         Vec2    p = {Vec2_id};
292 
293         s1 = (Tstring *) ((Match *) mptr->to)->to1;
294         s2 = (Tstring *) ((Match *) mptr->to)->to2;
295 
296         p = DD_EDGE_POS(s1->start);
297         closest = closest_in_index(rx, p);
298         if (closest == NULL)
299             closest = str_get_min(curv, dist_to_pos2, (void *) &p);     /* slow */
300 
301         for (dptr = s2->start;; dptr = dptr->next)
302         {
303             Edgel  *edge = DD_EDGE(dptr);
304             Vec3   *p3;
305             Vec4   *p4;
306 
307             p3 = cv_approx3(closest, edge->pos, start, end);
308             if (p3 != NULL)
309             {
310                 p4 = vec4_alloc();
311                 *(Vec3 *) p4 = *p3;
312                 vec4_w(*p4) = edge->orient;
313                 dispstr = dd_ref_addtostart(dispstr, (void *) p4, VEC4);
314                 rfree((void *) p3);
315             }
316             if (dptr == s2->end)
317                 break;
318         }
319     }
320     rx_free_links(rx);          /* free string index */
321     if (dispstr == NULL)
322         return (NULL);
323     return (str_make(STRING, dispstr, dd_get_end(dispstr)));
324 }
325 

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