MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
stepsol.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/base/stepsol.cc,v 1.85 2017/10/14 23:58:06 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  * Paolo Mantegazza <mantegazza@aero.polimi.it>
10  *
11  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12  * via La Masa, 34 - 20156 Milano, Italy
13  * http://www.aero.polimi.it
14  *
15  * Changing this copyright notice is forbidden.
16  *
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation (version 2 of the License).
20  *
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30  */
31 
32  /*
33  *
34  * Copyright (C) 2003-2017
35  * Giuseppe Quaranta <quaranta@aero.polimi.it>
36  *
37  * classi che impementano l'integrazione al passo
38  */
39 
40 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
41 
42 #include <limits>
43 
44 #include "schurdataman.h"
45 #include "external.h"
46 #include "ls.h"
47 #include "solver.h"
48 #include "invsolver.h"
49 #include "stepsol.h"
50 
52  const doublereal dT,
53  const doublereal dSolutionTol,
54  const integer stp,
55  const integer sts)
56 : pDM(0),
57 pDofs(0),
58 outputPred(false),
59 MaxIters(MaxIt),
60 dTol(dT),
61 dSolTol(dSolutionTol),
62 steps(stp),
63 unkstates(sts)
64 {
65  NO_OP;
66 }
67 
69 {
70  NO_OP;
71 }
72 
73 void
75 {
76  pDM = pDatMan;
77  pDofs = &pDM->GetDofs();
78 }
79 
80 integer
82 {
83  return steps;
84 }
85 
86 integer
88 {
89  return unkstates;
90 }
91 
92 integer
94 {
95  return MaxIters;
96 }
97 
100 {
101  return dTol;
102 }
103 
106 {
107  return dSolTol;
108 }
109 
110 void
112 {
113  outputPred = fpred;
114 }
115 
116 void
118 {
119  NO_OP;
120 }
121 
122 #include "stepsol.hc"
123 
125  const doublereal dT,
126  const doublereal dSolutionTol,
127  const integer stp,
128  const integer sts,
129  const bool bmod_res_test)
130 : StepIntegrator(MaxIt, dT, dSolutionTol, stp, sts),
131 bEvalProdCalledFirstTime(true),
132 pXCurr(0),
133 pXPrimeCurr(0),
134 bModResTest(bmod_res_test)
135 {
136  NO_OP;
137 }
138 
140 {
141  NO_OP;
142 }
143 
144 void
146  const VectorHandler& w, VectorHandler& z) const
147 {
148  /* matrix-free product
149  *
150  * J(XCurr) * w = -||w|| * (Res(XCurr + sigma * Tau * w/||w||) - f0) / (sigma * Tau)
151  *
152  */
154  XTau.Resize(w.iGetSize());
157  bEvalProdCalledFirstTime = false;
158  }
159 
160  SavedState = *pXCurr;
162 
163  /* if w = 0; J * w = 0 */
164  ASSERT(pDM != NULL);
165 
166  doublereal nw = w.Norm();
167  if (nw < std::numeric_limits<doublereal>::epsilon()) {
168  z.Reset();
169  return;
170  }
171  doublereal sigma = pXCurr->InnerProd(w);
172  sigma /= nw;
173  if (fabs(sigma) > std::numeric_limits<doublereal>::epsilon()) {
174  doublereal xx = (fabs( sigma) <= 1.) ? 1. : fabs(sigma);
175  Tau = copysign(Tau*xx, sigma);
176  }
177  Tau /= nw;
178 #ifdef DEBUG_ITERATIVE
179  std::cout << "Tau " << Tau << std::endl;
180 #endif /* DEBUG_ITERATIVE */
181 
182  XTau.Reset();
183  z.Reset();
184  XTau.ScalarMul(w, Tau);
185  Update(&XTau);
186 #ifdef USE_EXTERNAL
187  External::SendFreeze();
188 #endif /* USE_EXTERNAL */
189  /* deal with throwing elements: do not honor their requests while perfoming matrix free update */
190  try {
191  Residual(&z);
192  }
194  }
195  XTau.ScalarMul(XTau, -1.);
196 
197  /* riporta tutto nelle condizioni inziali */
198 #if 0
199  Update(&XTau);
200 #endif
201  *pXCurr = SavedState;
203  pDM->Update();
204  z -= f0;
205  z.ScalarMul(z, -1./Tau);
206 }
207 
208 /* scale factor for tests */
211 {
212  dCoef = 1.;
213 
214  if (bModResTest) {
215 #ifdef USE_MPI
216 #warning "ImplicitStepIntegrator::TestScale() not available with Schur solution"
217 #endif /* USE_MPI */
218 
219  doublereal dXPr = 0.;
220 
221  DataManager::DofIterator_const CurrDof = pDofs->begin();
222 
223  for (int iCntp1 = 1; iCntp1 <= pXPrimeCurr->iGetSize();
224  iCntp1++, ++CurrDof)
225  {
226  ASSERT(CurrDof != pDofs->end());
227 
228  if (CurrDof->Order == DofOrder::DIFFERENTIAL) {
229  doublereal d = pXPrimeCurr->operator()(iCntp1);
230  doublereal d2 = d*d;
231 
232  doublereal ds = pTest->dScaleCoef(iCntp1);
233  doublereal ds2 = ds*ds;
234  d2 *= ds2;
235 
236  dXPr += d2;
237  }
238  /* else if ALGEBRAIC: non aggiunge nulla */
239  }
240 
241  return 1./(1. + dXPr);
242 
243  } else {
244  return 1.;
245  }
246 }
247 
248 
250  const doublereal dSolTl,
251  const doublereal dC,
252  const integer iMaxIt,
253  const bool bmod_res_test,
254  const integer iMaxIterCoef,
255  const doublereal dFactorCoef)
256 : ImplicitStepIntegrator(iMaxIt, Tl, dSolTl, 1, 1, bmod_res_test),
257 dCoef(dC),
258 iMaxIterCoef(iMaxIterCoef),
259 dFactorCoef(dFactorCoef)
260 {
261  NO_OP;
262 }
263 
265 {
266  NO_OP;
267 }
268 
271  const doublereal TStep,
272  const doublereal /* dAph */,
273  const StepChange /* StType */,
274  std::deque<MyVectorHandler*>& qX,
275  std::deque<MyVectorHandler*>& qXPrime,
276  MyVectorHandler*const pX,
277  MyVectorHandler*const pXPrime,
278  integer& EffIter,
279  doublereal& Err,
280  doublereal& SolErr)
281 {
282  /* no predizione */
283  ASSERT(pDM != NULL);
284 
285  /* Make a deep copy of the current state in order to restore it later */
286  MyVectorHandler X(*pX);
287  MyVectorHandler XPrime(*pXPrime);
288 
289  pXCurr = &X;
290  pXPrimeCurr = &XPrime;
291 
292  try {
294 
295  bool bConverged = false;
296  const doublereal dInitialCoef = dCoef;
297  doublereal dCoefBest = dCoef;
298  doublereal dResErrMin = std::numeric_limits<doublereal>::max();
299  doublereal dSolErrMin = dResErrMin;
300  const integer iMaxPowerCoef = iMaxIterCoef > 0 ? 2 * iMaxIterCoef + 1 : 0;
301 
302  for (int i = 0; i <= iMaxPowerCoef; ++i) {
303  const bool bLastChance = i == iMaxPowerCoef;
304 
305  try {
306  Err = 0.;
307  pS->pGetNonlinearSolver()->Solve(this,
308  pS,
309  MaxIters,
310  dTol,
311  EffIter,
312  Err,
313  dSolTol,
314  SolErr);
315  bConverged = true;
317  if (bLastChance) {
318  throw;
319  }
321  if (bLastChance) {
322  throw;
323  }
324  } catch (LinearSolver::ErrFactor) {
325  if (bLastChance) {
326  throw;
327  }
328  }
329 
330  if (bConverged) {
331  break;
332  }
333 
334  /* Save smallest residual error and corresponding derivative coefficient. */
335  if (Err < dResErrMin) {
336  dResErrMin = Err;
337  dSolErrMin = SolErr;
338  dCoefBest = dCoef;
339  }
340 
341  /* Restore the state after assembly. */
342  *pXCurr = *pX;
343  *pXPrimeCurr = *pXPrime;
344 
346 
347  /* Reset reference rotation parameters and angular velocities */
348  pDM->AfterPredict();
349 
350  /* Try different values for derivatives coefficient. */
351  if (i < iMaxIterCoef) {
352  dCoef *= dFactorCoef;
353  } else if (i == iMaxIterCoef) {
354  dCoef = dInitialCoef / dFactorCoef;
355  } else if (i < 2 * iMaxIterCoef) {
356  dCoef /= dFactorCoef;
357  } else {
358  /* Convergence could not be achieved with any coefficient.
359  * Choose those coefficient with smallest residual error
360  * and increase the tolerance, so it will converge in any case. */
361  const doublereal dSafetyFactor = 1.01;
362 
363  dCoef = dCoefBest;
364  dTol = dSafetyFactor * dResErrMin;
365  dSolTol = dSafetyFactor * dSolErrMin;
366  }
367 
368  silent_cout("Derivatives(" << i + 1 << '/' << 2 * iMaxIterCoef + 1
369  << ") t=" << pDM->dGetTime()
370  << " coef=" << dCoef / TStep
371  << " tol=" << dTol
372  << std::endl);
373  }
374  /* if it gets here, it surely converged */
376 
377  *pX = *pXCurr;
378  *pXPrime = *pXPrimeCurr;
379 
380  pXCurr = 0;
381  pXPrimeCurr = 0;
382  } catch (...) {
383  /* Clean up pointers to local variables */
384  pXCurr = 0;
385  pXPrimeCurr = 0;
386  throw;
387  }
388 
389  return Err;
390 }
391 
392 void
394 {
395  ASSERT(pDM != NULL);
396  pDM->AssRes(*pRes, dCoef);
397 }
398 
399 void
401 {
402  ASSERT(pDM != NULL);
403  pDM->AssJac(*pJac, dCoef);
404 }
405 
406 void
408  const DofOrder::Order Order,
409  const VectorHandler* const pSol) const
410 {
411  doublereal d = pSol->operator()(DCount);
412  if (Order == DofOrder::DIFFERENTIAL) {
413  pXPrimeCurr->IncCoef(DCount, d);
414 
415 #if 1 /* FIXME: update state derivatives only */
416  pXCurr->IncCoef(DCount, dCoef*d);
417 #endif
418 
419  } else {
420  pXCurr->IncCoef(DCount, d);
421 #if 1 /* FIXME: update state only */
422  pXPrimeCurr->IncCoef(DCount, dCoef*d);
423 #endif
424  }
425 }
426 
427 void
429 {
430  DEBUGCOUTFNAME("DerivativeSolver::Update");
431  ASSERT(pDM != NULL);
432 
435 }
436 
437 /* scale factor for tests */
439 DerivativeSolver::TestScale(const NonlinearSolverTest *pTest, doublereal& dAlgebraicEqu) const
440 {
441  dAlgebraicEqu = dCoef;
442  return 1.;
443 }
444 
445 
447  const doublereal dT,
448  const doublereal dSolutionTol,
449  const integer stp,
450  const bool bmod_res_test)
451 : ImplicitStepIntegrator(MaxIt, dT, dSolutionTol, stp, 1, bmod_res_test),
452 db0Differential(0.),
453 db0Algebraic(0.)
454 {
455  NO_OP;
456 }
457 
459 {
460  NO_OP;
461 }
462 
463 void
465 {
466  ASSERT(pDM != NULL);
467  pDM->AssRes(*pRes, db0Differential);
468 }
469 
470 #include "naivemh.h"
471 void
473 {
474  ASSERT(pDM != NULL);
475  pDM->AssJac(*pJac, db0Differential);
476 
477 #ifdef MBDYN_FDJAC
478  // Finite difference check of Jacobian matrix
479  // Uncomment this whenever you need to debug your new Jacobian
480  // NOTE: might not be safe!
481  if (pDM->bFDJac()) {
482  NaiveMatrixHandler fdjac(pJac->iGetNumRows());
483  fdjac.Reset();
484  MyVectorHandler basesol(pJac->iGetNumRows());
485  MyVectorHandler incsol(pJac->iGetNumRows());
486  MyVectorHandler inc(pJac->iGetNumRows());
487  Residual(&basesol);
488  doublereal ddd = 0.001;
489  for (integer i = 1; i <= pJac->iGetNumRows(); i++) {
490  incsol.Reset();
491  inc.Reset();
492  inc.PutCoef(i, ddd);
493  Update(&inc);
494  // std::cerr << pXPrimeCurr->operator()(30) << std::endl;
495  pDM->AssRes(incsol, db0Differential);
496  inc.Reset();
497  inc.PutCoef(i, -ddd);
498  Update(&inc);
499  incsol -= basesol;
500  incsol *= (1./(-ddd));
501  for (integer j = 1; j <= pJac->iGetNumCols(); j++) {
502  fdjac.PutCoef(j, i, std::abs(incsol(j)) > 1.E-100 ? incsol(j) : 0.);
503  }
504  }
505 
506  std::cerr << "\nxxxxxxxxxxxxxxx\n" << std::endl;
507  std::cerr << *pJac << std::endl;
508  std::cerr << "\n---------------\n" << std::endl;
509  std::cerr << fdjac << std::endl;
510  std::cerr << "\n===============\n" << std::endl;
511  }
512 #endif // MBDYN_FDJAC
513 }
514 
515 void
516 StepNIntegrator::UpdateDof(const int DCount,
517  const DofOrder::Order Order,
518  const VectorHandler* const pSol) const
519 {
520  doublereal d = pSol->operator()(DCount);
521  if (Order == DofOrder::DIFFERENTIAL) {
522  pXPrimeCurr->IncCoef(DCount, d);
523 
524  /* Nota: b0Differential e b0Algebraic
525  * possono essere distinti;
526  * in ogni caso sono calcolati
527  * dalle funzioni di predizione
528  * e sono dati globali */
529  pXCurr->IncCoef(DCount, db0Differential*d);
530 
531  } else {
532  pXCurr->IncCoef(DCount, d);
533  pXPrimeCurr->IncCoef(DCount, db0Algebraic*d);
534  }
535 }
536 
537 void
539 {
540  DEBUGCOUTFNAME("StepNIntegrator::Update");
541  ASSERT(pDM != NULL);
542 
544  pDM->Update();
545 }
546 
548 {
549  const doublereal dDiffEqu = ImplicitStepIntegrator::TestScale(pTest, dAlgebraicEqu);
550  dAlgebraicEqu = db0Differential;
551 
552  return dDiffEqu;
553 }
554 
555 /* StepNIntegrator - end */
556 
557 
558 /* Step1Integrator - begin */
559 
561  const doublereal dT,
562  const doublereal dSolutionTol,
563  const bool bmod_res_test)
564 : StepNIntegrator(MaxIt, dT, dSolutionTol, 1, bmod_res_test),
565 pXPrev(NULL),
566 pXPrimePrev(NULL)
567 {
568  NO_OP;
569 }
570 
572 {
573  NO_OP;
574 }
575 
576 /* predizione valida per tutti i metodi del second'ordine
577  a patto di utilizzare le giuste funzioni implementate per
578  ciascun metodo
579  */
580 
581 void Step1Integrator::PredictDof(const int DCount,
582  const DofOrder::Order Order,
583  const VectorHandler* const pSol) const
584 {
585  if (Order == DofOrder::DIFFERENTIAL) {
586  doublereal dXnm1 = pXPrev->operator()(DCount);
587  doublereal dXPnm1 = pXPrimePrev->operator()(DCount);
588  doublereal dXPn = dPredDer(dXnm1, dXPnm1);
589  doublereal dXn = dPredState(dXnm1, dXPn, dXPnm1);
590  pXPrimeCurr->PutCoef(DCount, dXPn);
591  pXCurr->PutCoef(DCount, dXn);
592 
593  } else if (Order == DofOrder::ALGEBRAIC) {
594  doublereal dXnm1 = pXPrev->operator()(DCount);
595  doublereal dXInm1 =
596  pXPrimePrev->operator()(DCount);
597 
598  doublereal dXn = dPredDerAlg(dXInm1, dXnm1);
599  doublereal dXIn = dPredStateAlg(dXInm1, dXn, dXnm1);
600 
601  pXCurr->PutCoef(DCount, dXn);
602  pXPrimeCurr->PutCoef(DCount, dXIn);
603 
604  } else {
605  silent_cerr("Step1Integrator::"
606  "PredictDof(): "
607  "unknown order for local dof "
608  << DCount << std::endl);
610  }
611 }
612 
613 
614 void
616 {
617  DEBUGCOUTFNAME("Step1Integrator::Predict");
618  ASSERT(pDM != NULL);
620 }
621 
624  const doublereal TStep,
625  const doublereal dAph, const StepChange StType,
626  std::deque<MyVectorHandler*>& qX,
627  std::deque<MyVectorHandler*>& qXPrime,
628  MyVectorHandler*const pX,
629  MyVectorHandler*const pXPrime,
630  integer& EffIter,
631  doublereal& Err,
632  doublereal& SolErr)
633 {
634  ASSERT(pDM != NULL);
635  pXCurr = pX;
636  pXPrev = qX[0];
637 
638  pXPrimeCurr = pXPrime;
639  pXPrimePrev = qXPrime[0];
640 
641  /* predizione */
642  SetCoef(TStep, dAph, StType);
643  Predict();
645  pDM->AfterPredict();
646 
647 #ifdef DEBUG
648  integer iNumDofs = pDM->iGetNumDofs();
649  if (outputPred) {
650  std::cout << "After prediction, time=" << pDM->dGetTime() << std::endl;
651  std::cout << "Dof: | XCurr ";
652  for (unsigned idx = 0; idx < qX.size(); idx++) {
653  std::cout << "| XPrev[" << idx << "] ";
654  }
655  std::cout << "| XPrime ";
656  for (unsigned idx = 0; idx < qXPrime.size(); idx++) {
657  std::cout << "| XPPrev[" << idx << "] ";
658  }
659  std::cout << "|" << std::endl;
660  for (int iTmpCnt = 1; iTmpCnt <= iNumDofs; iTmpCnt++) {
661  std::cout << std::setw(8) << iTmpCnt << ": ";
662  std::cout << std::setw(12) << pX->operator()(iTmpCnt);
663  for (unsigned int ivec = 0; ivec < qX.size(); ivec++) {
664  std::cout << std::setw(12)
665  << (qX[ivec])->operator()(iTmpCnt);
666  }
667  std::cout << std::setw(12) << pXPrime->operator()(iTmpCnt);
668  for (unsigned int ivec = 0; ivec < qXPrime.size(); ivec++) {
669  std::cout << std::setw(12)
670  << (qXPrime[ivec])->operator()(iTmpCnt);
671  }
672  std::cout << " " << pDM->DataManager::GetDofDescription(iTmpCnt) << std::endl;
673  }
674  }
675 #endif /* DEBUG */
676 
677  Err = 0.;
678  pS->pGetNonlinearSolver()->Solve(this, pS, MaxIters, dTol,
679  EffIter, Err, dSolTol, SolErr);
680 
681  /* if it gets here, it surely converged */
683 
684  return Err;
685 }
686 
687 /* Step1Integrator - end */
688 
689 
690 /* Step2Integrator - begin */
691 
693  const doublereal dT,
694  const doublereal dSolutionTol,
695  const bool bmod_res_test)
696 : StepNIntegrator(MaxIt, dT, dSolutionTol, 2, bmod_res_test),
697 pXPrev(NULL),
698 pXPrev2(NULL),
699 pXPrimePrev(NULL),
700 pXPrimePrev2(NULL)
701 {
702  NO_OP;
703 }
704 
706 {
707  NO_OP;
708 }
709 
710 /* predizione valida per tutti i metodi del second'ordine
711  a patto di utilizzare le giuste funzioni implementate per
712  ciascun metodo
713  */
714 void Step2Integrator::PredictDof(const int DCount,
715  const DofOrder::Order Order,
716  const VectorHandler* const pSol) const
717 {
718  if (Order == DofOrder::DIFFERENTIAL) {
719  doublereal dXnm1 = pXPrev->operator()(DCount);
720  doublereal dXnm2 = pXPrev2->operator()(DCount);
721  doublereal dXPnm1 = pXPrimePrev->operator()(DCount);
722 
723  doublereal dXPnm2 =
724  pXPrimePrev2->operator()(DCount);
725  doublereal dXPn = dPredDer(dXnm1, dXnm2,
726  dXPnm1, dXPnm2);
727  doublereal dXn = dPredState(dXnm1, dXnm2,
728  dXPn, dXPnm1, dXPnm2);
729 
730  pXPrimeCurr->PutCoef(DCount, dXPn);
731  pXCurr->PutCoef(DCount, dXn);
732 
733  } else if (Order == DofOrder::ALGEBRAIC) {
734  doublereal dXnm1 = pXPrev->operator()(DCount);
735  doublereal dXnm2 = pXPrev2->operator()(DCount);
736  doublereal dXInm1 =
737  pXPrimePrev->operator()(DCount);
738 
739  doublereal dXn = dPredDerAlg(dXInm1,
740  dXnm1, dXnm2);
741  doublereal dXIn = dPredStateAlg(dXInm1,
742  dXn, dXnm1, dXnm2);
743 
744  pXCurr->PutCoef(DCount, dXn);
745  pXPrimeCurr->PutCoef(DCount, dXIn);
746 
747  } else {
748  silent_cerr("Step2Integrator::"
749  "PredictDof(): "
750  "unknown order for local dof "
751  << DCount << std::endl);
753  }
754 }
755 
756 void
758 {
759  DEBUGCOUTFNAME("Step2Integrator::Predict");
760  ASSERT(pDM != NULL);
762 }
763 
766  const doublereal TStep,
767  const doublereal dAph, const StepChange StType,
768  std::deque<MyVectorHandler*>& qX,
769  std::deque<MyVectorHandler*>& qXPrime,
770  MyVectorHandler*const pX,
771  MyVectorHandler*const pXPrime,
772  integer& EffIter,
773  doublereal& Err,
774  doublereal& SolErr)
775 {
776  ASSERT(pDM != NULL);
777  pXCurr = pX;
778  pXPrev = qX[0];
779  pXPrev2 = qX[1];
780 
781  pXPrimeCurr = pXPrime;
782  pXPrimePrev = qXPrime[0];
783  pXPrimePrev2 = qXPrime[1];
784 
785  /* predizione */
786  SetCoef(TStep, dAph, StType);
787  Predict();
789  pDM->AfterPredict();
790 
791 #ifdef DEBUG
792  integer iNumDofs = pDM->iGetNumDofs();
793  if (outputPred) {
794  std::cout << "After prediction, time=" << pDM->dGetTime() << std::endl;
795  std::cout << "Dof: | XCurr ";
796  for (unsigned idx = 0; idx < qX.size(); idx++) {
797  std::cout << "| XPrev[" << idx << "] ";
798  }
799  std::cout << "| XPrime ";
800  for (unsigned idx = 0; idx < qXPrime.size(); idx++) {
801  std::cout << "| XPPrev[" << idx << "] ";
802  }
803  std::cout << "|" << std::endl;
804  for (int iTmpCnt = 1; iTmpCnt <= iNumDofs; iTmpCnt++) {
805  std::cout << std::setw(8) << iTmpCnt << ": ";
806  std::cout << std::setw(12) << pX->operator()(iTmpCnt);
807  for (unsigned int ivec = 0; ivec < qX.size(); ivec++) {
808  std::cout << std::setw(12)
809  << (qX[ivec])->operator()(iTmpCnt);
810  }
811  std::cout << std::setw(12) << pXPrime->operator()(iTmpCnt);
812  for (unsigned int ivec = 0; ivec < qXPrime.size(); ivec++) {
813  std::cout << std::setw(12)
814  << (qXPrime[ivec])->operator()(iTmpCnt);
815  }
816  std::cout << " " << pDM->DataManager::GetDofDescription(iTmpCnt) << std::endl;
817  }
818  }
819 #endif /* DEBUG */
820 
821  Err = 0.;
822  pS->pGetNonlinearSolver()->Solve(this, pS, MaxIters, dTol,
823  EffIter, Err, dSolTol, SolErr);
824 
825  /* if it gets here, it surely converged */
827 
828  return Err;
829 }
830 
831 /* Step2Integrator - end */
832 
833 
834 /* CrankNicolson - begin */
835 
837  const doublereal dSolTl,
838  const integer iMaxIt,
839  const bool bmod_res_test)
840 : Step1Integrator(iMaxIt, dTl, dSolTl, bmod_res_test)
841 {
842  NO_OP;
843 }
844 
846 {
847  NO_OP;
848 }
849 
850 void
852  doublereal dAlpha,
853  enum StepChange /* NewStep */)
854 {
855  db0Differential = db0Algebraic = dT*dAlpha/2.;
856 }
857 
858 
861  const doublereal& dXPm1,
862  DofOrder::Order o) const
863 {
864  return dXPm1;
865 }
866 
869  const doublereal& dXP,
870  const doublereal& dXPm1,
871  DofOrder::Order o) const
872 {
873  if (o == DofOrder::ALGEBRAIC) {
874  return db0Differential*(dXP + dXPm1);
875  } /* else if (o == DofOrder::DIFFERENTIAL) */
876  return dXm1 + db0Differential*(dXP + dXPm1);
877 }
878 
879 /* Nota: usa predizione lineare per le derivate (massimo ordine possibile) */
882  const doublereal& dXPm1) const
883 {
884  return dXPm1;
885 }
886 
889  const doublereal& dXP,
890  const doublereal& dXPm1) const
891 {
892  return dXm1 + db0Differential*(dXP + dXPm1);
893 }
894 
897  const doublereal& dXPm1) const
898 {
899  return dXPm1;
900 }
901 
904  const doublereal& dXP,
905  const doublereal& dXPm1) const
906 {
907  return db0Differential*(dXP + dXPm1);
908 }
909 
910 /* CrankNicolson - end */
911 
912 /* Implicit Euler - begin */
913 
915  const doublereal dSolTl,
916  const integer iMaxIt,
917  const bool bmod_res_test)
918 : Step1Integrator(iMaxIt, dTl, dSolTl, bmod_res_test)
919 {
920  NO_OP;
921 }
922 
924 {
925  NO_OP;
926 }
927 
928 void
930  doublereal dAlpha,
931  enum StepChange /* NewStep */)
932 {
933  db0Differential = db0Algebraic = dT*dAlpha;
934 }
935 
936 
939  const doublereal& dXPm1,
940  DofOrder::Order o) const
941 {
942  return dXPm1;
943 }
944 
947  const doublereal& dXP,
948  const doublereal& dXPm1,
949  DofOrder::Order o) const
950 {
951  if (o == DofOrder::ALGEBRAIC) {
952  return db0Differential*dXP;
953  } /* else if (o == DofOrder::DIFFERENTIAL) */
954  return dXm1 + db0Differential*dXP;
955 }
956 
957 /* Nota: usa predizione lineare per le derivate (massimo ordine possibile) */
960  const doublereal& dXPm1) const
961 {
962  return dXPm1;
963 }
964 
967  const doublereal& dXP,
968  const doublereal& dXPm1) const
969 {
970  return dXm1 + db0Differential*dXP;
971 }
972 
975  const doublereal& dXPm1) const
976 {
977  return dXPm1;
978 }
979 
982  const doublereal& dXP,
983  const doublereal& dXPm1) const
984 {
985  return db0Differential*dXP;
986 }
987 
988 /* Implicit Euler - end */
989 
990 /* NostroMetodo - begin */
991 
993  const doublereal dSolTl,
994  const integer iMaxIt,
995  const DriveCaller* pRho,
996  const DriveCaller* pAlgRho,
997  const bool bmod_res_test)
998 :Step2Integrator(iMaxIt, Tl, dSolTl, bmod_res_test),
999 Rho(pRho), AlgebraicRho(pAlgRho)
1000 {
1001  ASSERT(pRho != NULL);
1002  ASSERT(pAlgRho != NULL);
1003 }
1004 
1006 {
1007  NO_OP;
1008 }
1009 
1010 void
1012 {
1013  Rho.pGetDriveCaller()->SetDrvHdl(pDH);
1015 }
1016 
1017 void
1019  doublereal dAlpha,
1020  enum StepChange /* NewStep */)
1021 {
1022  doublereal dRho = Rho.dGet();
1023  doublereal dAlgebraicRho = AlgebraicRho.dGet();
1024 
1025  doublereal dDen = 2.*(1.+dAlpha)-(1.-dRho)*(1.-dRho);
1026  doublereal dBeta = dAlpha*((1.-dRho)*(1.-dRho)*(2.+dAlpha)
1027  +2.*(2.*dRho-1.)*(1.+dAlpha))/dDen;
1028  doublereal dDelta = .5*dAlpha*dAlpha*(1.-dRho)*(1.-dRho)/dDen;
1029 
1030  mp[0] = -6.*dAlpha*(1.+dAlpha);
1031  mp[1] = -mp[0];
1032  np[0] = 1.+4.*dAlpha+3.*dAlpha*dAlpha;
1033  np[1] = dAlpha*(2.+3.*dAlpha);
1034 
1035  a[0][DIFFERENTIAL] = 1.-dBeta;
1036  a[1][DIFFERENTIAL] = dBeta;
1037  b[0][DIFFERENTIAL] = dT*(dDelta/dAlpha+dAlpha/2);
1038  b[1][DIFFERENTIAL] = dT*(dBeta/2.+dAlpha/2.-dDelta/dAlpha*(1.+dAlpha));
1039  b[2][DIFFERENTIAL] = dT*(dBeta/2.+dDelta);
1040 
1041  DEBUGCOUT("Predict()" << std::endl
1042  << "Alpha = " << dAlpha << std::endl
1043  << "Differential coefficients:" << std::endl
1044  << "beta = " << dBeta << std::endl
1045  << "delta = " << dDelta << std::endl
1046  << "a1 = " << a[0][DIFFERENTIAL] << std::endl
1047  << "a2 = " << a[1][DIFFERENTIAL] << std::endl
1048  << "b0 = " << b[0][DIFFERENTIAL] << std::endl
1049  << "b1 = " << b[1][DIFFERENTIAL] << std::endl
1050  << "b2 = " << b[2][DIFFERENTIAL] << std::endl);
1051 
1052  /* Coefficienti del metodo - variabili algebriche */
1053  if (dAlgebraicRho != dRho) {
1054  dDen = 2.*(1.+dAlpha)-(1.-dAlgebraicRho)*(1.-dAlgebraicRho);
1055  dBeta = dAlpha*((1.-dAlgebraicRho)*(1.-dAlgebraicRho)*(2.+dAlpha)
1056  +2.*(2.*dAlgebraicRho-1.)*(1.+dAlpha))/dDen;
1057  dDelta = .5*dAlpha*dAlpha*(1.-dAlgebraicRho)*(1.-dAlgebraicRho)/dDen;
1058 
1059  a[1][ALGEBRAIC] = dBeta;
1060  b[0][ALGEBRAIC] = dT*(dDelta/dAlpha+dAlpha/2.);
1061  b[1][ALGEBRAIC] = dT*(dBeta/2.+dAlpha/2.-dDelta/dAlpha*(1.+dAlpha));
1062  b[2][ALGEBRAIC] = dT*(dBeta/2.+dDelta);
1063 
1064  } else {
1065  a[1][ALGEBRAIC] = a[1][DIFFERENTIAL];
1066  b[0][ALGEBRAIC] = b[0][DIFFERENTIAL];
1067  b[1][ALGEBRAIC] = b[1][DIFFERENTIAL];
1068  b[2][ALGEBRAIC] = b[2][DIFFERENTIAL];
1069  }
1070 
1071  DEBUGCOUT("Algebraic coefficients:" << std::endl
1072  << "beta = " << dBeta << std::endl
1073  << "delta = " << dDelta << std::endl
1074  << "a2 = " << a[1][ALGEBRAIC] << std::endl
1075  << "b0 = " << b[0][ALGEBRAIC] << std::endl
1076  << "b1 = " << b[1][ALGEBRAIC] << std::endl
1077  << "b2 = " << b[2][ALGEBRAIC] << std::endl);
1078 
1079  DEBUGCOUT("Asymptotic rho: "
1080  << -b[1][DIFFERENTIAL]/(2.*b[0][DIFFERENTIAL]) << std::endl
1081  << "Discriminant: "
1082  << b[1][DIFFERENTIAL]*b[1][DIFFERENTIAL]-4.*b[2][DIFFERENTIAL]*b[0][DIFFERENTIAL]
1083  << std::endl
1084  << "Asymptotic rho for algebraic variables: "
1085  << -b[1][ALGEBRAIC]/(2.*b[0][ALGEBRAIC]) << std::endl
1086  << "Discriminant: "
1087  << b[1][ALGEBRAIC]*b[1][ALGEBRAIC]-4.*b[2][ALGEBRAIC]*b[0][ALGEBRAIC]
1088  << std::endl);
1089 
1090  /* Vengono modificati per la predizione, dopo che sono stati usati per
1091  * costruire gli altri coefficienti */
1092  mp[0] /= dT;
1093  mp[1] /= dT;
1094 
1095  /* valori di ritorno */
1096  db0Differential = b[0][DIFFERENTIAL];
1097  db0Algebraic = b[0][ALGEBRAIC];
1098 }
1099 
1100 
1101 doublereal
1103  const doublereal& dXm2,
1104  const doublereal& dXPm1,
1105  const doublereal& dXPm2,
1106  DofOrder::Order o) const
1107 {
1108  if (o == DofOrder::ALGEBRAIC) {
1109  return np[0]*dXPm1+np[1]*dXPm2-mp[1]*dXm1;
1110  } /* else if (o == DofOrder::DIFFERENTIAL) */
1111  return mp[0]*dXm1+mp[1]*dXm2+np[0]*dXPm1+np[1]*dXPm2;
1112 }
1113 
1114 doublereal
1116  const doublereal& dXm2,
1117  const doublereal& dXP,
1118  const doublereal& dXPm1,
1119  const doublereal& dXPm2,
1120  DofOrder::Order o) const
1121 {
1122  if (o == DofOrder::ALGEBRAIC) {
1123  return b[0][ALGEBRAIC]*dXP+b[1][ALGEBRAIC]*dXPm1+b[2][ALGEBRAIC]*dXPm2
1124  -a[1][ALGEBRAIC]*dXm1;
1125  } /* else if (o == DofOrder::DIFFERENTIAL) */
1126  return a[0][DIFFERENTIAL]*dXm1+a[1][DIFFERENTIAL]*dXm2
1127  +b[0][DIFFERENTIAL]*dXP+b[1][DIFFERENTIAL]*dXPm1+b[2][DIFFERENTIAL]*dXPm2;
1128 }
1129 
1130 
1131 /* Nota: usa predizione cubica per le derivate (massimo ordine possibile) */
1132 doublereal
1134  const doublereal& dXm2,
1135  const doublereal& dXPm1,
1136  const doublereal& dXPm2) const
1137 {
1138  return mp[0]*dXm1+mp[1]*dXm2+np[0]*dXPm1+np[1]*dXPm2;
1139 }
1140 
1141 doublereal
1143  const doublereal& dXm2,
1144  const doublereal& dXP,
1145  const doublereal& dXPm1,
1146  const doublereal& dXPm2) const
1147 {
1148  return a[0][DIFFERENTIAL]*dXm1+a[1][DIFFERENTIAL]*dXm2
1149  +b[0][DIFFERENTIAL]*dXP+b[1][DIFFERENTIAL]*dXPm1+b[2][DIFFERENTIAL]*dXPm2;
1150 }
1151 
1152 doublereal
1154  const doublereal& dXPm1,
1155  const doublereal& dXPm2) const
1156 {
1157  return np[0]*dXPm1+np[1]*dXPm2-mp[1]*dXm1;
1158 }
1159 
1160 doublereal
1162  const doublereal& dXP,
1163  const doublereal& dXPm1,
1164  const doublereal& dXPm2) const
1165 {
1166  return b[0][ALGEBRAIC]*dXP+b[1][ALGEBRAIC]*dXPm1+b[2][ALGEBRAIC]*dXPm2
1167  -a[1][ALGEBRAIC]*dXm1;
1168 }
1169 
1170 /* NostroMetodo - end */
1171 
1172 
1173 /* Hope - begin */
1174 
1176  const doublereal dSolTl,
1177  const integer iMaxIt,
1178  const DriveCaller* pRho,
1179  const DriveCaller* pAlgRho,
1180  const bool bmod_res_test)
1181 :Step2Integrator(iMaxIt, Tl, dSolTl, bmod_res_test),
1182 Rho(pRho), AlgebraicRho(pAlgRho), bStep(0)
1183 {
1184  ASSERT(pRho != NULL);
1185  ASSERT(pAlgRho != NULL);
1186 }
1187 
1189 {
1190  NO_OP;
1191 }
1192 
1193 void
1195 {
1196  Rho.pGetDriveCaller()->SetDrvHdl(pDH);
1198 }
1199 
1200 void
1202  doublereal dAlpha,
1203  enum StepChange NewStep)
1204 {
1205 #if 0
1206  if (dAlpha != 1.) {
1207  cerr << "HOPE time step integrator is not implemented yet in variable step form" << std::endl;
1208  throw ErrNotImplementedYet();
1209  }
1210 #endif
1211 
1212  if (NewStep == NEWSTEP) {
1213  ASSERT(bStep == flag(0) || bStep == flag(1));
1214  bStep = 1-bStep; /* Commuta il valore di bStep */
1215  }
1216 
1217  doublereal dTMod = dT*dAlpha;
1218 
1219  /* Differential coefficients */
1220  mp[0] = -6.*dAlpha*(1.+dAlpha);
1221  mp[1] = -mp[0];
1222  np[0] = 1.+4.*dAlpha+3.*dAlpha*dAlpha;
1223  np[1] = dAlpha*(2.+3.*dAlpha);
1224 
1225  if (bStep) {
1226  b[0][DIFFERENTIAL] = b[1][DIFFERENTIAL]
1227  = b[0][ALGEBRAIC] = b[1][ALGEBRAIC]
1228  = db0Algebraic = db0Differential = dTMod/2.;// dT/4.;
1229 
1230  } else {
1231  doublereal dRho = Rho.dGet();
1232  doublereal dALPHA = 4.*dRho/(3.+dRho);
1233 
1234  a[0][DIFFERENTIAL] = (4.-dALPHA)/3.;
1235  a[1][DIFFERENTIAL] = (dALPHA-1.)/3.;
1236  b[0][DIFFERENTIAL] = dTMod*(4.-dALPHA)/6.;// dT*(4.-dALPHA)/12.;
1237  b[1][DIFFERENTIAL] = dTMod*dALPHA/2.;// dT*dALPHA/4.;
1238 
1239  DEBUGCOUT("Predict()" << std::endl
1240  << "Alpha = " << dAlpha << std::endl
1241  << "Differential coefficients:" << std::endl
1242  << "HOPE-Alpha = " << dALPHA << std::endl
1243  << "a1 = " << a[0][DIFFERENTIAL] << std::endl
1244  << "a2 = " << a[1][DIFFERENTIAL] << std::endl
1245  << "b0 = " << b[0][DIFFERENTIAL] << std::endl
1246  << "b1 = " << b[1][DIFFERENTIAL] << std::endl);
1247 
1248  /* Coefficienti del metodo - variabili algebriche */
1249  doublereal dAlgebraicRho = AlgebraicRho.dGet();
1250  doublereal dAlgebraicALPHA = 4.*dAlgebraicRho/(3.+dAlgebraicRho);
1251 
1252  if (dAlgebraicRho != dRho) {
1253  a[1][ALGEBRAIC] = (dAlgebraicALPHA-1.)/3.;
1254  b[0][ALGEBRAIC] = dTMod*(4.-dAlgebraicALPHA)/6.; // dT*(4.-dAlgebraicALPHA)/12.;
1255  b[1][ALGEBRAIC] = dTMod*dAlgebraicALPHA/2.; // dT*dAlgebraicALPHA/4.;
1256 
1257  } else {
1258  a[1][ALGEBRAIC] = a[1][DIFFERENTIAL];
1259  b[0][ALGEBRAIC] = b[0][DIFFERENTIAL];
1260  b[1][ALGEBRAIC] = b[1][DIFFERENTIAL];
1261  }
1262 
1263  DEBUGCOUT("Algebraic coefficients:" << std::endl
1264  << "HOPE-Alpha = " << dAlgebraicALPHA << std::endl
1265  << "a2 = " << a[1][ALGEBRAIC] << std::endl
1266  << "b0 = " << b[0][ALGEBRAIC] << std::endl
1267  << "b1 = " << b[1][ALGEBRAIC] << std::endl);
1268 
1269  /* valori di ritorno */
1270  db0Differential = b[0][DIFFERENTIAL];
1271  db0Algebraic = b[0][ALGEBRAIC];
1272  }
1273 
1274  /* Vengono modificati per la predizione, dopo che sono stati usati per
1275  * costruire gli altri coefficienti */
1276  mp[0] /= dT;
1277  mp[1] /= dT;
1278 }
1279 
1280 doublereal
1282  const doublereal& dXm2,
1283  const doublereal& dXPm1,
1284  const doublereal& dXPm2,
1285  DofOrder::Order o) const
1286 {
1287  if (o == DofOrder::ALGEBRAIC) {
1288  return np[0]*dXPm1+np[1]*dXPm2-mp[1]*dXm1;
1289  } /* else if (o == DofOrder::DIFFERENTIAL) */
1290  return mp[0]*dXm1+mp[1]*dXm2+np[0]*dXPm1+np[1]*dXPm2;
1291 }
1292 
1293 doublereal
1295  const doublereal& dXm2,
1296  const doublereal& dXP,
1297  const doublereal& dXPm1,
1298  const doublereal& /* dXPm2 */ ,
1299  DofOrder::Order o) const
1300 {
1301  if (bStep) {
1302  if (o == DofOrder::ALGEBRAIC) {
1303  return b[0][ALGEBRAIC]*(dXP+dXPm1);
1304  } /* else if (o == DofOrder::DIFFERENTIAL) */
1305  return dXm1+b[0][ALGEBRAIC]*(dXP+dXPm1);
1306 
1307  } else {
1308  if (o == DofOrder::ALGEBRAIC) {
1309  return b[0][ALGEBRAIC]*dXP+b[1][ALGEBRAIC]*dXPm1
1310  -a[1][ALGEBRAIC]*dXm1;
1311  } /* else if (o == DofOrder::DIFFERENTIAL) */
1312  return a[0][DIFFERENTIAL]*dXm1+a[1][DIFFERENTIAL]*dXm2
1313  +b[0][DIFFERENTIAL]*dXP+b[1][DIFFERENTIAL]*dXPm1;
1314  }
1315 }
1316 
1317 /* Nota: usa predizione cubica per le derivate (massimo ordine possibile) */
1318 doublereal
1320  const doublereal& dXm2,
1321  const doublereal& dXPm1,
1322  const doublereal& dXPm2) const
1323 {
1324  return mp[0]*dXm1+mp[1]*dXm2+np[0]*dXPm1+np[1]*dXPm2;
1325 }
1326 
1327 doublereal
1329  const doublereal& dXm2,
1330  const doublereal& dXP,
1331  const doublereal& dXPm1,
1332  const doublereal& /* dXPm2 */ ) const
1333 {
1334  if (bStep) {
1335  return dXm1+b[0][ALGEBRAIC]*(dXP+dXPm1);
1336 
1337  } else {
1338  return a[0][DIFFERENTIAL]*dXm1+a[1][DIFFERENTIAL]*dXm2
1339  +b[0][DIFFERENTIAL]*dXP+b[1][DIFFERENTIAL]*dXPm1;
1340  }
1341 }
1342 
1343 doublereal
1345  const doublereal& dXPm1,
1346  const doublereal& dXPm2) const
1347 {
1348  return np[0]*dXPm1+np[1]*dXPm2-mp[1]*dXm1;
1349 }
1350 
1351 doublereal
1353  const doublereal& dXP,
1354  const doublereal& dXPm1,
1355  const doublereal& /* dXPm2 */ ) const
1356 {
1357  if (bStep) {
1358  return b[0][ALGEBRAIC]*(dXP+dXPm1);
1359  } else {
1360  return b[0][ALGEBRAIC]*dXP+b[1][ALGEBRAIC]*dXPm1
1361  -a[1][ALGEBRAIC]*dXm1;
1362  }
1363 }
1364 
1365 /* Hope - end */
1366 
1367 /* Inverse Dynamics - Begin */
1368 
1369 /*
1370  *
1371  * Portions Copyright (C) 2008
1372  * Alessandro Fumagalli
1373  *
1374  * <alessandro.fumagalli@polimi.it>
1375  *
1376  */
1377 
1379  const doublereal dT,
1380  const doublereal dSolutionTol,
1381  const integer stp,
1382  const integer sts,
1383  const bool bmod_res_test)
1384 : StepIntegrator(MaxIt, dT, dSolutionTol, stp, sts),
1385 XTau(0),
1386 SavedState(0),
1387 SavedDerState(0),
1388 bEvalProdCalledFirstTime(true),
1389 iOrder(InverseDynamics::UNDEFINED),
1390 m_bJacobian(true),
1391 pXCurr(0),
1392 pXPrimeCurr(0),
1393 pXPrimePrimeCurr(0),
1394 pLambdaCurr(0),
1395 bModResTest(bmod_res_test)
1396 {
1397  return;
1398 }
1399 
1401 {
1402  NO_OP;
1403 }
1404 
1405 void
1407  const VectorHandler& w, VectorHandler& z) const
1408 {
1409  /* matrix-free product
1410  *
1411  * J(XCurr) * w = -||w|| * (Res(XCurr + sigma * Tau * w/||w||) - f0) / (sigma * Tau)
1412  *
1413  */
1415  XTau.Resize(w.iGetSize());
1416  SavedState.Resize(w.iGetSize());
1418  bEvalProdCalledFirstTime = false;
1419  }
1420 
1421  SavedState = *pXCurr;
1423 
1424  /* if w = 0; J * w = 0 */
1425  ASSERT(pDM != NULL);
1426 
1427  doublereal nw = w.Norm();
1428  if (nw < std::numeric_limits<doublereal>::epsilon()) {
1429  z.Reset();
1430  return;
1431  }
1432  doublereal sigma = pXCurr->InnerProd(w);
1433  sigma /= nw;
1434  if (fabs(sigma) > std::numeric_limits<doublereal>::epsilon()) {
1435  doublereal xx = (fabs( sigma) <= 1.) ? 1. : fabs(sigma);
1436  Tau = copysign(Tau*xx, sigma);
1437  }
1438  Tau /= nw;
1439 #ifdef DEBUG_ITERATIVE
1440  std::cout << "Tau " << Tau << std::endl;
1441 #endif /* DEBUG_ITERATIVE */
1442 
1443  XTau.Reset();
1444  z.Reset();
1445  XTau.ScalarMul(w, Tau);
1446  Update(&XTau);
1447 #ifdef USE_EXTERNAL
1448  External::SendFreeze();
1449 #endif /* USE_EXTERNAL */
1450  /* deal with throwing elements: do not honor their requests while perfoming matrix free update */
1451  try {
1452  Residual(&z);
1453  }
1455  }
1456  XTau.ScalarMul(XTau, -1.);
1457 
1458  /* riporta tutto nelle condizioni inziali */
1459 #if 0
1460  Update(&XTau);
1461 #endif
1462  *pXCurr = SavedState;
1464  pDM->Update();
1465  z -= f0;
1466  z.ScalarMul(z, -1./Tau);
1467 }
1468 
1469 /* scale factor for tests */
1470 doublereal
1472 {
1473  dCoef = 1.;
1474 
1475  if (bModResTest) {
1476 #ifdef USE_MPI
1477 #warning "InverseDynamicsStepSolver::TestScale() not available with Schur solution"
1478 #endif /* USE_MPI */
1479 
1480  doublereal dXPr = 0.;
1481 
1482  DataManager::DofIterator_const CurrDof = pDofs->begin();
1483 
1484  for (int iCntp1 = 1; iCntp1 <= pXPrimeCurr->iGetSize();
1485  iCntp1++, ++CurrDof)
1486  {
1487  ASSERT(CurrDof != pDofs->end());
1488 
1489  if (CurrDof->Order == DofOrder::DIFFERENTIAL) {
1490  doublereal d = pXPrimeCurr->operator()(iCntp1);
1491  doublereal d2 = d*d;
1492 
1493  doublereal ds = pTest->dScaleCoef(iCntp1);
1494  doublereal ds2 = ds*ds;
1495  d2 *= ds2;
1496 
1497  dXPr += d2;
1498  }
1499  /* else if ALGEBRAIC: non aggiunge nulla */
1500  }
1501 
1502  return 1./(1. + dXPr);
1503 
1504  } else {
1505  return 1.;
1506  }
1507 }
1508 
1509 void
1511 {
1512  this->iOrder = iOrder;
1513 }
1514 
1517 {
1518  return iOrder;
1519 }
1520 
1521 bool
1523 {
1524  return m_bJacobian;
1525 }
1526 
1527 doublereal
1529  const doublereal TStep,
1530  const StepChange StType,
1531  MyVectorHandler*const pX,
1532  MyVectorHandler*const pXPrime,
1533  MyVectorHandler*const pXPrimePrime,
1534  MyVectorHandler*const pLambda,
1535  integer& EffIter,
1536  doublereal& Err,
1537  doublereal& SolErr)
1538 {
1539  ASSERT(pDM != NULL);
1540  pXCurr = pX;
1541  pXPrimeCurr = pXPrime;
1542  pXPrimePrimeCurr = pXPrimePrime;
1543  pLambdaCurr = pLambda;
1544 
1546 
1547  Err = 0.;
1548  NonlinearSolver *pNLSolver = pS->pGetNonlinearSolver();
1549 
1550  /* Position */
1552 
1553  /* Setting iOrder = 0, the residual is called only
1554  * for constraints, on positions */
1555  pNLSolver->Solve(this, pS, MaxIters, dTol,
1556  EffIter, Err, dSolTol, SolErr);
1557 
1558  SolutionManager *pSM = pS->pGetSolutionManager();
1559  MatrixHandler *pMat = pSM->pMatHdl();
1560  VectorHandler *pRes = pSM->pResHdl();
1561  VectorHandler *pSol = pSM->pSolHdl();
1562 
1563  /* use position */
1564  // Update(pSol);
1565 
1566  /* Velocity */
1568 
1569  pRes->Reset();
1570  pSol->Reset();
1571  /* there's no need to check changes in
1572  * equation structure... it is already
1573  * performed by NonlinearSolver->Solve() */
1574  Residual(pRes);
1575 
1576  if (pS->outputRes()) {
1577  silent_cout("Residual(velocity):" << std::endl);
1578  pS->PrintResidual(*pRes, 0);
1579  }
1580 
1581  if (bJacobian()) {
1582  pSM->MatrReset();
1583  Jacobian(pMat);
1584 
1585  if (pS->outputJac()) {
1586  silent_cout("Jacobian(velocity):" << std::endl << *pMat);
1587  }
1588 
1589  EffIter++;
1590  }
1591 
1592  pSM->Solve();
1593 
1594  if (pS->outputSol()) {
1595  silent_cout("Solution(velocity):" << std::endl);
1596  pS->PrintSolution(*pSol, 0);
1597  }
1598 
1599  /* use velocity */
1600  Update(pSol);
1601 
1602  // TODO: if UNDERDETERMINED_UNDERACTUATED_COLLOCATED,
1603  // InverseDynamics::ACCELERATION and InverseDynamics::INVERSE_DYNAMICS
1604  // are solved together
1605 
1606  /* Acceleration */
1608 
1609  pRes->Reset();
1610  pSol->Reset();
1611  Residual(pRes);
1612 
1613  if (pS->outputRes()) {
1614  silent_cout("Residual(acceleration):" << std::endl);
1615  pS->PrintResidual(*pRes, 0);
1616  }
1617 
1618  if (bJacobian()) {
1619  pSM->MatrReset();
1620  Jacobian(pMat);
1621 
1622  if (pS->outputJac()) {
1623  silent_cout("Jacobian(acceleration):" << std::endl << *pMat);
1624  }
1625 
1626  EffIter++;
1627  }
1628 
1629  pSM->Solve();
1630 
1631  if (pS->outputSol()) {
1632  silent_cout("Solution(acceleration):" << std::endl);
1633  pS->PrintSolution(*pSol, 0);
1634  }
1635 
1636  Update(pSol);
1637 
1638  /* Forces */
1640 
1641  pRes->Reset();
1642  pSol->Reset();
1643  Residual(pRes);
1644 
1645  if (pS->outputRes()) {
1646  silent_cout("Residual(inverseDynamics):" << std::endl);
1647  pS->PrintResidual(*pRes, 0);
1648  }
1649 
1650  if (bJacobian()) {
1651  pSM->MatrReset();
1652  Jacobian(pMat);
1653 
1654  if (pS->outputJac()) {
1655  silent_cout("Jacobian(inverseDynamics):" << std::endl << *pMat);
1656  }
1657 
1658  EffIter++;
1659  }
1660 
1661  switch (dynamic_cast<const InverseSolver *>(pDM->GetSolver())->GetProblemType()) {
1663  pSM->SolveT();
1664  break;
1665 
1666  default:
1667  pSM->Solve();
1668  }
1669 
1670  if (pS->outputSol()) {
1671  silent_cout("Solution(inverseDynamics):" << std::endl);
1672  pS->PrintSolution(*pSol, 0);
1673  }
1674 
1675  Update(pSol);
1676 
1678 
1679  return Err;
1680 }
1681 
1682 void
1684 {
1685  ASSERT(pDM != NULL);
1686  switch (iOrder) {
1688  pDM->AssRes(*pRes);
1689  break;
1690 
1691  default:
1692  pDM->AssConstrRes(*pRes, iOrder);
1693  break;
1694  }
1695 }
1696 
1697 void
1699 {
1700  ASSERT(pDM != NULL);
1701  pDM->AssConstrJac(*pJac);
1702 }
1703 
1704 void
1706 {
1707  DEBUGCOUTFNAME("InverseDynamicsStepSolver::Update");
1708  ASSERT(pDM != NULL);
1709 
1710  switch (iOrder) {
1712  *pXCurr += *pSol;
1713  break;
1714 
1716  *pXPrimeCurr = *pSol;
1717  break;
1718 
1720  *pXPrimePrimeCurr = *pSol;
1721  break;
1722 
1724  *pLambdaCurr = *pSol;
1725  break;
1726 
1727  default:
1728  ASSERT(0);
1730  }
1731 
1732  pDM->Update(iOrder);
1733 
1734  // prepare m_bJacobian for next phase
1735  switch (dynamic_cast<const InverseSolver *>(pDM->GetSolver())->GetProblemType()) {
1737  switch (iOrder) {
1740  m_bJacobian = true;
1741  break;
1742 
1745  m_bJacobian = false;
1746  break;
1747 
1748  default:
1749  break;
1750  }
1751  break;
1752 
1754  switch (iOrder) {
1758  m_bJacobian = true;
1759  break;
1760 
1762  m_bJacobian = false;
1763  break;
1764 
1765  default:
1766  break;
1767  }
1768  break;
1769 
1771  // TODO
1773 
1775  m_bJacobian = true;
1776  break;
1777 
1778  default:
1779  break;
1780  }
1781 }
1782 
1783 /* Inverse Dynamics - End */
const doublereal dFactorCoef
Definition: stepsol.h:189
doublereal dCoef
Definition: stepsol.h:187
void Reset(void)
Definition: naivemh.cc:151
doublereal dPredStateAlg(const doublereal &dXm1, const doublereal &dXP, const doublereal &dXPm1, const doublereal &dXPm2) const
Definition: stepsol.cc:1352
virtual void Reset(void)=0
virtual VectorHandler * pResHdl(void) const =0
Step1Integrator(const integer MaxIt, const doublereal dT, const doublereal dSolutionTol, const bool bmod_res_test)
Definition: stepsol.cc:560
doublereal np[2]
Definition: stepsol.h:601
virtual SolutionManager * pGetSolutionManager(void) const
Definition: solver.h:398
void SetCoef(doublereal dT, doublereal dAlpha, enum StepChange NewStep)
Definition: stepsol.cc:851
doublereal dPredDer(const doublereal &dXm1, const doublereal &dXm2, const doublereal &dXPm1, const doublereal &dXPm2) const
Definition: stepsol.cc:1319
doublereal dPredStateAlg(const doublereal &dXm1, const doublereal &dXP, const doublereal &dXPm1, const doublereal &dXPm2) const
Definition: stepsol.cc:1161
void SetCoef(doublereal dT, doublereal dAlpha, enum StepChange NewStep)
Definition: stepsol.cc:1018
doublereal dPredDerAlg(const doublereal &dXm1, const doublereal &dXPm1, const doublereal &dXPm2) const
Definition: stepsol.cc:1153
bool outputPred
Definition: stepsol.h:96
virtual doublereal dPredDer(const doublereal &dXm1, const doublereal &dXPm1) const =0
void UpdateDof(const int DCount, const DofOrder::Order Order, const VectorHandler *const pSol=0) const
Definition: stepsol.cc:407
doublereal dPredictState(const doublereal &dXm1, const doublereal &dXP, const doublereal &dXPm1, DofOrder::Order o=DofOrder::DIFFERENTIAL) const
Definition: stepsol.cc:868
virtual integer iGetNumCols(void) const =0
integer unkstates
Definition: stepsol.h:101
virtual void EvalProd(doublereal Tau, const VectorHandler &f0, const VectorHandler &w, VectorHandler &z) const
Definition: stepsol.cc:145
doublereal dPredDer(const doublereal &dXm1, const doublereal &dXPm1) const
Definition: stepsol.cc:881
virtual void SetCoef(doublereal dT, doublereal dAlpha, enum StepChange NewStep)=0
virtual doublereal dPredDerAlg(const doublereal &dXm1, const doublereal &dXPm1, const doublereal &dXPm2) const =0
~DerivativeSolver(void)
Definition: stepsol.cc:264
long int flag
Definition: mbdyn.h:43
void Jacobian(MatrixHandler *pJac) const
Definition: stepsol.cc:400
ImplicitStepIntegrator(const integer MaxIt, const doublereal dT, const doublereal dSolutionTol, const integer stp, const integer sts, const bool bmod_res_test)
Definition: stepsol.cc:124
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
StepIntegrator(const integer MaxIt, const doublereal dT, const doublereal dSolutionTol, const integer stp, const integer sts)
Definition: stepsol.cc:51
VectorHandler * pXPrimeCurr
Definition: stepsol.h:684
#define DEBUGCOUTFNAME(fname)
Definition: myassert.h:256
MyVectorHandler XTau
Definition: stepsol.h:672
DerivativeSolver(const doublereal Tl, const doublereal dSolTl, const doublereal dC, const integer iMaxIt, const bool bmod_res_test, const integer iMaxIterCoef, const doublereal dFactorCoef)
Definition: stepsol.cc:249
integer steps
Definition: stepsol.h:100
void Jacobian(MatrixHandler *pJac) const
Definition: stepsol.cc:1698
bool bEvalProdCalledFirstTime
Definition: stepsol.h:160
doublereal dPredState(const doublereal &dXm1, const doublereal &dXP, const doublereal &dXPm1) const
Definition: stepsol.cc:888
static double * f0
virtual void OutputTypes(const bool fpred)
Definition: stepsol.cc:111
virtual void PrintResidual(const VectorHandler &Res, integer iIterCnt) const
Definition: solver.cc:5194
doublereal dGetTime(void) const
Definition: dataman2.cc:165
virtual void Predict(void)
Definition: stepsol.cc:615
doublereal dPredictState(const doublereal &dXm1, const doublereal &dXm2, const doublereal &dXP, const doublereal &dXPm1, const doublereal &dXPm2, DofOrder::Order o=DofOrder::DIFFERENTIAL) const
Definition: stepsol.cc:1294
virtual void IncCoef(integer iRow, const doublereal &dCoef)=0
doublereal dSolTol
Definition: stepsol.h:99
virtual doublereal InnerProd(const VectorHandler &VH) const
Definition: vh.cc:269
doublereal dPredDerAlg(const doublereal &dXm1, const doublereal &dXPm1) const
Definition: stepsol.cc:896
virtual ~Step2Integrator(void)
Definition: stepsol.cc:705
virtual integer GetIntegratorMaxIters(void) const
Definition: stepsol.cc:93
virtual doublereal TestScale(const NonlinearSolverTest *pTest, doublereal &dCoef) const
Definition: stepsol.cc:210
doublereal Advance(Solver *pS, const doublereal TStep, const doublereal, const StepChange, std::deque< MyVectorHandler * > &qX, std::deque< MyVectorHandler * > &qXPrime, MyVectorHandler *const pX, MyVectorHandler *const pXPrime, integer &EffIter, doublereal &Err, doublereal &SolErr)
Definition: stepsol.cc:270
DriveOwner AlgebraicRho
Definition: stepsol.h:516
VectorHandler * pXPrev
Definition: stepsol.h:270
void SetOrder(InverseDynamics::Order iOrder)
Definition: stepsol.cc:1510
virtual void AssRes(VectorHandler &ResHdl, doublereal dCoef)
Definition: elman.cc:498
doublereal dPredictDerivative(const doublereal &dXm1, const doublereal &dXPm1, DofOrder::Order o=DofOrder::DIFFERENTIAL) const
Definition: stepsol.cc:938
bool outputRes(void) const
virtual void Residual(VectorHandler *pRes) const =0
StepNIntegrator(const integer MaxIt, const doublereal dT, const doublereal dSolutionTol, const integer stp, const bool bmod_res_test)
Definition: stepsol.cc:446
DriveOwner AlgebraicRho
Definition: stepsol.h:593
bool outputJac(void) const
void SetDataManager(DataManager *pDatMan)
Definition: stepsol.cc:74
#define NO_OP
Definition: myassert.h:74
virtual doublereal TestScale(const NonlinearSolverTest *pTest, doublereal &dAlgebraicEqu) const
Definition: stepsol.cc:547
VectorHandler * pXPrimePrev
Definition: stepsol.h:271
Step2Integrator(const integer MaxIt, const doublereal dT, const doublereal dSolutionTol, const bool bmod_res_test)
Definition: stepsol.cc:692
virtual void DerivativesUpdate(void) const
Definition: dataman2.cc:2587
MyVectorHandler XTau
Definition: stepsol.h:157
virtual integer iGetSize(void) const =0
virtual doublereal Norm(void) const
Definition: vh.cc:262
VectorHandler * pXPrev2
Definition: stepsol.h:434
GradientExpression< UnaryExpr< FuncFabs, Expr > > fabs(const GradientExpression< Expr > &u)
Definition: gradient.h:2973
DriveOwner Rho
Definition: stepsol.h:515
virtual VectorHandler & ScalarMul(const VectorHandler &VH, const doublereal &d)
Definition: vh.cc:519
virtual void AfterConvergence(void) const
Definition: dataman2.cc:2527
virtual void Resize(integer iSize)
Definition: vh.cc:347
doublereal dPredictDerivative(const doublereal &dXm1, const doublereal &dXPm1, DofOrder::Order o=DofOrder::DIFFERENTIAL) const
Definition: stepsol.cc:860
bool bJacobian(void) const
Definition: stepsol.cc:1522
InverseDynamicsStepSolver(const integer MaxIt, const doublereal dT, const doublereal dSolutionTol, const integer stp, const integer sts, const bool bmod_res_test)
Definition: stepsol.cc:1378
doublereal dPredictDerivative(const doublereal &dXm1, const doublereal &dXm2, const doublereal &dXPm1, const doublereal &dXPm2, DofOrder::Order o=DofOrder::DIFFERENTIAL) const
Definition: stepsol.cc:1102
virtual void IDAfterConvergence(void) const
Definition: invdataman.cc:834
CrankNicolsonIntegrator(const doublereal Tl, const doublereal dSolTl, const integer iMaxIt, const bool bmod_res_test)
Definition: stepsol.cc:836
virtual doublereal dPredState(const doublereal &dXm1, const doublereal &dXm2, const doublereal &dXP, const doublereal &dXPm1, const doublereal &dXPm2) const =0
virtual void AfterPredict(void) const
Definition: dataman2.cc:2472
InverseDynamics::Order iOrder
Definition: stepsol.h:679
doublereal a[2][2]
Definition: stepsol.h:597
void Residual(VectorHandler *pRes) const
Definition: stepsol.cc:393
bool outputSol(void) const
void LinkToSolution(VectorHandler &XCurr, VectorHandler &XPrimeCurr)
Definition: dataman2.cc:172
virtual void SetDrvHdl(const DriveHandler *pDH)
Definition: drive.cc:487
doublereal mp[2]
Definition: stepsol.h:521
virtual doublereal dPredStateAlg(const doublereal &dXm1, const doublereal &dXP, const doublereal &dXPm1) const =0
InverseDynamics::Order GetOrder(void) const
Definition: stepsol.cc:1516
const DataManager::DofVecType * pDofs
Definition: stepsol.h:94
virtual void SolveT(void)
Definition: solman.cc:78
doublereal b[2][2]
Definition: stepsol.h:598
doublereal dPredState(const doublereal &dXm1, const doublereal &dXm2, const doublereal &dXP, const doublereal &dXPm1, const doublereal &dXPm2) const
Definition: stepsol.cc:1142
doublereal dPredStateAlg(const doublereal &dXm1, const doublereal &dXP, const doublereal &dXPm1) const
Definition: stepsol.cc:903
~HopeSolver(void)
Definition: stepsol.cc:1188
virtual MatrixHandler * pMatHdl(void) const =0
virtual doublereal dPredState(const doublereal &dXm1, const doublereal &dXP, const doublereal &dXPm1) const =0
doublereal dPredState(const doublereal &dXm1, const doublereal &dXP, const doublereal &dXPm1) const
Definition: stepsol.cc:966
virtual doublereal Advance(Solver *pS, const doublereal TStep, const doublereal dAlph, const StepChange StType, std::deque< MyVectorHandler * > &qX, std::deque< MyVectorHandler * > &qXPrime, MyVectorHandler *const pX, MyVectorHandler *const pXPrime, integer &EffIter, doublereal &Err, doublereal &SolErr)
Definition: stepsol.cc:623
doublereal dPredDerAlg(const doublereal &dXm1, const doublereal &dXPm1, const doublereal &dXPm2) const
Definition: stepsol.cc:1344
Definition: mbdyn.h:76
DataManager * pDM
Definition: stepsol.h:93
HopeSolver(const doublereal Tl, const doublereal dSolTol, const integer iMaxIt, const DriveCaller *pRho, const DriveCaller *pAlgRho, const bool bmod_res_test)
Definition: stepsol.cc:1175
void Update(const VectorHandler *pSol) const
Definition: stepsol.cc:428
MyVectorHandler SavedState
Definition: stepsol.h:158
VectorHandler * pXPrimePrimeCurr
Definition: stepsol.h:685
doublereal copysign(doublereal x, doublereal y)
Definition: gradient.h:97
virtual VectorHandler & ScalarMul(const VectorHandler &VH, const doublereal &d)
Definition: vh.cc:145
virtual NonlinearSolver * pGetNonlinearSolver(void) const
Definition: solver.h:404
virtual doublereal GetIntegratorDTol(void) const
Definition: stepsol.cc:99
VectorHandler * pXPrimeCurr
Definition: stepsol.h:164
virtual ~StepIntegrator(void)
Definition: stepsol.cc:68
#define DEBUGCOUT(msg)
Definition: myassert.h:232
virtual void Predict(void)
Definition: stepsol.cc:757
VectorHandler * pXCurr
Definition: stepsol.h:163
bool bStep
Definition: stepsol.h:595
integer MaxIters
Definition: stepsol.h:98
void UpdateDof(const int DCount, const DofOrder::Order Order, const VectorHandler *const pSol=0) const
Definition: stepsol.cc:516
Definition: mbdyn.h:77
virtual void MatrReset(void)=0
void PredictDof(const int DCount, const DofOrder::Order Order, const VectorHandler *const pSol=0) const
Definition: stepsol.cc:714
virtual void AssJac(MatrixHandler &JacHdl, doublereal dCoef)
Definition: elman.cc:392
virtual doublereal Advance(Solver *pS, const doublereal TStep, const doublereal dAlph, const StepChange StType, std::deque< MyVectorHandler * > &qX, std::deque< MyVectorHandler * > &qXPrime, MyVectorHandler *const pX, MyVectorHandler *const pXPrime, integer &EffIter, doublereal &Err, doublereal &SolErr)
Definition: stepsol.h:708
virtual void Jacobian(MatrixHandler *pJac) const
Definition: stepsol.cc:472
virtual void Solve(void)=0
void SetDriveHandler(const DriveHandler *pDH)
Definition: stepsol.cc:1011
virtual doublereal TestScale(const NonlinearSolverTest *pTest, doublereal &dCoef) const
Definition: stepsol.cc:1471
doublereal dPredictState(const doublereal &dXm1, const doublereal &dXP, const doublereal &dXPm1, DofOrder::Order o=DofOrder::DIFFERENTIAL) const
Definition: stepsol.cc:946
doublereal dPredDerAlg(const doublereal &dXm1, const doublereal &dXPm1) const
Definition: stepsol.cc:974
virtual ~StepNIntegrator(void)
Definition: stepsol.cc:458
MyVectorHandler SavedDerState
Definition: stepsol.h:159
doublereal dPredictState(const doublereal &dXm1, const doublereal &dXm2, const doublereal &dXP, const doublereal &dXPm1, const doublereal &dXPm2, DofOrder::Order o=DofOrder::DIFFERENTIAL) const
Definition: stepsol.cc:1115
void SetCoef(doublereal dT, doublereal dAlpha, enum StepChange NewStep)
Definition: stepsol.cc:1201
~ImplicitEulerIntegrator(void)
Definition: stepsol.cc:923
DofVecType::const_iterator DofIterator_const
Definition: dataman.h:802
#define ASSERT(expression)
Definition: colamd.c:977
doublereal dPredDer(const doublereal &dXm1, const doublereal &dXm2, const doublereal &dXPm1, const doublereal &dXPm2) const
Definition: stepsol.cc:1133
Order
Definition: shapefnc.h:42
virtual void EvalProd(doublereal Tau, const VectorHandler &f0, const VectorHandler &w, VectorHandler &z) const
Definition: stepsol.cc:1406
virtual void AssConstrJac(MatrixHandler &JacHdl)
Definition: invdataman.cc:94
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
virtual void Reset(void)
Definition: vh.cc:459
~MultistepSolver(void)
Definition: stepsol.cc:1005
const DofVecType & GetDofs(void) const
Definition: dataman.h:806
virtual doublereal dPredStateAlg(const doublereal &dXm1, const doublereal &dXP, const doublereal &dXPm1, const doublereal &dXPm2) const =0
DriveCaller * pGetDriveCaller(void) const
Definition: drive.cc:658
doublereal db0Differential
Definition: stepsol.h:235
virtual void SetCoef(doublereal dT, doublereal dAlpha, enum StepChange NewStep)=0
MyVectorHandler SavedState
Definition: stepsol.h:673
Definition: solver.h:78
void Residual(VectorHandler *pRes) const
Definition: stepsol.cc:1683
virtual void SetDriveHandler(const DriveHandler *pDH)
Definition: stepsol.cc:117
MultistepSolver(const doublereal Tl, const doublereal dSolTol, const integer iMaxIt, const DriveCaller *pRho, const DriveCaller *pAlgRho, const bool bmod_res_test)
Definition: stepsol.cc:992
MyVectorHandler SavedDerState
Definition: stepsol.h:676
const int iMaxIterCoef
Definition: stepsol.h:188
virtual const doublereal & dScaleCoef(const integer &iIndex) const
Definition: nonlin.cc:167
ImplicitEulerIntegrator(const doublereal Tl, const doublereal dSolTl, const integer iMaxIt, const bool bmod_res_test)
Definition: stepsol.cc:914
VectorHandler * pXPrimePrev
Definition: stepsol.h:435
doublereal dPredState(const doublereal &dXm1, const doublereal &dXm2, const doublereal &dXP, const doublereal &dXPm1, const doublereal &dXPm2) const
Definition: stepsol.cc:1328
doublereal dPredStateAlg(const doublereal &dXm1, const doublereal &dXP, const doublereal &dXPm1) const
Definition: stepsol.cc:981
VectorHandler * pXCurr
Definition: stepsol.h:683
virtual doublereal Advance(Solver *pS, const doublereal TStep, const doublereal dAlph, const StepChange StType, std::deque< MyVectorHandler * > &qX, std::deque< MyVectorHandler * > &qXPrime, MyVectorHandler *const pX, MyVectorHandler *const pXPrime, integer &EffIter, doublereal &Err, doublereal &SolErr)
Definition: stepsol.cc:765
void Update(const VectorHandler *pSol) const
Definition: stepsol.cc:1705
virtual integer GetIntegratorNumPreviousStates(void) const
Definition: stepsol.cc:81
static unsigned steps
doublereal dGet(const doublereal &dVar) const
Definition: drive.cc:664
doublereal dPredDer(const doublereal &dXm1, const doublereal &dXPm1) const
Definition: stepsol.cc:959
virtual void PrintSolution(const VectorHandler &Sol, integer iIterCnt) const
Definition: solver.cc:5200
virtual ~Step1Integrator(void)
Definition: stepsol.cc:571
const Solver * GetSolver(void) const
Definition: dataman.h:343
virtual void Update(const VectorHandler *pSol) const
Definition: stepsol.cc:538
doublereal db0Algebraic
Definition: stepsol.h:236
virtual void Update(void) const
Definition: dataman2.cc:2511
DriveOwner Rho
Definition: stepsol.h:592
virtual doublereal TestScale(const NonlinearSolverTest *pTest, doublereal &dAlgebraicEqu) const
Definition: stepsol.cc:439
doublereal a[2][2]
Definition: stepsol.h:518
void PredictDof(const int DCount, const DofOrder::Order Order, const VectorHandler *const pSol=0) const
Definition: stepsol.cc:581
virtual VectorHandler * pSolHdl(void) const =0
virtual void Update(const VectorHandler *pSol) const =0
virtual void AssConstrRes(VectorHandler &ResHdl, InverseDynamics::Order iOrder)
Definition: invdataman.cc:303
void UpdateLoop(const T *const t, void(T::*pUpd)(const int DCount, const DofOrder::Order Order, const VectorHandler *const pSol) const, const VectorHandler *const pSol=0) const
doublereal b[3][2]
Definition: stepsol.h:519
void SetDriveHandler(const DriveHandler *pDH)
Definition: stepsol.cc:1194
virtual doublereal dPredDer(const doublereal &dXm1, const doublereal &dXm2, const doublereal &dXPm1, const doublereal &dXPm2) const =0
virtual void Solve(const NonlinearProblem *pNLP, Solver *pS, const integer iMaxIter, const doublereal &Tol, integer &iIterCnt, doublereal &dErr, const doublereal &SolTol, doublereal &dSolErr)=0
double doublereal
Definition: colamd.c:52
doublereal mp[2]
Definition: stepsol.h:600
long int integer
Definition: colamd.c:51
virtual void Residual(VectorHandler *pRes) const
Definition: stepsol.cc:464
VectorHandler * pLambdaCurr
Definition: stepsol.h:686
void SetCoef(doublereal dT, doublereal dAlpha, enum StepChange NewStep)
Definition: stepsol.cc:929
doublereal np[2]
Definition: stepsol.h:522
~CrankNicolsonIntegrator(void)
Definition: stepsol.cc:845
doublereal dTol
Definition: stepsol.h:99
VectorHandler * pXPrimePrev2
Definition: stepsol.h:435
doublereal dPredictDerivative(const doublereal &dXm1, const doublereal &dXm2, const doublereal &dXPm1, const doublereal &dXPm2, DofOrder::Order o=DofOrder::DIFFERENTIAL) const
Definition: stepsol.cc:1281
virtual integer iGetNumRows(void) const =0
VectorHandler * pXPrev
Definition: stepsol.h:434
integer iGetNumDofs(void) const
Definition: dataman.h:809
virtual integer GetIntegratorNumUnknownStates(void) const
Definition: stepsol.cc:87
virtual doublereal GetIntegratorDSolTol(void) const
Definition: stepsol.cc:105
virtual doublereal dPredDerAlg(const doublereal &dXm1, const doublereal &dXPm1) const =0
virtual ~ImplicitStepIntegrator(void)
Definition: stepsol.cc:139