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

Linux Cross Reference
Tina4/src/vision/curve2/curvature.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 
 11 static double qspline_curvature(Vec2 p0, Vec2 p1, Vec2 p2)
 12 {
 13     double  x0 = vec2_x(p0), x1 = vec2_x(p1), x2 = vec2_x(p2);
 14     double  y0 = vec2_y(p0), y1 = vec2_y(p1), y2 = vec2_y(p2);
 15     double  dx, dy, ddx, ddy, ds;
 16 
 17     dx = 0.5 * (x2 - x0);
 18     ddx = x2 - 2.0 * x1 + x0;
 19     dy = 0.5 * (y2 - y0);
 20     ddy = y2 - 2.0 * y1 + y0;
 21 
 22     ds = sqrt(dx * dx + dy * dy);
 23     return ((dx * ddy - dy * ddx) / (ds * ds * ds));
 24 }
 25 
 26 double  dds_curvature(List * s, int n)
 27 {
 28     int     i;
 29     List *ptr0;
 30     List *ptr2;
 31     Vec2    p0 = {Vec2_id};
 32     Vec2    p1 = {Vec2_id};
 33     Vec2    p2 = {Vec2_id};
 34 
 35     /** try to go n points forward && back **/
 36 
 37     for (ptr0 = s, i = 0; ptr0->last != NULL && i < n; ptr0 = ptr0->last, i++)
 38         ;
 39     for (ptr2 = s, i = 0; ptr2->next != NULL && i < n; ptr2 = ptr2->next, i++)
 40         ;
 41 
 42     DD_GET_POS2(ptr0, p0);
 43     DD_GET_POS2(s, p1);
 44     DD_GET_POS2(ptr2, p2);
 45 
 46     return (qspline_curvature(p0, p1, p2));
 47 }
 48 
 49 double  es_curvature(Tstring * es, List * p, double arcratio, double arcmin, double arcmax)
 50 /* edge type string */
 51 /* point on the string at which to measure curvature */
 52 /* try to achieve this ratio of arc length to radius */
 53 /* min and max arc length to use */
 54 {
 55     List *start = es->start;
 56     List *end = es->end;
 57     List *ptr1;
 58     List *ptr2;
 59     List *ptrm;
 60     Vec2    p1 = {Vec2_id};
 61     Vec2    p2 = {Vec2_id};
 62     Vec2    pm = {Vec2_id};
 63     double  arc, curvature;
 64     int     extend1, extend2;
 65 
 66     arc = arcmin;
 67     ptr1 = ptr2 = p;
 68     DD_GET_POS2(ptr1, p1);
 69     DD_GET_POS2(ptr2, p2);
 70     extend1 = (ptr1 == start) ? 0 : 1;
 71     extend2 = (ptr2 == end) ? 0 : 1;
 72     while (extend1 || extend2)
 73     {
 74         if (extend1)
 75         {
 76             DD_GET_POS2(ptr1->last, p1);
 77             if (vec2_dist(p1, p2) > arc)
 78                 extend1 = 0;
 79             else
 80                 ptr1 = ptr1->last;
 81             if (ptr1 == start)
 82                 extend1 = 0;
 83         }
 84         if (extend2)
 85         {
 86             DD_GET_POS2(ptr2->next, p2);
 87             if (vec2_dist(p1, p2) > arc)
 88                 extend2 = 0;
 89             else
 90                 ptr2 = ptr2->next;
 91             if (ptr2 == end)
 92                 extend2 = 0;
 93         }
 94         if (!extend1 && !extend2)
 95         {
 96             double  arcnew;
 97 
 98             ptrm = ddstr_mid_point(ptr1, ptr2);
 99             if (ptr1 == ptrm || ptr2 == ptrm)
100                 return (0.0);
101             DD_GET_POS2(ptrm, pm);
102             curvature = qspline_curvature(p1, pm, p2);
103             arcnew = ((arcnew = arcratio / curvature) > arcmax) ? arcmax : arcnew;
104             if (arcnew > arc)
105             {
106                 arc = arcnew;
107                 if (ptr1 != start)
108                     extend1 = 1;
109                 if (ptr2 != end)
110                     extend2 = 1;
111             }
112         }
113     }
114     return (curvature);
115 }
116 
117 static double qspline_diffgeom(Vec2 p0, Vec2 p1, Vec2 p2, Vec2 * p, Vec2 * t, double *k)
118 {
119     double  x0 = vec2_x(p0), x1 = vec2_x(p1), x2 = vec2_x(p2);
120     double  y0 = vec2_y(p0), y1 = vec2_y(p1), y2 = vec2_y(p2);
121     double  dx, dy, ddx, ddy, ds;
122 
123     dx = 0.5 * (x2 - x0);
124     ddx = x2 - 2.0 * x1 + x0;
125     dy = 0.5 * (y2 - y0);
126     ddy = y2 - 2.0 * y1 + y0;
127 
128     ds = sqrt(dx * dx + dy * dy);
129     *p = p1;
130     *t = vec2(dx / ds, dy / ds);
131     *k = (dx * ddy - dy * ddx) / (ds * ds * ds);
132     return(ds);
133 }
134 
135 double  dds_diffgeom(List * s, Vec2 * p, Vec2 * t, double *k, int n)
136 {
137     int     i;
138     List *ptr0;
139     List *ptr2;
140     Vec2    p0 = {Vec2_id};
141     Vec2    p1 = {Vec2_id};
142     Vec2    p2 = {Vec2_id};
143 
144     /** try to go n points forward && back **/
145 
146     for (ptr0 = s, i = 0; ptr0->last != NULL && i < n; ptr0 = ptr0->last, i++)
147         ;
148     for (ptr2 = s, i = 0; ptr2->next != NULL && i < n; ptr2 = ptr2->next, i++)
149         ;
150     DD_GET_POS2(ptr0, p0);
151     DD_GET_POS2(s, p1);
152     DD_GET_POS2(ptr2, p2);
153     qspline_diffgeom(p0, p1, p2, p, t, k);
154     /* BUG control reaches end of non-void function */
155 }
156 
157 double  es_diffgeom(Tstring * es, List * p, double arcratio, double arcmin, double arcmax, Vec2 * pos, Vec2 * t, double *k)
158 /* edge type string */
159 /* point on the string at which to measure curvature */
160 /* try to achieve this ratio of arc length to radius */
161 /* min and max arc length to use */
162 
163 
164 {
165     List *start = es->start;
166     List *end = es->end;
167     List *ptr1;
168     List *ptr2;
169     List *ptrm;
170     Vec2    p1 = {Vec2_id};
171     Vec2    p2 = {Vec2_id};
172     Vec2    pm = {Vec2_id};
173     double  arc, curvature;
174     int     extend1, extend2;
175 
176     arc = arcmin;
177     ptr1 = ptr2 = p;
178     DD_GET_POS2(ptr1, p1);
179     DD_GET_POS2(ptr2, p2);
180     extend1 = (ptr1 == start) ? 0 : 1;
181     extend2 = (ptr2 == end) ? 0 : 1;
182     while (extend1 || extend2)
183     {
184         if (extend1)
185         {
186             DD_GET_POS2(ptr1->last, p1);
187             if (vec2_dist(p1, p2) > arc)
188                 extend1 = 0;
189             else
190                 ptr1 = ptr1->last;
191             if (ptr1 == start)
192                 extend1 = 0;
193         }
194         if (extend2)
195         {
196             DD_GET_POS2(ptr2->next, p2);
197             if (vec2_dist(p1, p2) > arc)
198                 extend2 = 0;
199             else
200                 ptr2 = ptr2->next;
201             if (ptr2 == end)
202                 extend2 = 0;
203         }
204         if (!extend1 && !extend2)
205         {
206             double  arcnew;
207 
208             ptrm = ddstr_mid_point(ptr1, ptr2);
209             DD_GET_POS2(ptrm, pm);
210             qspline_diffgeom(p1, pm, p2, pos, t, k);
211             curvature = *k;
212             arcnew = ((arcnew = arcratio / curvature) > arcmax) ? arcmax : arcnew;
213             if (arcnew > arc)
214             {
215                 arc = arcnew;
216                 if (ptr1 != start)
217                     extend1 = 1;
218                 if (ptr2 != end)
219                     extend2 = 1;
220             }
221         }
222     }
223     /* BUG control reaches end of non-void function */
224 }
225 
226 static Vec2 *str_pos2_to_array(Tstring * s, int *n)
227 {
228     int     i, m;
229     Vec2   *va;
230     List *dd;
231 
232     if (s == NULL)
233     {
234         /* BUGFIX JB 22/11/93 was *n == -1; */
235         *n = -1;
236         return (NULL);
237     }
238     m = str_length(s);
239 
240     va = ts_nralloc(m, Vec2);
241     for (dd = s->start, i = 0; i < m; dd = dd->next, i++)
242         DD_GET_POS2(dd, va[i]);
243 
244     *n = m;
245     return (va);
246 }
247 
248 static void str_pos2_from_array(Tstring * s, Vec2 * va)
249 {
250     List *dd;
251     int     i;
252 
253     if (s == NULL || va == NULL)
254         return;
255 
256     for (dd = s->start, i = 0;; dd = dd->next, i++)
257     {
258         DD_SET_POS2(dd, va[i]);
259         if (dd == s->end)
260             break;
261     }
262 }
263 
264 Vec2    vec2_smooth(Vec2 v0, Vec2 v1, Vec2 v2)
265 {
266     double  x0 = vec2_x(v0), y0 = vec2_y(v0);
267     double  x1 = vec2_x(v1), y1 = vec2_y(v1);
268     double  x2 = vec2_x(v2), y2 = vec2_y(v2);
269 
270     x1 = 0.25 * (x0 + 2.0 * x1 + x2);
271     y1 = 0.25 * (y0 + 2.0 * y1 + y2);
272 
273     return (vec2(x1, y1));
274 }
275 
276 void    loop_smooth(int l, Vec2 * array, Vec2 * store)
277 {
278     int     i;
279 
280     store[0] = vec2_smooth(array[l - 1], array[0], array[1]);
281     for (i = 1; i < l - 1; i++)
282         store[i] = vec2_smooth(array[i - 1], array[i], array[i + 1]);
283     store[l - 1] = vec2_smooth(array[l - 2], array[l - 1], array[0]);
284 
285     for (i = 0; i < l; i++)
286         array[i] = store[i];
287 }
288 
289 void    endfix_smooth(int l, Vec2 * array, Vec2 * store)
290 {
291     int     i;
292 
293     for (i = 1; i < l - 1; i++)
294         store[i] = vec2_smooth(array[i - 1], array[i], array[i + 1]);
295 
296     for (i = 1; i < l - 1; i++)
297         array[i] = store[i];
298 }
299 
300 void    str_smooth(Tstring * s, int n)
301 {
302     Vec2   *array;
303     Vec2   *store;
304     int     i, l;
305 
306     array = str_pos2_to_array(s, &l);
307     store = ts_nralloc(l, Vec2);
308 
309     for (i = 0; i < n; i++)
310     {
311         if (s->type == LOOP)
312             loop_smooth(l, array, store);
313         else
314             endfix_smooth(l, array, store);
315     }
316     str_pos2_from_array(s, array);
317 
318     rfree((void *) array);
319     rfree((void *) store);
320 }
321 

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