MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
dae-intg.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/utils/dae-intg.cc,v 1.53 2017/01/12 15:10:27 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2017
7  *
8  * Pierangelo Masarati <masarati@aero.polimi.it>
9  *
10  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
11  * via La Masa, 34 - 20156 Milano, Italy
12  * http://www.aero.polimi.it
13  *
14  * Changing this copyright notice is forbidden.
15  *
16  * This program is free software; you can redistribute it and/or modify
17  * it under the terms of the GNU General Public License as published by
18  * the Free Software Foundation (version 2 of the License).
19  *
20  *
21  * This program is distributed in the hope that it will be useful,
22  * but WITHOUT ANY WARRANTY; without even the implied warranty of
23  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
24  * GNU General Public License for more details.
25  *
26  * You should have received a copy of the GNU General Public License
27  * along with this program; if not, write to the Free Software
28  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
29  */
30 
31 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
32 
33 #ifdef USE_RUNTIME_LOADING
34 
35 #include <cstring>
36 #include <cstdio>
37 #include <ltdl.h>
38 
39 #include "ac/getopt.h"
40 #include "myassert.h"
41 #include "solman.h"
42 #include "fullmh.h"
43 #include "linsol.h"
44 
45 #include "dae-intg.h"
46 
47 enum method_t {
48  METHOD_UNKNOWN,
49  METHOD_MULTISTEP,
50  METHOD_HOPE,
51  METHOD_CUBIC,
52  METHOD_RADAU_II,
53  METHOD_CRANK_NICOLSON
54 };
55 
56 struct integration_data {
57  doublereal ti;
58  doublereal tf;
59  doublereal dt;
60  doublereal tol;
61  int maxiter;
62  doublereal rho;
63 };
64 
65 static int method_multistep(const char*, integration_data*, void*, const char*);
66 static int method_hope(const char*, integration_data*, void*, const char*);
67 static int method_cubic(const char*, integration_data*, void*, const char*);
68 static int method_radau_II(const char*, integration_data*, void*, const char*);
69 static int method_cn(const char*, integration_data*, void*, const char*);
70 
71 static void* get_method_data(method_t, const char*);
72 
73 static int open_module(const char* module);
74 
75 static struct funcs *ff = NULL;
76 static bool print_help = false;
77 static void* p_data = NULL;
78 
79 int
80 main(int argc, char *const argv[])
81 {
82  struct s_method {
83  const char* s;
84  method_t m;
85  } s_m[] = {
86  { "ms", METHOD_MULTISTEP },
87  { "hope", METHOD_HOPE },
88  { "cubic", METHOD_CUBIC },
89  { "radau-II", METHOD_RADAU_II },
90  { "crank-nicolson", METHOD_CRANK_NICOLSON },
91 
92  { NULL, METHOD_UNKNOWN }
93  };
94  method_t curr_method = METHOD_UNKNOWN;
95  const char* module = "intg-default.so";
96  char* user_defined = NULL;
97  void* method_data = NULL;
98  integration_data d = { 0., 1., 1.e-3, 1.e-6, 10 };
99 
100  /* opzioni */
101  const char optstring[] = "i:lm:t:T:n:r:u:hH";
102  const struct option longopts[] = {
103  { "integrator", required_argument, NULL, int('i') },
104  { "method-data", required_argument, NULL, int('m') },
105  { "timing", required_argument, NULL, int('t') },
106  { "tolerance", required_argument, NULL, int('T') },
107  { "iterations", required_argument, NULL, int('n') },
108  { "rho", required_argument, NULL, int('r') },
109  { "user-defined", required_argument, NULL, int('u') },
110  { "help", no_argument, NULL, int('h') },
111  { "print-help", no_argument, NULL, int('H') },
112 
113  { NULL, no_argument, NULL, int(0) }
114  };
115 
116  while (true) {
117  int curropt;
118 
119  curropt = getopt_long(argc, argv, optstring, longopts, NULL);
120 
121  if (curropt == EOF) {
122  break;
123  }
124 
125  switch(curropt) {
126  case int('?'):
127  /*
128  * std::cerr << "unknown option -" << char(optopt) << std::endl;
129  */
130  break;
131 
132  case int(':'):
133  std::cerr << "missing parameter for option -"
134  << optopt << std::endl;
135  break;
136 
137  case int('h'):
138  std::cerr << "usage: int -[imtTnruh] [module]"
139  << std::endl
140  << std::endl
141  << " -i,--integrator : integration method"
142  << std::endl
143  << " -l : list available methods"
144  << std::endl
145  << " -m,--method-data : method-dependent data"
146  << std::endl
147  << " -t,--timing : ti:dt:tf" << std::endl
148  << " -T,--tolerance" << std::endl
149  << " -n,--maxiter" << std::endl
150  << " -r,--rho : asymptotic radius"
151  << std::endl
152  << " -u,--user : user-defined data"
153  << std::endl
154  << std::endl
155  << " -h,--help : print this message "
156  "and exit" << std::endl
157 
158  << " -H,--print-help : print module's "
159  "help message" << std::endl;
160  exit(EXIT_SUCCESS);
161 
162  case int('H'):
163  print_help = true;
164  break;
165 
166  case int('i'): {
167  s_method* p = s_m;
168  while (p->s != NULL) {
169  if (strcmp(p->s, optarg) == 0) {
170  curr_method = p->m;
171  break;
172  }
173  p++;
174  }
175  if (p->s == NULL) {
176  std::cerr << "unknown integrator "
177  << optarg << std::endl;
178  exit(EXIT_FAILURE);
179  }
180  break;
181  }
182 
183  case 'l':
184  for (int i = 0; s_m[i].s; i++) {
185  std::cout << " " << s_m[i].s << std::endl;
186  }
187  break;
188 
189  case int('m'):
190  method_data = get_method_data(curr_method, optarg);
191  break;
192 
193  case int('t'): {
194  char *next = optarg;
195 
196  d.ti = strtod(next, &next);
197  if (next[0] != ':') {
198  std::cerr << "syntax: ti:dt:tf" << std::endl;
199  exit(EXIT_FAILURE);
200  }
201 
202  next++;
203  d.dt = strtod(next, &next);
204  if (next[0] != ':') {
205  std::cerr << "syntax: ti:dt:tf" << std::endl;
206  exit(EXIT_FAILURE);
207  }
208 
209  next++;
210  d.tf = strtod(next, &next);
211  if (next[0] != '\0') {
212  std::cerr << "syntax: ti:dt:tf" << std::endl;
213  exit(EXIT_FAILURE);
214  }
215 
216  break;
217  }
218 
219  case int ('T'):
220  d.tol = strtod(optarg, NULL);
221  break;
222 
223  case int ('n'):
224  d.maxiter = strtol(optarg, NULL, 10);
225  break;
226 
227  case int ('r'):
228  d.rho = strtod(optarg, NULL);
229  break;
230 
231  case int('u'):
232  user_defined = optarg;
233  break;
234 
235  default:
236  /* std::cerr << "unknown option" << std::endl; */
237  break;
238  }
239  }
240 
241  if (optind >= argc) {
242  std::cerr << "need a module" << std::endl;
243  exit(EXIT_FAILURE);
244  }
245 
246  module = argv[optind];
247 
248  open_module(module);
249 
250  if (::ff->size == 0) {
251  std::cerr << "no \"size\" func in module" << std::endl;
252  exit(EXIT_FAILURE);
253  }
254 
255  if (::ff->func == 0) {
256  std::cerr << "no \"func\" func in module" << std::endl;
257  exit(EXIT_FAILURE);
258  }
259 
260  if (::ff->grad == 0) {
261  std::cerr << "no \"grad\" func in module" << std::endl;
262  exit(EXIT_FAILURE);
263  }
264 
265  if (::ff->read && ::ff->read(&p_data, user_defined)) {
266  std::cerr << "unable to read(" << user_defined << ") "
267  "data for module \"" << module << "\"" << std::endl;
268  exit(EXIT_FAILURE);
269  }
270 
271  if (print_help) {
272  if (::ff->help) {
273  ::ff->help(p_data, std::cerr);
274  } else {
275  std::cerr << "help not available for module "
276  "\"" << module << "\" (ignored)" << std::endl;
277  }
278  }
279 
280  int rc = 0;
281  switch (curr_method) {
282  case METHOD_MULTISTEP :
283  rc = method_multistep(module, &d, method_data, user_defined);
284  break;
285  case METHOD_HOPE:
286  rc = method_hope(module, &d, method_data, user_defined);
287  break;
288  case METHOD_CUBIC:
289  rc = method_cubic(module, &d, method_data, user_defined);
290  break;
291  case METHOD_RADAU_II:
292  rc = method_radau_II(module, &d, method_data, user_defined);
293  break;
294  case METHOD_CRANK_NICOLSON:
295  rc = method_cn(module, &d, method_data, user_defined);
296  break;
297  default:
298  std::cerr << "you must select an integration method" << std::endl;
299  exit(EXIT_FAILURE);
300  }
301 
302  if (lt_dlexit()) {
303  std::cerr << "lt_dlexit() failed" << std::endl;
304  exit(EXIT_FAILURE);
305  }
306 
307  return 0;
308 }
309 
310 static void*
311 get_method_data(method_t curr_method, const char* optarg)
312 {
313  switch (curr_method) {
314  case METHOD_CUBIC:
315  case METHOD_RADAU_II:
316  if (strcasecmp(optarg, "linear") == 0) {
317  bool *pi = new bool;
318  *pi = true;
319  return pi;
320 
321  } else if (strcasecmp(optarg, "cubic") == 0) {
322  bool *pi = new bool;
323  *pi = false;
324  return pi;
325 
326  } else {
327  std::cerr << "unknown data \"" << optarg << "\""
328  << std::endl;
329  return NULL;
330  }
331  break;
332 
333  default:
334  std::cerr << "not implemented yet" << std::endl;
335  exit(EXIT_FAILURE);
336  }
337 
338  return NULL;
339 }
340 
341 static int
342 open_module(const char* module)
343 {
344  const char* err = NULL;
345 
346  lt_dlhandle handle;
347 
348  if (lt_dlinit()) {
349  std::cerr << "lt_dlinit() failed" << std::endl;
350  exit(EXIT_FAILURE);
351  }
352 
353  if ((handle = lt_dlopen(module)) == NULL) {
354  err = lt_dlerror();
355  std::cerr << "lt_dlopen(\"" << module
356  << "\") returned \"" << err << "\"" << std::endl;
357  exit(EXIT_FAILURE);
358  }
359 
360  struct funcs **pf = (struct funcs **)lt_dlsym(handle, "ff");
361  if (pf == NULL) {
362  err = lt_dlerror();
363  std::cerr << "lt_dlsym(\"ff\") returned \"" << err << "\""
364  << std::endl;
365  exit(EXIT_FAILURE);
366  }
367 
368  ::ff = *pf;
369  if (::ff == NULL) {
370  std::cerr << "invalid \"ff\" symbol in \"" << module << "\""
371  << std::endl;
372  exit(EXIT_FAILURE);
373  }
374 
375  return 0;
376 }
377 
378 static void
379 flip(VectorHandler** ppX, VectorHandler** ppXP,
380  VectorHandler** ppXm1, VectorHandler** ppXPm1,
381  VectorHandler** ppXm2, VectorHandler** ppXPm2)
382 {
383  VectorHandler* p = *ppXm2;
384  *ppXm2 = *ppXm1;
385  *ppXm1 = *ppX;
386  *ppX = p;
387 
388  p = *ppXPm2;
389  *ppXPm2 = *ppXPm1;
390  *ppXPm1 = *ppXP;
391  *ppXP = p;
392 }
393 
394 static doublereal
395 cm0(const doublereal& xi)
396 {
397  return 1 - 3*xi*xi - 2*xi*xi*xi;
398 }
399 
400 static doublereal
401 cm0p(const doublereal& xi)
402 {
403  return - 6.*xi - 6.*xi*xi;
404 }
405 
406 static doublereal
407 cm1(const doublereal& xi)
408 {
409  return 3.*xi*xi + 2.*xi*xi*xi;
410 }
411 
412 static doublereal
413 cm1p(const doublereal& xi)
414 {
415  return 6.*xi + 6.*xi*xi;
416 }
417 
418 static doublereal
419 cn0(const doublereal& xi)
420 {
421  return xi + 2.*xi*xi + xi*xi*xi;
422 }
423 
424 static doublereal
425 cn0p(const doublereal& xi)
426 {
427  return 1. + 4.*xi + 3.*xi*xi;
428 }
429 
430 static doublereal
431 cn1(const doublereal& xi)
432 {
433  return xi*xi + xi*xi*xi;
434 }
435 
436 static doublereal
437 cn1p(const doublereal& xi)
438 {
439  return 2.*xi + 3.*xi*xi;
440 }
441 
442 static int
443 method_multistep(const char* module, integration_data* d,
444  void* method_data, const char* user_defined)
445 {
446  /* prepara le strutture dati per il calcolo */
447  int size = ::ff->size(p_data);
448  MyVectorHandler v0(size);
449  MyVectorHandler v1(size);
450  MyVectorHandler v2(size);
451  MyVectorHandler v3(size);
452  MyVectorHandler v4(size);
453  MyVectorHandler v5(size);
454 
455  VectorHandler* pX = &v0;
456  VectorHandler* pXP = &v1;
457  VectorHandler* pXm1 = &v2;
458  VectorHandler* pXPm1 = &v3;
459  VectorHandler* pXm2 = &v4;
460  VectorHandler* pXPm2 = &v5;
461  pX->Reset();
462  pXP->Reset();
463  pXm1->Reset();
464  pXPm1->Reset();
465  pXm2->Reset();
466  pXPm2->Reset();
467 
468  doublereal* pd = new doublereal[2 * size*size];
469  doublereal** ppd = new doublereal*[2 * size];
470  FullMatrixHandler J(pd, ppd, size*size, size, size);
471  FullMatrixHandler JP(pd+size*size, ppd+size, size*size, size, size);
472  MyVectorHandler R(size);
473 
474  // TODO: abstract from a specific solution manager?
475  LinSol ls;
476  SolutionManager *sm = ls.GetSolutionManager(size);
477 
478  MatrixHandler* Jac = sm->pMatHdl();
479  VectorHandler* Res = sm->pResHdl();
480  VectorHandler* Sol = sm->pSolHdl();
481 
482  /* parametri di integrazione */
483  doublereal ti = d->ti;
484  doublereal dt = d->dt;
485  doublereal tf = d->tf;
486 
487  doublereal tol = d->tol;
488  int maxiter = d->maxiter;
489 
490  /* coefficienti del metodo */
491  doublereal rho = d->rho;
492  doublereal beta =
493  (4.*rho*rho-(1.-rho)*(1.-rho))/(4.-(1.-rho)*(1.-rho));
494  doublereal delta = (1.-rho)*(1.-rho)/(2.*(4.-(1.-rho)*(1.-rho)));
495  doublereal a1 = 1.-beta;
496  doublereal a2 = beta;
497  doublereal b0 = delta+.5;
498  doublereal b1 = .5*beta+.5-2.*delta;
499  doublereal b2 = .5*beta+delta;
500 
501  doublereal t = ti;
502 
503  /* inizializza la soluzione */
504  if (::ff->init) {
505  ::ff->init(p_data, *pX, *pXP);
506  }
507  for (int k = 1; k <= size; k++) {
508  doublereal x = (*pX)(k);
509  doublereal xp = (*pXP)(k);
510  (*pXPm1)(k) = xp;
511  (*pXm1)(k) = x-dt*xp;
512  }
513 
514  /* output iniziale */
515  std::cout << ti << " " << 0. << " ";
516  if (::ff->out) {
517  ::ff->out(p_data, std::cout, *pX, *pXP) << std::endl;
518  }
519 
520  flip(&pX, &pXP, &pXm1, &pXPm1, &pXm2, &pXPm2);
521 
522  while (t < tf) {
523  t += dt;
524 
525  /* predict */
526  for (int ir = size; ir > 0; ir--) {
527  doublereal xm1 = (*pXm1)(ir);
528  doublereal xPm1 = (*pXPm1)(ir);
529  doublereal xm2 = (*pXm2)(ir);
530  doublereal xPm2 = (*pXPm2)(ir);
531  doublereal xP = cm0p(1.)*xm1
532  + cm1p(1.)*xm2
533  + dt*(cn0p(1.)*xPm1 + cn1p(1.)*xPm2);
534  doublereal x = a1*xm1
535  + a2*xm2
536  + dt*(b0*xP + b1*xPm1 + b2*xPm2);
537  (*pX)(ir) = x;
538  (*pXP)(ir) = xP;
539  }
540 
541  /* test */
542  int j = 0;
543  doublereal test;
544  doublereal coef = dt*b0;
545  do {
546  Jac = sm->pMatHdl();
547  Res = sm->pResHdl();
548  Sol = sm->pSolHdl();
549 
550  ::ff->func(p_data, *Res, *pX, *pXP, t);
551 
552  test = Res->Norm();
553  std::cerr << j << " " << test << std::endl;
554  if (test < tol) {
555  break;
556  }
557  if (++j > maxiter) {
558  std::cerr << "current iteration " << j
559  << " exceeds max iteration number "
560  << maxiter << std::endl;
561  exit(EXIT_FAILURE);
562  }
563 
564  /* correct */
565  sm->MatrReset();
566  J.Reset();
567  JP.Reset();
568  ::ff->grad(p_data, J, JP, *pX, *pXP, t);
569  for (int ir = 1; ir <= size; ir++) {
570  for (int ic = 1; ic <= size; ic++) {
571  (*Jac)(ir, ic) = JP(ir, ic)
572  + coef*J(ir, ic);
573  }
574  }
575  sm->Solve();
576 
577  /* update */
578  for (int ir = size; ir > 0; ir--) {
579  doublereal dxP = (*Sol)(ir);
580  (*pXP)(ir) += dxP;
581  (*pX)(ir) += coef*dxP;
582  }
583  } while (true);
584 
585  /* output */
586  std::cout << t << " " << test << " ";
587  if (::ff->out) {
588  ::ff->out(p_data, std::cout, *pX, *pXP) << std::endl;
589  }
590 
591  flip(&pX, &pXP, &pXm1, &pXPm1, &pXm2, &pXPm2);
592  }
593 
594  if (::ff->destroy) {
595  ::ff->destroy(&p_data);
596  }
597 
598  delete[] pd;
599  delete[] ppd;
600 
601  return 0;
602 }
603 
604 static int
605 method_hope(const char* module, integration_data* d,
606  void* method_data, const char* user_defined)
607 {
608  std::cerr << __FUNCTION__ << "not implemented yet!" << std::endl;
609  exit(EXIT_FAILURE);
610 
611  return 0;
612 }
613 
614 static int
615 method_cubic(const char* module, integration_data* d,
616  void* method_data, const char* user_defined)
617 {
618  bool linear = false;
619 
620  if (method_data) {
621  bool *pi = (bool *)method_data;
622 
623  linear = *pi;
624  }
625 
626  /* prepara le strutture dati per il calcolo */
627  int size = ::ff->size(p_data);
628  MyVectorHandler v0(size);
629  MyVectorHandler v1(size);
630  MyVectorHandler v2(size);
631  MyVectorHandler v3(size);
632  MyVectorHandler v4(size);
633  MyVectorHandler v5(size);
634 
635  VectorHandler* pX = &v0;
636  VectorHandler* pXP = &v1;
637  VectorHandler* pXm1 = &v2;
638  VectorHandler* pXPm1 = &v3;
639  VectorHandler* pXm2 = &v4;
640  VectorHandler* pXPm2 = &v5;
641  pX->Reset();
642  pXP->Reset();
643  pXm1->Reset();
644  pXPm1->Reset();
645  pXm2->Reset();
646  pXPm2->Reset();
647 
648  doublereal* pd = new doublereal[4*size*size];
649  doublereal** ppd = new doublereal*[4*size];
650  FullMatrixHandler Jz(pd, ppd, size*size, size, size);
651  FullMatrixHandler J0(pd+size*size, ppd+size, size*size, size, size);
652  FullMatrixHandler JPz(pd+2*size*size, ppd+2*size,
653  size*size, size, size);
654  FullMatrixHandler JP0(pd+3*size*size, ppd+3*size,
655  size*size, size, size);
656  MyVectorHandler Xz(size);
657  MyVectorHandler XPz(size);
658 
659  // TODO: abstract from a specific solution manager?
660  LinSol ls;
661  SolutionManager *sm = ls.GetSolutionManager(2*size);
662 
663  MatrixHandler* Jac = sm->pMatHdl();
664  VectorHandler* Res = sm->pResHdl();
665  VectorHandler* Sol = sm->pSolHdl();
666 
667  MyVectorHandler Resz(size, Res->pdGetVec() + size);
668 
669  /* parametri di integrazione */
670  doublereal ti = d->ti;
671  doublereal dt = d->dt;
672  doublereal tf = d->tf;
673 
674  doublereal tol = d->tol;
675  int maxiter = d->maxiter;
676 
677  /* coefficienti del metodo */
678  doublereal rho = d->rho;
679  doublereal z = -rho/(1.+rho);
680  doublereal w1 = (2.+3.*z)/(6.*(1.+z));
681  doublereal wz = -1./(6.*z*(1.+z));
682  doublereal w0 = (1.+3.*z)/(6.*z);
683 
684  doublereal jzz = (1.+3.*rho)/(6.*rho*(1.+rho))*dt;
685  doublereal jz0 = -1./(6.*rho*(1.+rho)*(1.+rho))*dt;
686  doublereal j0z = (1.+rho)*(1.+rho)/(6.*rho)*dt;
687  doublereal j00 = (2.*rho-1.)/(6.*rho)*dt;
688 
689  doublereal t = ti;
690 
691  /* inizializza la soluzione */
692  if (::ff->init) {
693  ::ff->init(p_data, *pX, *pXP);
694  }
695  ::ff->func(p_data, *Res, *pX, *pXP, t);
696  for (int k = 1; k <= size; k++) {
697  doublereal x = (*pX)(k);
698  doublereal xp = (*pXP)(k);
699  (*pXPm1)(k) = xp;
700  (*pXm1)(k) = x - dt*xp;
701  }
702 
703  /* output iniziale */
704  std::cout << ti << " " << 0. << " ";
705  if (::ff->out) {
706  ::ff->out(p_data, std::cout, *pX, *pXP) << std::endl;
707  }
708 
709  flip(&pX, &pXP, &pXm1, &pXPm1, &pXm2, &pXPm2);
710 
711  while (t < tf) {
712  t += dt;
713 
714  if (linear) {
715 
716  std::cerr << "linear predictor" << std::endl;
717 
718  /* predict lineare */
719  for (int k = 1; k <= size; k++) {
720  doublereal xm1 = (*pXm1)(k);
721  doublereal xPm1 = (*pXPm1)(k);
722  doublereal xP = xPm1;
723  doublereal xPz = xPm1;
724  doublereal x = xm1 + dt*xP;
725  doublereal xz = xm1 + dt*(1.+z)*xP;
726 
727  (*pX)(k) = x;
728  (*pXP)(k) = xP;
729  Xz(k) = xz;
730  XPz(k) = xPz;
731  }
732 
733  } else {
734 
735  /* predict cubico */
736  for (int k = 1; k <= size; k++) {
737  doublereal xm1 = (*pXm1)(k);
738  doublereal xPm1 = (*pXPm1)(k);
739  doublereal xm2 = (*pXm2)(k);
740  doublereal xPm2 = (*pXPm2)(k);
741  doublereal xP = (cm0p(1.)*xm1 + cm1p(1.)*xm2)/dt
742  + cn0p(1.)*xPm1 + cn1p(1.)*xPm2;
743  doublereal xPz = (cm0p(1. + z)*xm1 + cm1p(1. + z)*xm2)/dt
744  + cn0p(1. + z)*xPm1 + cn1p(1. + z)*xPm2;
745  doublereal x = xm1 + dt*(w1*xPm1 + wz*xPz + w0*xP);
746  doublereal xz = cm0(z)*x + cm1(z)*xm1 + dt*(cn0(z)*xP + cn1(z)*xPm1);
747 
748  (*pX)(k) = x;
749  (*pXP)(k) = xP;
750  Xz(k) = xz;
751  XPz(k) = xPz;
752  }
753  }
754 
755 
756  /* test */
757  int j = 0;
758  doublereal test;
759  do {
760 
761  Jac = sm->pMatHdl();
762  Res = sm->pResHdl();
763  Sol = sm->pSolHdl();
764 
765  Resz.Attach(size, Res->pdGetVec() + size);
766 
767  Res->Reset();
768  ::ff->func(p_data, *Res, *pX, *pXP, t);
769  /* Res->Reset() zeros Resz as well */
770  ::ff->func(p_data, Resz, Xz, XPz, t + z*dt);
771 
772  /* Res->Norm() computes Resz norm as well */
773  test = Res->Norm();
774  if (test < tol) {
775  break;
776  }
777  if (++j > maxiter) {
778  std::cerr << "current iteration " << j
779  << " exceeds max iteration number "
780  << maxiter << std::endl;
781  exit(EXIT_FAILURE);
782  }
783 
784  /* correct */
785  sm->MatrReset();
786  Jz.Reset();
787  JPz.Reset();
788  J0.Reset();
789  JP0.Reset();
790  ::ff->grad(p_data, Jz, JPz, Xz, XPz, t + z*dt);
791  ::ff->grad(p_data, J0, JP0, *pX, *pXP, t);
792 
793  for (int ir = 1; ir <= size; ir++) {
794  for (int ic = 1; ic <= size; ic++) {
795  (*Jac)(ir, ic)
796  = j00*J0(ir, ic)
797  + JP0(ir, ic);
798  (*Jac)(ir, size + ic)
799  = j0z*J0(ir, ic);
800  (*Jac)(size + ir, ic)
801  = jz0*Jz(ir, ic);
802  (*Jac)(size + ir, size + ic)
803  = jzz*Jz(ir, ic)
804  + JPz(ir, ic);
805  }
806  }
807 
808  sm->Solve();
809 
810  /* update */
811  for (int ir = size; ir > 0; ir--) {
812  doublereal dxP0 = (*Sol)(ir);
813  doublereal dxPz = (*Sol)(size + ir);
814  (*pXP)(ir) += dxP0;
815  XPz(ir) += dxPz;
816  (*pX)(ir) += dt*(wz*dxPz + w0*dxP0);
817  Xz(ir) += dt*(cm0(z)*(wz*dxPz + w0*dxP0)+cn0(z)*dxP0);
818  }
819  } while (true);
820 
821  /* output */
822  std::cout << t << " " << test << " ";
823  if (::ff->out) {
824  ::ff->out(p_data, std::cout, *pX, *pXP) << std::endl;
825  }
826 
827  flip(&pX, &pXP, &pXm1, &pXPm1, &pXm2, &pXPm2);
828  }
829 
830  if (::ff->destroy) {
831  ::ff->destroy(&p_data);
832  }
833  delete[] pd;
834  delete[] ppd;
835 
836  return 0;
837 }
838 
839 static doublereal
840 radau_II_cm0(const doublereal& xi)
841 {
842  return 1 - xi*xi;
843 }
844 
845 static doublereal
846 radau_II_cm0p(const doublereal& xi)
847 {
848  return - 2.*xi;
849 }
850 
851 static doublereal
852 radau_II_cm1(const doublereal& xi)
853 {
854  return xi*xi;
855 }
856 
857 static doublereal
858 radau_II_cm1p(const doublereal& xi)
859 {
860  return 2.*xi;
861 }
862 
863 static doublereal
864 radau_II_cn0(const doublereal& xi)
865 {
866  return xi + xi*xi;
867 }
868 
869 static doublereal
870 radau_II_cn0p(const doublereal& xi)
871 {
872  return 1. + 2.*xi;
873 }
874 
875 static doublereal
876 radau_II_cn1(const doublereal& xi)
877 {
878  return 0.;
879 }
880 
881 static doublereal
882 radau_II_cn1p(const doublereal& xi)
883 {
884  return 0.;
885 }
886 
887 static int
888 method_radau_II(const char* module, integration_data* d,
889  void* method_data, const char* user_defined)
890 {
891  bool linear = false;
892 
893  if (method_data) {
894  bool *pi = (bool *)method_data;
895 
896  linear = *pi;
897  }
898 
899  /* prepara le strutture dati per il calcolo */
900  int size = ::ff->size(p_data);
901  MyVectorHandler v0(size);
902  MyVectorHandler v1(size);
903  MyVectorHandler v2(size);
904  MyVectorHandler v3(size);
905  MyVectorHandler v4(size);
906  MyVectorHandler v5(size);
907 
908  VectorHandler* pX = &v0;
909  VectorHandler* pXP = &v1;
910  VectorHandler* pXm1 = &v2;
911  VectorHandler* pXPm1 = &v3;
912  VectorHandler* pXm2 = &v4;
913  VectorHandler* pXPm2 = &v5;
914  pX->Reset();
915  pXP->Reset();
916  pXm1->Reset();
917  pXPm1->Reset();
918  pXm2->Reset();
919  pXPm2->Reset();
920 
921  doublereal* pd = new doublereal[4*size*size];
922  doublereal** ppd = new doublereal*[4*size];
923  FullMatrixHandler Jz(pd, ppd, size*size, size, size);
924  FullMatrixHandler J0(pd+size*size, ppd+size, size*size, size, size);
925  FullMatrixHandler JPz(pd+2*size*size, ppd+2*size,
926  size*size, size, size);
927  FullMatrixHandler JP0(pd+3*size*size, ppd+3*size,
928  size*size, size, size);
929  MyVectorHandler Xz(size);
930  MyVectorHandler XPz(size);
931 
932  // TODO: abstract from a specific solution manager?
933  LinSol ls;
934  SolutionManager *sm = ls.GetSolutionManager(2*size);
935 
936  MatrixHandler* Jac = sm->pMatHdl();
937  VectorHandler* Res = sm->pResHdl();
938  VectorHandler* Sol = sm->pSolHdl();
939 
940  MyVectorHandler Resz(size, Res->pdGetVec() + size);
941 
942  /* parametri di integrazione */
943  doublereal ti = d->ti;
944  doublereal dt = d->dt;
945  doublereal tf = d->tf;
946 
947  doublereal tol = d->tol;
948  int maxiter = d->maxiter;
949 
950  /* coefficienti del metodo */
951  doublereal z = -2./3.;
952  doublereal w1 = 0.;
953  doublereal wz = -1./(2.*z);
954  doublereal w0 = (1.+2.*z)/(2.*z);
955  doublereal m0 = radau_II_cm0(z);
956  doublereal m1 = radau_II_cm1(z);
957  doublereal n0 = radau_II_cn0(z);
958  doublereal n1 = radau_II_cn1(z);
959 
960  doublereal jzz = 5./12.*dt;
961  doublereal jz0 = -1./12.*dt;
962  doublereal j0z = 3./4.*dt;
963  doublereal j00 = 1./4.*dt;
964 
965  doublereal t = ti;
966 
967  /* inizializza la soluzione */
968  if (::ff->init) {
969  ::ff->init(p_data, *pX, *pXP);
970  }
971  ::ff->func(p_data, *Res, *pX, *pXP, t);
972  for (int k = 1; k <= size; k++) {
973  doublereal x = (*pX)(k);
974  doublereal xp = (*pXP)(k);
975  (*pXPm1)(k) += xp;
976  (*pXm1)(k) += x - dt*xp;
977  }
978 
979  /* output iniziale */
980  std::cout << ti << " " << 0. << " ";
981  if (::ff->out) {
982  ::ff->out(p_data, std::cout, *pX, *pXP) << std::endl;
983  }
984 
985  flip(&pX, &pXP, &pXm1, &pXPm1, &pXm2, &pXPm2);
986 
987  while (t < tf) {
988  t += dt;
989 
990  if (linear) {
991 
992  std::cerr << "linear predictor" << std::endl;
993 
994  /* predict lineare */
995  for (int k = 1; k <= size; k++) {
996  doublereal xm1 = (*pXm1)(k);
997  doublereal xPm1 = (*pXPm1)(k);
998  doublereal xP = xPm1;
999  doublereal xPz = xPm1;
1000  doublereal x = xm1 + dt*xP;
1001  doublereal xz = xm1 + dt*(1.+z)*xP;
1002 
1003  (*pX)(k) = x;
1004  (*pXP)(k) = xP;
1005  Xz(k) = xz;
1006  XPz(k) = xPz;
1007  }
1008 
1009  } else {
1010 
1011  /* predict cubico */
1012  for (int k = 1; k <= size; k++) {
1013  doublereal xm1 = (*pXm1)(k);
1014  doublereal xPm1 = (*pXPm1)(k);
1015  doublereal xm2 = (*pXm2)(k);
1016  doublereal xPm2 = (*pXPm2)(k);
1017  doublereal xP = (radau_II_cm0p(1.)*xm1 + radau_II_cm1p(1.)*xm2)/dt
1018  + radau_II_cn0p(1.)*xPm1 + radau_II_cn1p(1.)*xPm2;
1019  doublereal xPz = (radau_II_cm0p(1. + z)*xm1 + radau_II_cm1p(1. + z)*xm2)/dt
1020  + radau_II_cn0p(1. + z)*xPm1 + radau_II_cn1p(1. + z)*xPm2;
1021  doublereal x = xm1 + dt*(w1*xPm1 + wz*xPz + w0*xP);
1022  doublereal xz = m0*x + m1*xm1 + dt*(n0*xP + n1*xPm1);
1023 
1024  (*pX)(k) = x;
1025  (*pXP)(k) = xP;
1026  Xz(k) = xz;
1027  XPz(k) = xPz;
1028  }
1029  }
1030 
1031 
1032  /* test */
1033  int j = 0;
1034  doublereal test;
1035  do {
1036 
1037  Jac = sm->pMatHdl();
1038  Res = sm->pResHdl();
1039  Sol = sm->pSolHdl();
1040 
1041  Resz.Attach(size, Res->pdGetVec() + size);
1042 
1043  Res->Reset();
1044  ::ff->func(p_data, *Res, *pX, *pXP, t);
1045  /* Res->Reset() zeros Resz as well */
1046  ::ff->func(p_data, Resz, Xz, XPz, t + z*dt);
1047 
1048  /* Res->Norm() computes Resz norm as well */
1049  test = Res->Norm();
1050  if (test < tol) {
1051  break;
1052  }
1053  if (++j > maxiter) {
1054  std::cerr << "current iteration " << j
1055  << " exceeds max iteration number "
1056  << maxiter << std::endl;
1057  exit(EXIT_FAILURE);
1058  }
1059 
1060  /* correct */
1061  sm->MatrReset();
1062  Jz.Reset();
1063  JPz.Reset();
1064  J0.Reset();
1065  JP0.Reset();
1066  ::ff->grad(p_data, Jz, JPz, Xz, XPz, t + z*dt);
1067  ::ff->grad(p_data, J0, JP0, *pX, *pXP, t);
1068 
1069  for (int ir = 1; ir <= size; ir++) {
1070  for (int ic = 1; ic <= size; ic++) {
1071  (*Jac)(ir, ic)
1072  = j00*J0(ir, ic)
1073  + JP0(ir, ic);
1074  (*Jac)(ir, size + ic)
1075  = j0z*J0(ir, ic);
1076  (*Jac)(size + ir, ic)
1077  = jz0*Jz(ir, ic);
1078  (*Jac)(size + ir, size + ic)
1079  = jzz*Jz(ir, ic)
1080  + JPz(ir, ic);
1081  }
1082  }
1083 
1084  sm->Solve();
1085 
1086  /* update */
1087  for (int ir = size; ir > 0; ir--) {
1088  doublereal dxP0 = (*Sol)(ir);
1089  doublereal dxPz = (*Sol)(size + ir);
1090  (*pXP)(ir) += dxP0;
1091  XPz(ir) += dxPz;
1092  (*pX)(ir) += dt*(wz*dxPz + w0*dxP0);
1093  Xz(ir) += dt*(m0*(wz*dxPz + w0*dxP0)+n0*dxP0);
1094  }
1095  } while (true);
1096 
1097  /* output */
1098  std::cout << t << " " << test << " ";
1099  if (::ff->out) {
1100  ::ff->out(p_data, std::cout, *pX, *pXP) << std::endl;
1101  }
1102 
1103  flip(&pX, &pXP, &pXm1, &pXPm1, &pXm2, &pXPm2);
1104  }
1105 
1106  if (::ff->destroy) {
1107  ::ff->destroy(&p_data);
1108  }
1109  delete[] pd;
1110  delete[] ppd;
1111 
1112  return 0;
1113 }
1114 
1115 static int
1116 method_cn(const char* module, integration_data* d,
1117  void* method_data, const char* user_defined)
1118 {
1119  std::cerr << __FUNCTION__ << "not implemented yet!" << std::endl;
1120  exit(EXIT_FAILURE);
1121 
1122  return 0;
1123 }
1124 
1125 #else // ! USE_RUNTIME_LOADING
1126 
1127 #include <iostream>
1128 #include <stdlib.h>
1129 
1130 int
1131 main(void)
1132 {
1133  std::cerr << "Need dynamic load capabilities" << std::endl;
1134  exit(EXIT_FAILURE);
1135 }
1136 
1137 #endif // ! USE_RUNTIME_LOADING
1138 
VectorHandler::Reset
virtual void Reset(void)=0
SolutionManager::pResHdl
virtual VectorHandler * pResHdl(void) const =0
optopt
int optopt
Definition: getopt.c:73
optind
int optind
Definition: getopt.c:72
VectorHandler::pdGetVec
virtual doublereal * pdGetVec(void) const =0
funcs::init
pinit_f * init
Definition: dae-intg.h:53
funcs::destroy
pdestroy_f * destroy
Definition: dae-intg.h:58
main
int main(void)
Definition: dae-intg.cc:1131
bool
int bool
Definition: mbdyn.h:74
LinSol::GetSolutionManager
SolutionManager *const GetSolutionManager(integer iNLD, integer iLWS=0) const
Definition: linsol.cc:455
funcs::func
pfunc_f * func
Definition: dae-intg.h:56
VectorHandler::Norm
virtual doublereal Norm(void) const
Definition: vh.cc:262
LinSol
Definition: linsol.h:39
funcs::help
phelp_f * help
Definition: dae-intg.h:52
funcs::size
psize_f * size
Definition: dae-intg.h:54
SolutionManager::pMatHdl
virtual MatrixHandler * pMatHdl(void) const =0
true
Definition: mbdyn.h:77
SolutionManager::MatrReset
virtual void MatrReset(void)=0
SolutionManager::Solve
virtual void Solve(void)=0
funcs::read
pread_f * read
Definition: dae-intg.h:51
funcs::out
pout_f * out
Definition: dae-intg.h:57
optarg
char * optarg
Definition: getopt.c:74
SolutionManager::pSolHdl
virtual VectorHandler * pSolHdl(void) const =0
v0
static const std::vector< doublereal > v0
Definition: fixedstep.cc:45
doublereal
double doublereal
Definition: colamd.c:52
funcs
Definition: dae-intg.h:50
funcs::grad
pgrad_f * grad
Definition: dae-intg.h:55
R
Mat3x3 R
Definition: mbdyn_util_oct.cc:74