1 /*
2 * -----------------------------------------------------------------
3 * $Revision:$
4 * $Date:$
5 * -----------------------------------------------------------------
6 * Programmer: Radu Serban @ LLNL
7 * -----------------------------------------------------------------
8 * Example problem for the event detection in CPODES: a 2-pendulum
9 * Newton's craddle. Two identical pendulums of length L and
10 * mass M are suspended at points (+R,0) and (-R,0), where R is the
11 * radius of the balls.
12 * We consider elastic impact with a coefficient of restitution C=1.
13 *
14 * Case 1: The first pendulum has an initial horizontal
15 * velocity V0, while the second one is at rest.
16 * Case 2: Both pendulums are initially at rest. The first one is
17 * then moved from rest after a short interval, by applying
18 * an external force, F = m*frc(t), where
19 * frc(t) = 3(1-cos(5t)) on t =[0,2*pi/5] and 0 otherwise.
20 *
21 * Each pendulum is modeled with 2 coordinates (x and y) and one
22 * constraint (x^2 + y^2 = L^2), resulting in a 1st order system
23 * with 8 diferential equations and 4 constraints.
24 * -----------------------------------------------------------------
25 */
26
27 #include <stdio.h>
28 #include <stdlib.h>
29 #include <math.h>
30
31 #include <cpodes/cpodes.h>
32 #include <cpodes/cpodes_dense.h>
33 #include <nvector/nvector_serial.h>
34
35 /* User data structure */
36
37 typedef struct {
38 realtype R;
39 realtype L;
40 realtype m;
41 realtype g;
42 } *PbData;
43
44 /* Functions Called by the Solver */
45
46 static int ffun1(realtype t, N_Vector y, N_Vector ydot, void *f_data);
47 static int ffun2(realtype t, N_Vector y, N_Vector ydot, void *f_data);
48
49 static int gfun(realtype t, N_Vector y, N_Vector yp, realtype *gout, void *g_data);
50
51 static int cfun(realtype t, N_Vector yy, N_Vector cout, void *c_data);
52
53 static void contact(N_Vector yy, PbData data);
54
55 static void PrintFinalStats(void *cpode_mem);
56
57 /*
58 *-------------------------------
59 * Main Program
60 *-------------------------------
61 */
62
main()63 int main()
64 {
65 PbData data;
66 realtype R, L, m, g, V0;
67 void *cpode_mem;
68 N_Vector yy, yp, ctols;
69 realtype reltol, abstol;
70 realtype t0, tf, t;
71 realtype x1, y1, x2, y2;
72 int flag, Neq, Nc;
73 int rdir[1], iroots[1];
74
75 FILE *fout;
76
77 /* --------------------------------
78 * INITIALIZATIONS
79 * -------------------------------- */
80
81 R = 0.1; /* ball radius */
82 L = 1.0; /* pendulum length */
83 m = 1.0; /* pendulum mass */
84 g = 9.8; /* gravitational acc. */
85
86 V0 = 3.0; /* initial velocity for pendulum 1 */
87
88 /* Set-up user data structure */
89
90 data = (PbData)malloc(sizeof *data);
91 data->R = R;
92 data->L = L;
93 data->m = m;
94 data->g = g;
95
96 /* Problem dimensions */
97
98 Neq = 2*2*2;
99 Nc = 2*2;
100
101 /* Solution vectors */
102
103 yy = N_VNew_Serial(Neq);
104 yp = N_VNew_Serial(Neq);
105
106 /* Integration limits */
107
108 t0 = 0.0;
109 tf = 6.0;
110
111 /* Integration and projection tolerances */
112
113 reltol = 1.0e-8;
114 abstol = 1.0e-8;
115
116 ctols = N_VNew_Serial(Nc);
117 N_VConst(1.0e-8, ctols);
118
119 /* Direction of monitored events
120 * (only zero-crossing with decreasing even function) */
121
122 rdir[0] = -1;
123
124 /* --------------------------------
125 * CASE 1
126 * -------------------------------- */
127
128 fout = fopen("newton1.out","w");
129
130 /* Initial conditions */
131
132 NV_Ith_S(yy,0) = R; NV_Ith_S(yy,4) = -R;
133 NV_Ith_S(yy,1) = -L; NV_Ith_S(yy,5) = -L;
134 NV_Ith_S(yy,2) = V0; NV_Ith_S(yy,6) = 0.0;
135 NV_Ith_S(yy,3) = 0.0; NV_Ith_S(yy,7) = 0.0;
136
137 /* Initialize solver */
138
139 cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
140 flag = CPodeInit(cpode_mem, ffun1, data, t0, yy, yp, CP_SS, reltol, &abstol);
141 flag = CPDense(cpode_mem, Neq);
142
143 flag = CPodeRootInit(cpode_mem, 1, gfun, data);
144 flag = CPodeSetRootDirection(cpode_mem, rdir);
145
146 /* Set-up the internal projection */
147
148 flag = CPodeProjInit(cpode_mem, CP_PROJ_L2NORM, CP_CNSTR_NONLIN, cfun, data, ctols);
149 flag = CPodeSetProjTestCnstr(cpode_mem, TRUE);
150 flag = CPDenseProj(cpode_mem, Nc, Neq, CPDIRECT_LU);
151
152 /* Integrate in ONE_STEP mode, while monitoring events */
153
154 t = t0;
155 while(t<tf) {
156
157 flag = CPode(cpode_mem, tf, &t, yy, yp, CP_ONE_STEP);
158
159 if (flag < 0) break;
160
161 x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
162 y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
163 fprintf(fout, "%lf %14.10lf %14.10lf %14.10lf %14.10lf", t, x1, y1, x2, y2);
164
165 if (flag == CP_ROOT_RETURN) {
166
167 CPodeGetRootInfo(cpode_mem, iroots);
168 fprintf(fout, " %d\n", iroots[0]);
169
170 /* Note: the test iroots[0]<0 is really needed ONLY if not using rdir */
171
172 if (iroots[0] < 0) {
173 /* Update velocities in yy */
174 contact(yy, data);
175 /* reinitialize CPODES solver */
176 flag = CPodeReInit(cpode_mem, ffun1, data, t, yy, yp, CP_SS, reltol, &abstol);
177 }
178
179 } else {
180
181 fprintf(fout, " 0\n");
182
183 }
184
185 }
186
187 PrintFinalStats(cpode_mem);
188
189 CPodeFree(&cpode_mem);
190
191 fclose(fout);
192
193 /* --------------------------------
194 * CASE 2
195 * -------------------------------- */
196
197 fout = fopen("newton2.out","w");
198
199 /* Initial conditions */
200
201 NV_Ith_S(yy,0) = R; NV_Ith_S(yy,4) = -R;
202 NV_Ith_S(yy,1) = -L; NV_Ith_S(yy,5) = -L;
203 NV_Ith_S(yy,2) = 0.0; NV_Ith_S(yy,6) = 0.0;
204 NV_Ith_S(yy,3) = 0.0; NV_Ith_S(yy,7) = 0.0;
205
206 /* Initialize solver */
207
208 cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
209 flag = CPodeInit(cpode_mem, ffun2, data, t0, yy, yp, CP_SS, reltol, &abstol);
210 flag = CPDense(cpode_mem, Neq);
211
212 flag = CPodeRootInit(cpode_mem, 1, gfun, data);
213 flag = CPodeSetRootDirection(cpode_mem, rdir);
214
215 /* Set-up the internal projection */
216
217 flag = CPodeProjInit(cpode_mem, CP_PROJ_L2NORM, CP_CNSTR_NONLIN, cfun, data, ctols);
218 flag = CPodeSetProjTestCnstr(cpode_mem, TRUE);
219 flag = CPDenseProj(cpode_mem, Nc, Neq, CPDIRECT_LU);
220
221 /* Integrate in ONE_STEP mode, while monitoring events */
222
223 t = t0;
224 while(t<tf) {
225
226 flag = CPode(cpode_mem, tf, &t, yy, yp, CP_ONE_STEP);
227
228 if (flag < 0) break;
229
230 x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
231 y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
232 fprintf(fout, "%lf %14.10lf %14.10lf %14.10lf %14.10lf", t, x1, y1, x2, y2);
233
234 if (flag == CP_ROOT_RETURN) {
235
236 CPodeGetRootInfo(cpode_mem, iroots);
237 fprintf(fout, " %d\n", iroots[0]);
238
239 /* Note: the test iroots[0]<0 is really needed ONLY if not using rdir */
240
241 if (iroots[0] < 0) {
242 /* Update velocities in yy */
243 contact(yy, data);
244 /* reinitialize CPODES solver */
245 flag = CPodeReInit(cpode_mem, ffun2, data, t, yy, yp, CP_SS, reltol, &abstol);
246 }
247
248 } else {
249
250 fprintf(fout, " 0\n");
251
252 }
253
254 }
255
256 PrintFinalStats(cpode_mem);
257
258 CPodeFree(&cpode_mem);
259
260 fclose(fout);
261
262 /* --------------------------------
263 * CLEAN-UP
264 * -------------------------------- */
265
266 free(data);
267 N_VDestroy_Serial(yy);
268 N_VDestroy_Serial(yp);
269 N_VDestroy_Serial(ctols);
270
271 return(0);
272 }
273
274
ffun1(realtype t,N_Vector yy,N_Vector fy,void * f_data)275 static int ffun1(realtype t, N_Vector yy, N_Vector fy, void *f_data)
276 {
277 PbData data;
278 realtype x1, y1, x2, y2;
279 realtype vx1, vy1, vx2, vy2;
280 realtype ax1, ay1, ax2, ay2;
281 realtype lam1, lam2;
282 realtype R, L, m, g;
283
284 data = (PbData) f_data;
285 R = data->R;
286 L = data->L;
287 m = data->m;
288 g = data->g;
289
290 x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
291 y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
292 vx1 = NV_Ith_S(yy,2); vx2 = NV_Ith_S(yy,6);
293 vy1 = NV_Ith_S(yy,3); vy2 = NV_Ith_S(yy,7);
294
295 lam1 = m/(2*L*L)*(vx1*vx1 + vy1*vy1 - g*y1);
296 lam2 = m/(2*L*L)*(vx2*vx2 + vy2*vy2 - g*y2);
297
298 ax1 = -2.0*(x1-R)*lam1/m;
299 ay1 = -2.0*y1*lam1/m - g;
300
301 ax2 = -2.0*(x2+R)*lam2/m;
302 ay2 = -2.0*y2*lam2/m - g;
303
304 NV_Ith_S(fy,0) = vx1; NV_Ith_S(fy, 4) = vx2;
305 NV_Ith_S(fy,1) = vy1; NV_Ith_S(fy, 5) = vy2;
306 NV_Ith_S(fy,2) = ax1; NV_Ith_S(fy, 6) = ax2;
307 NV_Ith_S(fy,3) = ay1; NV_Ith_S(fy, 7) = ay2;
308
309 return(0);
310 }
311
ffun2(realtype t,N_Vector yy,N_Vector fy,void * f_data)312 static int ffun2(realtype t, N_Vector yy, N_Vector fy, void *f_data)
313 {
314 PbData data;
315 realtype x1, y1, x2, y2;
316 realtype vx1, vy1, vx2, vy2;
317 realtype ax1, ay1, ax2, ay2;
318 realtype lam1, lam2;
319 realtype R, L, m, g;
320 realtype pi, frc;
321
322
323 pi = 4.0*atan(1.0);
324 if ( (t <= 2*pi/5.0) ) {
325 frc = 3.0 * ( 1.0 - cos(5.0*t) );
326 } else {
327 frc = 0.0;
328 }
329
330 data = (PbData) f_data;
331 R = data->R;
332 L = data->L;
333 m = data->m;
334 g = data->g;
335
336 x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
337 y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
338 vx1 = NV_Ith_S(yy,2); vx2 = NV_Ith_S(yy,6);
339 vy1 = NV_Ith_S(yy,3); vy2 = NV_Ith_S(yy,7);
340
341 lam1 = m/(2*L*L)*(vx1*vx1 + vy1*vy1 + frc*(x1-R) - g*y1);
342 lam2 = m/(2*L*L)*(vx2*vx2 + vy2*vy2 - g*y2);
343
344 ax1 = -2.0*(x1-R)*lam1/m + frc;
345 ay1 = -2.0*y1*lam1/m - g;
346
347 ax2 = -2.0*(x2+R)*lam2/m;
348 ay2 = -2.0*y2*lam2/m - g;
349
350 NV_Ith_S(fy,0) = vx1; NV_Ith_S(fy, 4) = vx2;
351 NV_Ith_S(fy,1) = vy1; NV_Ith_S(fy, 5) = vy2;
352 NV_Ith_S(fy,2) = ax1; NV_Ith_S(fy, 6) = ax2;
353 NV_Ith_S(fy,3) = ay1; NV_Ith_S(fy, 7) = ay2;
354
355 return(0);
356 }
357
cfun(realtype t,N_Vector yy,N_Vector c,void * c_data)358 static int cfun(realtype t, N_Vector yy, N_Vector c, void *c_data)
359 {
360 PbData data;
361 realtype x1, y1, x2, y2;
362 realtype vx1, vy1, vx2, vy2;
363 realtype R, L;
364
365 data = (PbData) c_data;
366 R = data->R;
367 L = data->L;
368
369 x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
370 y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
371 vx1 = NV_Ith_S(yy,2); vx2 = NV_Ith_S(yy,6);
372 vy1 = NV_Ith_S(yy,3); vy2 = NV_Ith_S(yy,7);
373
374 NV_Ith_S(c,0) = (x1-R)*(x1-R) + y1*y1 - L*L;
375 NV_Ith_S(c,1) = (x1-R)*vx1 + y1*vy1;
376
377 NV_Ith_S(c,2) = (x2+R)*(x2+R) + y2*y2 - L*L;
378 NV_Ith_S(c,3) = (x2+R)*vx2 + y2*vy2;
379
380 return(0);
381 }
382
gfun(realtype t,N_Vector yy,N_Vector yp,realtype * gval,void * g_data)383 static int gfun(realtype t, N_Vector yy, N_Vector yp, realtype *gval, void *g_data)
384 {
385 PbData data;
386 realtype x1, y1, x2, y2;
387 realtype R, L, m, g;
388
389 data = (PbData) g_data;
390 R = data->R;
391 L = data->L;
392 m = data->m;
393 g = data->g;
394
395 x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
396 y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
397
398 gval[0] = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) - 4.0*R*R;
399
400 return(0);
401 }
402
403
contact(N_Vector yy,PbData data)404 static void contact(N_Vector yy, PbData data)
405 {
406 realtype x1, y1, x2, y2;
407 realtype vx1, vy1, vx2, vy2;
408 realtype vt1, vn1, vn1_, vt2, vn2, vn2_;
409 realtype alpha, ca, sa;
410
411 x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
412 y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
413 vx1 = NV_Ith_S(yy,2); vx2 = NV_Ith_S(yy,6);
414 vy1 = NV_Ith_S(yy,3); vy2 = NV_Ith_S(yy,7);
415
416 /* Angle of contact line */
417
418 alpha = atan2(y2-y1, x2-x1);
419 ca = cos(alpha);
420 sa = sin(alpha);
421
422 /* Normal and tangential velocities before impact
423 * (rotate velocity vectors by +alpha) */
424
425 vn1 = ca*vx1 + sa*vy1;
426 vt1 = -sa*vx1 + ca*vy1;
427
428 vn2 = ca*vx2 + sa*vy2;
429 vt2 = -sa*vx2 + ca*vy2;
430
431 /* New normal velocities (M1=M2 and COR=1.0) */
432
433 vn1_ = vn2;
434 vn2_ = vn1;
435
436 vn1 = vn1_;
437 vn2 = vn2_;
438
439 /* Velocities after impact (rotate back by -alpha) */
440
441 vx1 = ca*vn1 - sa*vt1;
442 vy1 = sa*vn1 + ca*vt1;
443
444 vx2 = ca*vn2 - sa*vt2;
445 vy2 = sa*vn2 + ca*vt2;
446
447 NV_Ith_S(yy,2) = vx1; NV_Ith_S(yy,6) = vx2;
448 NV_Ith_S(yy,3) = vy1; NV_Ith_S(yy,7) = vy2;
449
450 return;
451 }
452
PrintFinalStats(void * cpode_mem)453 static void PrintFinalStats(void *cpode_mem)
454 {
455 realtype h0u;
456 long int nst, nfe, nsetups, nje, nfeLS, nni, ncfn, netf;
457 long int nproj, nce, nsetupsP, nprf;
458 int flag;
459
460 flag = CPodeGetActualInitStep(cpode_mem, &h0u);
461 flag = CPodeGetNumSteps(cpode_mem, &nst);
462 flag = CPodeGetNumFctEvals(cpode_mem, &nfe);
463 flag = CPodeGetNumLinSolvSetups(cpode_mem, &nsetups);
464 flag = CPodeGetNumErrTestFails(cpode_mem, &netf);
465 flag = CPodeGetNumNonlinSolvIters(cpode_mem, &nni);
466 flag = CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
467
468 flag = CPDlsGetNumJacEvals(cpode_mem, &nje);
469 flag = CPDlsGetNumFctEvals(cpode_mem, &nfeLS);
470
471 flag = CPodeGetProjStats(cpode_mem, &nproj, &nce, &nsetupsP, &nprf);
472
473 printf("\nFinal Statistics:\n");
474 printf("h0u = %g\n",h0u);
475 printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld\n",
476 nst, nfe, nsetups);
477 printf("nfeLS = %-6ld nje = %ld\n",
478 nfeLS, nje);
479 printf("nni = %-6ld ncfn = %-6ld netf = %-6ld \n",
480 nni, ncfn, netf);
481 printf("nproj = %-6ld nce = %-6ld nsetupsP = %-6ld nprf = %-6ld\n",
482 nproj, nce, nsetupsP, nprf);
483 }
484
485