MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
intg.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/utils/intg.cc,v 1.41 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 <cstdlib>
36 #include <cstdio>
37 #include <cstring>
38 #include <ltdl.h>
39 
40 #include "ac/getopt.h"
41 #include <iostream>
42 
43 #include "myassert.h"
44 #include "solman.h"
45 #include "linsol.h"
46 #include "fullmh.h"
47 #include "harwrap.h"
48 #include "mschwrap.h"
49 
50 #include "intg.h"
51 
52 struct integration_data {
53  doublereal ti;
54  doublereal tf;
55  doublereal dt;
56  doublereal tol;
57  int maxiter;
58  doublereal rho;
59 };
60 
61 static int open_module(const char* module);
62 
63 static int method_multistep(const char*, integration_data*, void*, const char*);
64 static int method_hope(const char*, integration_data*, void*, const char*);
65 static int method_cubic(const char*, integration_data*, void*, const char*);
66 static int method_cn(const char*, integration_data*, void*, const char*);
67 
68 void* get_method_data(int, const char*);
69 
70 static struct funcs *ff = NULL;
71 static bool print_help = false;
72 static void* p_data = NULL;
73 
74 int
75 main(int argn, char *const argv[])
76 {
77  enum {
78  METHOD_UNKNOWN,
79  METHOD_MULTISTEP,
80  METHOD_HOPE,
81  METHOD_CUBIC,
82  METHOD_CRANK_NICOLSON
83  };
84 
85  struct s_method {
86  const char* s;
87  int m;
88  } s_m[] = {
89  { "ms", METHOD_MULTISTEP },
90  { "hope", METHOD_HOPE },
91  { "cubic", METHOD_CUBIC },
92  { "crank-nicolson", METHOD_CRANK_NICOLSON },
93 
94  { 0, METHOD_UNKNOWN }
95  };
96  int curr_method = METHOD_UNKNOWN;
97  char* module = 0;
98  char* user_defined = 0;
99  void* method_data = 0;
100  integration_data d = { 0., 1., 1.e-3, 1.e-6, 10 };
101 
102  /* opzioni */
103  const char optstring[] = "i:m:t:T:n:r:u:h";
104  const struct option longopts[] = {
105  { "integrator", required_argument, 0, int('i') },
106  { "method-data", required_argument, 0, int('m') },
107  { "timing", required_argument, 0, int('t') },
108  { "tolerance", required_argument, 0, int('T') },
109  { "iterations", required_argument, 0, int('n') },
110  { "rho", required_argument, 0, int('r') },
111  { "user-defined", required_argument, 0, int('u') },
112  { "help", no_argument, 0, int('h') },
113  { "print-help", no_argument, 0, int('H') },
114 
115  { 0, no_argument, 0, int(0) }
116  };
117 
118  while (true) {
119  int curropt;
120 
121  curropt = getopt_long(argn, argv, optstring, longopts, 0);
122 
123  if (curropt == EOF) {
124  break;
125  }
126 
127  switch(curropt) {
128  case int('?'):
129  /*
130  * std::cerr << "unknown option -" << char(optopt) << std::endl;
131  */
132  break;
133 
134  case int(':'):
135  std::cerr << "missing parameter for option -"
136  << optopt << std::endl;
137  break;
138 
139  case int('h'):
140  std::cerr << "usage: int -[imtTnruh] [module]" << std::endl
141  << std::endl
142  << " -i,--integrator : integration method" << std::endl
143  << " -m,--method-data : method-dependent data" << std::endl
144  << " -t,--timing : ti:dt:tf" << std::endl
145  << " -T,--tolerance" << std::endl
146  << " -n,--maxiter" << std::endl
147  << " -r,--rho : asymptotic radius" << std::endl
148  << " -u,--user : user-defined data" << std::endl
149  << std::endl
150  << " -h,--help : print this message and exit" << std::endl;
151  exit(EXIT_SUCCESS);
152 
153  case int('H'):
154  print_help = true;
155  break;
156 
157  case int('i'): {
158  s_method* p = s_m;
159  while (p->s != 0) {
160  if (strcmp(p->s, optarg) == 0) {
161  curr_method = p->m;
162  break;
163  }
164  p++;
165  }
166  if (p->s == 0) {
167  std::cerr << "unknown integrator "
168  << optarg << std::endl;
169  exit(EXIT_FAILURE);
170  }
171  break;
172  }
173 
174  case int('m'):
175  method_data = get_method_data(curr_method, optarg);
176  break;
177 
178  case int('t'): {
179  char* s = new char[strlen(optarg)+1];
180  strcpy(s, optarg);
181  char* p = std::strrchr(s, ':');
182  if (p == 0) {
183  std::cerr << "syntax: ti:dt:tf" << std::endl;
184  exit(EXIT_FAILURE);
185  }
186  d.tf = atof(p+1);
187  p[0] = '\0';
188  p = std::strrchr(s, ':');
189  if (p == 0) {
190  std::cerr << "syntax: ti:dt:tf" << std::endl;
191  exit(EXIT_FAILURE);
192  }
193  d.dt = atof(p+1);
194  p[0] = '\0';
195  d.ti = atof(s);
196  delete[] s;
197  break;
198  }
199 
200  case int ('T'):
201  d.tol = atof(optarg);
202  break;
203 
204  case int ('n'):
205  d.maxiter = atoi(optarg);
206  break;
207 
208  case int ('r'):
209  d.rho = atof(optarg);
210  break;
211 
212  case int('u'):
213  user_defined = optarg;
214  break;
215 
216  default:
217  /* std::cerr << "unknown option" << std::endl; */
218  break;
219  }
220  }
221 
222  module = argv[optind];
223 
224  open_module(module);
225 
226  if (::ff->size == 0) {
227  std::cerr << "no \"size\" func in module" << std::endl;
228  exit(EXIT_FAILURE);
229  }
230 
231  if (::ff->func == 0) {
232  std::cerr << "no \"func\" func in module" << std::endl;
233  exit(EXIT_FAILURE);
234  }
235 
236  if (::ff->grad == 0) {
237  std::cerr << "no \"grad\" func in module" << std::endl;
238  exit(EXIT_FAILURE);
239  }
240 
241  if (::ff->read && ::ff->read(&p_data, user_defined)) {
242  std::cerr << "unable to read(" << user_defined << ") "
243  "data for module \"" << module << "\"" << std::endl;
244  exit(EXIT_FAILURE);
245  }
246 
247  if (print_help) {
248  if (::ff->help) {
249  ::ff->help(p_data, std::cerr);
250  } else {
251  std::cerr << "help not available for module "
252  "\"" << module << "\" (ignored)" << std::endl;
253  }
254  }
255 
256  int rc = 0;
257  switch (curr_method) {
258  case METHOD_MULTISTEP :
259  rc = method_multistep(module, &d, method_data, user_defined);
260  break;
261  case METHOD_HOPE:
262  rc = method_hope(module, &d, method_data, user_defined);
263  break;
264  case METHOD_CUBIC:
265  rc = method_cubic(module, &d, method_data, user_defined);
266  break;
267  case METHOD_CRANK_NICOLSON:
268  rc = method_cn(module, &d, method_data, user_defined);
269  break;
270  default:
271  std::cerr << "you must select an integration method" << std::endl;
272  exit(EXIT_FAILURE);
273  }
274 
275  return 0;
276 }
277 
278 void *
279 get_method_data(int curr_method, const char* optarg)
280 {
281  switch (curr_method) {
282  default:
283  std::cerr << "not implemented yet" << std::endl;
284  exit(EXIT_FAILURE);
285  }
286 
287  return 0;
288 }
289 
290 static int
291 open_module(const char* module)
292 {
293  const char* err = 0;
294 
295  lt_dlhandle handle;
296 
297  if (lt_dlinit()) {
298  std::cerr << "lt_dlinit() failed" << std::endl;
299  exit(EXIT_FAILURE);
300  }
301 
302  handle = lt_dlopen(module);
303  if (handle == 0) {
304  err = lt_dlerror();
305  std::cerr << "lt_dlopen(\"" << module
306  << "\") returned \"" << err << "\"" << std::endl;
307  exit(EXIT_FAILURE);
308  }
309 
310  struct funcs **pf = (struct funcs **)lt_dlsym(handle, "ff");
311  if (pf == 0) {
312  err = lt_dlerror();
313  std::cerr << "lt_dlsym(\"ff\") returned \"" << err << "\""
314  << std::endl;
315  exit(EXIT_FAILURE);
316  }
317 
318  ::ff = *pf;
319  if (::ff == 0) {
320  std::cerr << "invalid \"ff\" symbol in \"" << module << "\""
321  << std::endl;
322  exit(EXIT_FAILURE);
323  }
324 
325  return 0;
326 }
327 
328 static void
329 flip(VectorHandler** ppX, VectorHandler** ppXP,
330  VectorHandler** ppXm1, VectorHandler** ppXPm1,
331  VectorHandler** ppXm2, VectorHandler** ppXPm2)
332 {
333  VectorHandler* p = *ppXm2;
334  *ppXm2 = *ppXm1;
335  *ppXm1 = *ppX;
336  *ppX = p;
337 
338  p = *ppXPm2;
339  *ppXPm2 = *ppXPm1;
340  *ppXPm1 = *ppXP;
341  *ppXP = p;
342 }
343 
344 static int
345 method_multistep(const char* module, integration_data* d,
346  void* method_data, const char* user_defined)
347 {
348  // prepara i dati
349  void* p_data = 0;
350  ::ff->read(&p_data, user_defined);
351 
352  // prepara le strutture dati per il calcolo
353  int size = ::ff->size(p_data);
354  MyVectorHandler v0(size);
355  MyVectorHandler v1(size);
356  MyVectorHandler v2(size);
357  MyVectorHandler v3(size);
358  MyVectorHandler v4(size);
359  MyVectorHandler v5(size);
360 
361  VectorHandler* pX = &v0;
362  VectorHandler* pXP = &v1;
363  VectorHandler* pXm1 = &v2;
364  VectorHandler* pXPm1 = &v3;
365  VectorHandler* pXm2 = &v4;
366  VectorHandler* pXPm2 = &v5;
367  pX->Reset();
368  pXP->Reset();
369  pXm1->Reset();
370  pXPm1->Reset();
371  pXm2->Reset();
372  pXPm2->Reset();
373 
374  FullMatrixHandler J(size);
375  MyVectorHandler R(size);
376 
377  // TODO: abstract from a specific solution manager?
378  LinSol ls;
379  SolutionManager *sm = ls.GetSolutionManager(size);
380 
381  MatrixHandler& Jac = *sm->pMatHdl();
382  VectorHandler& Res = *sm->pResHdl();
383  VectorHandler& Sol = *sm->pSolHdl();
384 
385  // parametri di integrazione
386  doublereal ti = d->ti;
387  doublereal dt = d->dt;
388  doublereal tf = d->tf;
389 
390  doublereal tol = d->tol;
391  int maxiter = d->maxiter;
392 
393  // coefficienti del metodo
394  doublereal rho = d->rho;
395  doublereal beta = (4.*rho*rho-(1.-rho)*(1.-rho))/(4.-(1.-rho)*(1.-rho));
396  doublereal delta = (1.-rho)*(1.-rho)/(2.*(4.-(1.-rho)*(1.-rho)));
397  doublereal a1 = 1.-beta;
398  doublereal a2 = beta;
399  doublereal b0 = delta+.5;
400  doublereal b1 = .5*beta+.5-2.*delta;
401  doublereal b2 = .5*beta+delta;
402 
403  doublereal t = ti;
404 
405  // inizializza la soluzione
406  ::ff->init(p_data, *pX);
407  ::ff->func(p_data, *pXP, *pX, t);
408  for (int k = 1; k <= size; k++) {
409  doublereal x = pX->operator()(k);
410  doublereal xp = pXP->operator()(k);
411  pXPm1->PutCoef(k, xp);
412  pXm1->PutCoef(k, x-dt*xp);
413  }
414 
415  // output iniziale
416  std::cout << ti << " " << 0. << " ";
417  ::ff->out(p_data, std::cout, *pX, *pXP) << std::endl;
418 
419  flip(&pX, &pXP, &pXm1, &pXPm1, &pXm2, &pXPm2);
420 
421  while (t < tf) {
422  t += dt;
423  // predict
424  for (int k = size; k > 0; k--) {
425  doublereal xm1 = pXm1->operator()(k);
426  doublereal xPm1 = pXPm1->operator()(k);
427  doublereal xm2 = pXm2->operator()(k);
428  doublereal xPm2 = pXPm2->operator()(k);
429  doublereal x = -4.*xm1+5.*xm2+dt*(4.*xPm1+2.*xPm2);
430  pX->PutCoef(k, x);
431  R.PutCoef(k, a1*xm1+a2*xm2+dt*(b1*xPm1+b2*xPm2));
432  }
433 
434  // test
435  int j = 0;
436  doublereal test;
437  doublereal coef = dt*b0;
438  do {
439  ::ff->func(p_data, *pXP, *pX, t);
440  for (int k = 1; k <= size; k++) {
441  doublereal x = pX->operator()(k);
442  doublereal xP = pXP->operator()(k);
443  Res.PutCoef(k, R(k)-x+coef*xP);
444  }
445 
446  test = Res.Norm();
447  if (test < tol) {
448  break;
449  }
450  if (++j > maxiter) {
451  std::cerr << "current iteration " << j
452  << " exceeds max iteration number "
453  << maxiter << std::endl;
454  exit(EXIT_FAILURE);
455  }
456 
457  // correct
458  sm->MatrReset();
459  J.Reset();
460  ::ff->grad(p_data, J, *pX, t);
461  for (int k = 1; k <= size; k++) {
462  for (int l = 1; l <= size; l++) {
463  Jac.PutCoef(k, l, -coef*J(k, l));
464  }
465  Jac.IncCoef(k, k, 1.);
466  }
467  sm->Solve();
468 
469  // update
470  for (int k = size; k > 0; k--) {
471  doublereal dx = Sol(k);
472  pX->IncCoef(k, dx);
473  }
474  } while (true);
475 
476  // output
477  std::cout << t << " " << test << " ";
478  ::ff->out(p_data, std::cout, *pX, *pXP) << std::endl;
479 
480  flip(&pX, &pXP, &pXm1, &pXPm1, &pXm2, &pXPm2);
481  }
482 
483  ::ff->destroy(&p_data);
484 
485  return 0;
486 }
487 
488 static int
489 method_hope(const char* module, integration_data* d,
490  void* method_data, const char* user_defined)
491 {
492  std::cerr << __FUNCTION__ << "not implemented yet!" << std::endl;
493  exit(EXIT_FAILURE);
494 
495  return 0;
496 }
497 
498 static int
499 method_cubic(const char* module, integration_data* d,
500  void* method_data, const char* user_defined)
501 {
502  // prepara i dati
503  void* p_data = 0;
504  ::ff->read(&p_data, user_defined);
505 
506  // prepara le strutture dati per il calcolo
507  int size = ::ff->size(p_data);
508  MyVectorHandler v0(size);
509  MyVectorHandler v1(size);
510  MyVectorHandler v2(size);
511  MyVectorHandler v3(size);
512  MyVectorHandler v4(size);
513  MyVectorHandler v5(size);
514 
515  VectorHandler* pX = &v0;
516  VectorHandler* pXP = &v1;
517  VectorHandler* pXm1 = &v2;
518  VectorHandler* pXPm1 = &v3;
519  VectorHandler* pXm2 = &v4;
520  VectorHandler* pXPm2 = &v5;
521  pX->Reset();
522  pXP->Reset();
523  pXm1->Reset();
524  pXPm1->Reset();
525  pXm2->Reset();
526  pXPm2->Reset();
527 
528  FullMatrixHandler Jz(size);
529  FullMatrixHandler J0(size);
530  MyVectorHandler Xz(size);
531  MyVectorHandler XPz(size);
532 
533  // TODO: abstract from a specific solution manager?
534  LinSol ls;
535  SolutionManager *sm = ls.GetSolutionManager(size);
536 
537  MatrixHandler& Jac = *sm->pMatHdl();
538  VectorHandler& Res = *sm->pResHdl();
539  VectorHandler& Sol = *sm->pSolHdl();
540 
541  // paramteri di integrazione
542  doublereal ti = d->ti;
543  doublereal dt = d->dt;
544  doublereal tf = d->tf;
545 
546  doublereal tol = d->tol;
547  int maxiter = d->maxiter;
548 
549  // coefficienti del metodo
550  doublereal rho = d->rho;
551  doublereal z = -rho/(1.+rho);
552  doublereal w1 = (2.+3.*z)/(6.*(1.+z));
553  doublereal wz = -1./(6.*z*(1.+z));
554  doublereal w0 = (1.+3.*z)/(6.*z);
555  doublereal m0 = 1.-z*z*(3.+2.*z);
556  doublereal m1 = z*z*(3.+2.*z);
557  doublereal n0 = z*(1.+z)*(1.+z);
558  doublereal n1 = z*z*(1.+z);
559 
560  doublereal t = ti;
561 
562  // inizializza la soluzione
563  ::ff->init(p_data, *pX);
564  ::ff->func(p_data, *pXP, *pX, t);
565  for (int k = 1; k <= size; k++) {
566  doublereal x = pX->operator()(k);
567  doublereal xp = pXP->operator()(k);
568  pXPm1->PutCoef(k, xp);
569  pXm1->PutCoef(k, x-dt*xp);
570  }
571 
572  // output iniziale
573  std::cout << ti << " " << 0. << " ";
574  ::ff->out(p_data, std::cout, *pX, *pXP) << std::endl;
575 
576  flip(&pX, &pXP, &pXm1, &pXPm1, &pXm2, &pXPm2);
577 
578  while (t < tf) {
579  t += dt;
580  // predict
581  for (int k = 1; k <= size; k++) {
582  doublereal xm1 = pXm1->operator()(k);
583  doublereal xPm1 = pXPm1->operator()(k);
584  doublereal xm2 = pXm2->operator()(k);
585  doublereal xPm2 = pXPm2->operator()(k);
586  doublereal x = -4.*xm1+5.*xm2+dt*(4.*xPm1+2.*xPm2);
587  pX->PutCoef(k, x);
588  }
589 
590  // test
591  int j = 0;
592  doublereal test;
593  do {
594  pXP->Reset();
595  ::ff->func(p_data, *pXP, *pX, t);
596  for (int k = 1; k <= size; k++) {
597  doublereal x = pX->operator()(k);
598  doublereal xP = pXP->operator()(k);
599  doublereal xm1 = pXm1->operator()(k);
600  doublereal xPm1 = pXPm1->operator()(k);
601  doublereal xz = m0*x+m1*xm1+dt*(n0*xP+n1*xPm1);
602  Xz.PutCoef(k, xz);
603  }
604  XPz.Reset();
605  ::ff->func(p_data, XPz, Xz, t+z*dt);
606  for (int k = 1; k <= size; k++) {
607  doublereal d = dt*(
608  w1*pXPm1->operator()(k)
609  + wz*XPz(k)
610  + w0*pXP->operator()(k)
611  ) - (
612  pX->operator()(k)
613  - pXm1->operator()(k)
614  );
615  Res.PutCoef(k, d);
616  }
617 
618  test = Res.Norm();
619  if (test < tol) {
620  break;
621  }
622  if (++j > maxiter) {
623  std::cerr << "current iteration " << j
624  << " exceeds max iteration number "
625  << maxiter << std::endl;
626  exit(EXIT_FAILURE);
627  }
628 
629  // correct
630  sm->MatrReset();
631  Jz.Reset();
632  J0.Reset();
633  ::ff->grad(p_data, Jz, Xz, t+z*dt);
634  ::ff->grad(p_data, J0, *pX, t);
635  for (int k = 1; k <= size; k++) {
636  for (int l = 1; l <= size; l++) {
637  doublereal d = 0.;
638  for (int m = 1; m <= size; m++) {
639  d += Jz(k, m)*J0(m, l);
640  }
641  d = -dt*(wz*(Jz(k, l)+dt*n0*d)+w0*J0(k, l));
642  Jac.PutCoef(k, l, d);
643  }
644  Jac.IncCoef(k, k, 1.);
645  }
646  sm->Solve();
647 
648  // update
649  for (int k = size; k > 0; k--) {
650  doublereal dx = Sol(k);
651  pX->IncCoef(k, dx);
652  }
653  } while (true);
654 
655  // output
656  std::cout << t << " " << test << " ";
657  ::ff->out(p_data, std::cout, *pX, *pXP) << std::endl;
658 
659  flip(&pX, &pXP, &pXm1, &pXPm1, &pXm2, &pXPm2);
660  }
661 
662  ::ff->destroy(&p_data);
663 
664  return 0;
665 }
666 
667 static int
668 method_cn(const char* module, integration_data* d,
669  void* method_data, const char* user_defined)
670 {
671  std::cerr << __FUNCTION__ << "not implemented yet!" << std::endl;
672  exit(EXIT_FAILURE);
673 
674  return 0;
675 }
676 
677 #else // ! USE_RUNTIME_LOADING
678 
679 #include <iostream>
680 #include <stdlib.h>
681 
682 int
683 main(void)
684 {
685  std::cerr << "Need dynamic load capabilities" << std::endl;
686  exit(EXIT_FAILURE);
687 }
688 
689 #endif // ! USE_RUNTIME_LOADING
virtual void Reset(void)=0
virtual VectorHandler * pResHdl(void) const =0
int optopt
Definition: getopt.c:73
int optind
Definition: getopt.c:72
virtual void IncCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: mh.cc:374
SolutionManager *const GetSolutionManager(integer iNLD, integer iLWS=0) const
Definition: linsol.cc:455
virtual void IncCoef(integer iRow, const doublereal &dCoef)=0
pfunc_f * func
Definition: dae-intg.h:56
virtual doublereal Norm(void) const
Definition: vh.cc:262
Definition: linsol.h:39
phelp_f * help
Definition: dae-intg.h:52
psize_f * size
Definition: dae-intg.h:54
virtual MatrixHandler * pMatHdl(void) const =0
Definition: mbdyn.h:77
virtual void MatrReset(void)=0
virtual void PutCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: mh.cc:384
virtual void Solve(void)=0
pread_f * read
Definition: dae-intg.h:51
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
int main(void)
Definition: intg.cc:683
char * optarg
Definition: getopt.c:74
virtual VectorHandler * pSolHdl(void) const =0
static const std::vector< doublereal > v0
Definition: fixedstep.cc:45
double doublereal
Definition: colamd.c:52
Definition: dae-intg.h:50
pgrad_f * grad
Definition: dae-intg.h:55
Mat3x3 R