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

Linux Cross Reference
Tina4/src/covira/snake_run.c

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

  1 /*
  2 *
  3 * snake_run.c
  4 *
  5 * Snake dynamics functions
  6 *
  7 */
  8  
  9 #include <tina/all_tina.h>
 10 #include <tina/brain.h>
 11 #include <tina/brainfuncs.h>
 12 
 13 static double sgn(double x)
 14 {
 15     if(x < 0.0)
 16         return(-1.0);
 17     else if(x == 0.0)
 18         return(0.0);
 19     else
 20         return(1.0);
 21 }
 22 
 23 /*
 24 solve cyclic pentiadiagonal system of equations in place
 25 (call with r.h.s. in x, returns with x set to solution)
 26 */
 27 static void pentadiag0(int n,
 28     double a0, double b0, double c0, double d0, double e0, double *x)
 29 {
 30     static double *a, *b, *c, *d, *e;
 31     static int n0 = 0;
 32     int i;
 33 
 34     if (n0 != n)
 35     {
 36         /* allocate workspace on first call, extend if necessary */
 37         n0 = n;
 38         tvector_free(a, 0, double);
 39         tvector_free(b, 0, double);
 40         tvector_free(c, 0, double);
 41         tvector_free(d, 0, double);
 42         tvector_free(e, 0, double);
 43         a = tvector_alloc(0, n0, double);
 44         b = tvector_alloc(0, n0, double);
 45         c = tvector_alloc(0, n0, double);
 46         d = tvector_alloc(0, n0, double);
 47         e = tvector_alloc(0, n0, double);
 48     }
 49     for (i = 0; i < n0; i++)
 50     {
 51         a[i] = a0;
 52         b[i] = b0;
 53         c[i] = c0;
 54         d[i] = d0;
 55         e[i] = e0;
 56     }
 57     pentadiag(n0, a, b, c, d, e, x);
 58 }
 59 
 60 /*
 61 returns maximum distance between corresponding knots of two snakes
 62 */
 63 double snake_dist(Snake * s1, Snake * s2)
 64 {
 65     int i, n = s1->n;
 66     double d, dmax = 0.0;
 67 
 68     for (i = 0; i < n; i++)
 69     {
 70         d = vec2_dist(s1->p[i], s2->p[i]);
 71         if (dmax < d)
 72             dmax = d;
 73     }
 74     return (dmax);
 75 }
 76 
 77 /*
 78 return snake force given by projection of gradient of potential
 79 normal to snake
 80 */
 81 void snake_gradient_forces(Snake * snake, Imrect * pot)
 82 {
 83     int i, n = snake->n;
 84     for (i = 0; i < n; i++)
 85     {
 86         Vec2 f, n;
 87 
 88         f = im_grad_vec2(pot, snake->p[i]);
 89         n = snake_perp(snake, i);
 90         snake->f[i] = vec2_projpar(f, n);
 91     }
 92 }
 93 
 94 /*
 95 return snake force given by component of hessian of potential
 96 normal to snake
 97 */
 98 void snake_hessian_forces(Snake * snake, Imrect * pot)
 99 {
100     int i, n = snake->n;
101 
102     for (i = 0; i < n; i++)
103     {
104         Mat2 h;
105         Vec2 n;
106         h = im_hess_mat2(pot, snake->p[i]);
107         n = snake_perp(snake, i);
108         snake->f[i] = vec2_projpar(mat2_vprod(h, n), n);
109     }
110 }
111 
112 void snake_trans_force(Snake * snake)
113 {
114     int i, n = snake->n;
115     snake->ftrans = vec2_zero();
116     for (i = 0; i < n; i++)
117         snake->ftrans = vec2_sum(snake->ftrans, snake->f[i]);
118     snake->ftrans = vec2_times(1.0 / n, snake->ftrans);
119     for (i = 0; i < n; i++)
120         snake->f[i] = vec2_diff(snake->f[i], snake->ftrans);
121 }
122 
123 void snake_scale_force(Snake * snake)
124 {
125     int i, n = snake->n;
126     Vec2 *p = snake->p;
127     Vec2 *f = snake->f;
128     Vec2 c = snake_centroid(snake);
129     double inertia = 0.0, k;
130 
131     snake->fscale = 0.0;
132     for (i = 0; i < n; i++)
133     {
134         Vec2 dp;
135 
136         dp = vec2_diff(p[i], c);
137         snake->fscale += vec2_dot(dp, f[i]);
138         inertia += vec2_sqrmod(dp);
139     }
140     k = snake->fscale / inertia;
141     for (i = 0; i < n; i++)
142     {
143         Vec2 dp;
144         dp = vec2_diff(p[i], c);
145         f[i] = vec2_diff(f[i], vec2_times(k, dp));
146     }
147 }
148 
149 void snake_rot_force(Snake * snake)
150 {
151     int i, n = snake->n;
152     Vec2 *p = snake->p;
153     Vec2 *f = snake->f;
154     Vec2 c = snake_centroid(snake);
155     double inertia = 0.0, k;
156 
157     snake->frot = 0.0;
158     for (i = 0; i < n; i++)
159     {
160         Vec2 dp;
161 
162         dp = vec2_diff(p[i], c);
163         snake->frot += vec2_cross(dp, f[i]);
164         inertia += vec2_sqrmod(dp);
165     }
166     k = snake->frot / inertia;
167     for (i = 0; i < n; i++)
168     {
169         Vec2 perp;
170         perp = vec2_perp(vec2_diff(p[i], c));
171         f[i] = vec2_diff(f[i], vec2_times(k, perp));
172     }
173 }
174 
175 void snake_trans_step(Snake * snake, double maxstep)
176 {
177     int i;
178     Vec2 *p = snake->p;
179     Vec2 trans = vec2_times(-maxstep, vec2_unit(snake->ftrans));
180     for(i = 0; i < snake->n; i++)
181         p[i] = vec2_sum(p[i], trans);
182 }
183 
184 void snake_scale_step(Snake * snake, double maxstep)
185 {
186     int i;
187     Vec2 c = snake_centroid(snake);
188     Vec2 *p = snake->p;
189     double scale = 1.0 - maxstep * sgn(snake->fscale);
190     for(i = 0; i < snake->n; i++)
191     {
192         Vec2 dp = vec2_diff(p[i], c);
193         p[i] = vec2_sum(c, vec2_times(scale, dp));
194     }
195 }
196 
197 void snake_rot_step(Snake * snake, double maxstep)
198 {
199     int i;
200     Vec2 c = snake_centroid(snake);
201     Mat2 r = rot2(-maxstep*sgn(snake->frot));
202     Vec2 *p = snake->p;
203     for(i = 0; i < snake->n; i++)
204     {
205         Vec2 dp = vec2_diff(p[i], c);
206         p[i] = vec2_sum(c, mat2_vprod(r, dp));
207     }
208 }
209 
210 /**
211 Move snake under forces f making maximum step maxstep
212 and find appropriate dt
213 **/
214 
215 void snake_external_step(Snake * snake, double maxstep, double *pdt)
216 {
217     int i, n = snake->n;
218     double fmax = 0.0, dt;
219     Vec2 *p = snake->p, *f = snake->f;
220     for (i = 0; i < n; i++)
221     {
222         fmax = MAX(fmax, fabs(vec2_x(f[i])));
223         fmax = MAX(fmax, fabs(vec2_y(f[i])));
224     }
225     if (fmax != 0.0)
226         dt = maxstep / fmax;
227     else
228         dt = 0.0;
229     for (i = 0; i < n; i++)
230         p[i] = vec2_diff(p[i], vec2_times(dt, f[i]));
231     *pdt = dt;
232 }
233 
234 void snake_internal_step(Snake * snake, double alpha, double beta, double dt)
235 {
236     static int nlast = 0;
237     static double *x = NULL, *y = NULL;
238     double a, b, c, d, e;
239     int i, n;
240     Vec2 *p;
241 
242     if (snake == NULL)
243         return;
244     n = snake->n;
245     p = snake->p;
246     if (nlast != n)
247     {
248         tvector_free(x, 0, double);
249         tvector_free(y, 0, double);
250         x = tvector_alloc(0, n, double);
251         y = tvector_alloc(0, n, double);
252         nlast = n;
253     }
254     for (i = 0; i < n; i++)
255     {
256         x[i] = vec2_x(p[i]);
257         y[i] = vec2_y(p[i]);
258     }
259     alpha *= dt*n*n/(32.0*32.0);
260     beta *= dt*n*n*n*n/(32.0*32.0*32.0*32.0);
261     e = a = beta;
262     d = b = -alpha - 4.0 * beta;
263     c = 1.0 + 2.0 * alpha + 6.0 * beta;
264     pentadiag0(n, a, b, c, d, e, x);
265     pentadiag0(n, a, b, c, d, e, y);
266     for (i = 0; i < n; i++)
267         p[i] = vec2(x[i], y[i]);
268 }
269 
270 double snake_run_trans(Snake * snake, Imrect * pot, int steps)
271 {
272     int i;
273     double ds;
274     Snake *copy = snake_copy(snake);
275 
276     for (i = 0; i < steps; i++)
277     {
278         snake->forcelaw(snake, pot);
279         snake_trans_force(snake);
280         snake_trans_step(snake, 0.5);
281     }
282     ds = snake_dist(snake, copy);
283     snake_free(copy);
284     return (ds);
285 }
286 
287 double snake_run_scale(Snake * snake, Imrect * pot, int steps)
288 {
289     int i;
290     double ds;
291     Snake *copy = snake_copy(snake);
292 
293     for (i = 0; i < steps; i++)
294     {
295         snake->forcelaw(snake, pot);
296         snake_trans_force(snake);
297         snake_scale_force(snake);
298         snake_scale_step(snake, 0.01);
299     }
300     ds = snake_dist(snake, copy);
301     snake_free(copy);
302     return (ds);
303 }
304 
305 double snake_run_rot(Snake * snake, Imrect * pot, int steps)
306 {
307     int i;
308     double ds;
309     Snake *copy = snake_copy(snake);
310 
311     for (i = 0; i < steps; i++)
312     {
313         snake->forcelaw(snake, pot);
314         snake_rot_force(snake);
315         snake_rot_step(snake, PI / 360.0);
316     }
317     ds = snake_dist(snake, copy);
318     snake_free(copy);
319     return (ds);
320 }
321 
322 double snake_run_rts(Snake * snake, Imrect * pot, int steps)
323 {
324     int i;
325     double ds;
326     Snake *copy = snake_copy(snake);
327 
328     for (i = 0; i < steps; i++)
329     {
330         snake->forcelaw(snake, pot);
331         snake_trans_force(snake);
332         snake_trans_step(snake, 2.0);
333         snake_scale_force(snake);
334         snake_scale_step(snake, 0.02);
335         snake_rot_force(snake);
336         snake_rot_step(snake, PI / 180.0);
337     }
338     for (i = 0; i < steps / 2; i++)
339     {
340         snake->forcelaw(snake, pot);
341         snake_trans_force(snake);
342         snake_trans_step(snake, 1.0);
343         snake_scale_force(snake);
344         snake_scale_step(snake, 0.01);
345         snake_rot_force(snake);
346         snake_rot_step(snake, PI / 360.0);
347     }
348     for (i = 0; i < steps / 4; i++)
349     {
350         snake->forcelaw(snake, pot);
351         snake_trans_force(snake);
352         snake_trans_step(snake, 0.5);
353         snake_scale_force(snake);
354         snake_scale_step(snake, 0.005);
355         snake_rot_force(snake);
356         snake_rot_step(snake, PI / 720.0);
357     }
358     ds = snake_dist(snake, copy);
359     snake_free(copy);
360     return (ds);
361 }
362 
363 double snake_run_all(Snake * snake, Imrect * pot, double alpha, double beta, int steps)
364 {
365     int i;
366     double ds;
367     Snake *copy = snake_copy(snake);
368 
369     for (i = 0; i < steps; i++)
370     {
371         double dt;
372         snake->forcelaw(snake, pot);
373         snake_external_step(snake, 1.0, &dt);
374         snake_internal_step(snake, alpha, beta, dt);
375     }
376     for (i = 0; i < steps / 2; i++)
377     {
378         double dt;
379         snake->forcelaw(snake, pot);
380         snake_external_step(snake, 0.5, &dt);
381         snake_internal_step(snake, alpha, beta, dt);
382     }
383     ds = snake_dist(snake, copy);
384     snake_free(copy);
385     return (ds);
386 }
387 

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