18 #ifndef _BOOST_ODEINT_INTEGRATOR_H_ 19 #define _BOOST_ODEINT_INTEGRATOR_H_ 21 #include "root_finding_integrator.h" 22 #include <mbsim/dynamic_system_solver.h> 23 #include <mbsim/utils/eps.h> 24 #include <boost/numeric/odeint/util/is_resizeable.hpp> 25 #include <boost/numeric/odeint/stepper/stepper_categories.hpp> 26 #include <boost/numeric/ublas/matrix.hpp> 27 #include <boost/numeric/ublas/matrix_proxy.hpp> 34 struct is_resizeable<fmatvec::Vec> {
35 typedef boost::true_type type;
36 static const bool value=
true;
49 namespace BoostOdeintHelper {
56 if(d.size()!=s.size()) d.resize(s.size(),fmatvec::NONINIT);
59 template<
typename Dst,
typename Src>
60 inline void assign(Dst &d,
const Src &s) {
61 if(static_cast<size_t>(d.size())!=static_cast<size_t>(s.size()))
63 copy(s.begin(), s.end(), d.begin());
74 template<
class SystemCategory,
class ZdFunc,
class JacFunc>
77 template<
class ZdFunc,
class JacFunc>
81 const ZdFunc& operator()()
const {
return zdFunc_; }
83 const ZdFunc& zdFunc_;
86 template<
class ZdFunc,
class JacFunc>
89 BoostOdeintSystem(
const ZdFunc& zdFunc__,
const JacFunc& jacFunc__) : implFunc_(make_pair(zdFunc__, jacFunc__)) {}
90 const std::pair<ZdFunc, JacFunc>& operator()()
const {
return implFunc_; }
92 const std::pair<ZdFunc, JacFunc> implFunc_;
102 template<
typename DOS>
106 static constexpr
bool isControlled{std::is_base_of<boost::numeric::odeint::controlled_stepper_tag,
107 typename DOS::UnderlayingStepperCategory>::value};
110 using EnableIfControlled=
typename std::enable_if<std::is_base_of<boost::numeric::odeint::controlled_stepper_tag,
111 typename H::UnderlayingStepperCategory>::value>::type;
115 void preIntegrate()
override;
116 void subIntegrate(
double tSamplePoint)
override;
117 void postIntegrate()
override;
123 template<
typename H=DOS,
typename=EnableIfControlled<H>>
127 template<
typename H=DOS,
typename=EnableIfControlled<H>>
131 template<
typename H=DOS,
typename=EnableIfControlled<H>>
137 void zd(
const typename DOS::state_type &z,
typename DOS::state_type &zd,
const double t);
139 void jac(
const typename DOS::state_type &z, boost::numeric::ublas::matrix<double> &jac,
const double t,
140 typename DOS::state_type &ft);
142 const std::function<void(
const typename DOS::state_type&,
typename DOS::state_type&,
const double)> zdFunc
145 const std::function<void(
const typename DOS::state_type&, boost::numeric::ublas::matrix<double>&c,
const double,
146 typename DOS::state_type&)> jacFunc
147 {bind(&BoostOdeintDOS<DOS>::jac,
this, std::placeholders::_1, std::placeholders::_2,
148 std::placeholders::_3, std::placeholders::_4)};
151 const BoostOdeintHelper::BoostOdeintSystem<
typename DOS::SystemCategory, decltype(zdFunc), decltype(jacFunc)>
152 boostOdeintSystem{zdFunc, jacFunc};
155 std::unique_ptr<DOS> dos;
165 typename DOS::state_type zTemp;
182 template<
typename DOS>
183 void BoostOdeintDOS<DOS>::zd(
const typename DOS::state_type &z,
typename DOS::state_type &zd,
const double t) {
187 BoostOdeintHelper::assign(system->getState(), z);
188 system->resetUpToDate();
189 BoostOdeintHelper::assign(zd, system->evalzd());
194 template<
typename DOS>
195 void BoostOdeintDOS<DOS>::jac(
const typename DOS::state_type &z, boost::numeric::ublas::matrix<double> &jac,
const double t,
196 typename DOS::state_type &ft) {
199 if(static_cast<size_t>(z.size())!=static_cast<size_t>(jac.size1()) ||
200 static_cast<size_t>(z.size())!=static_cast<size_t>(jac.size2()))
201 jac.resize(z.size(), z.size());
203 BoostOdeintHelper::assign(system->getState(), z);
204 system->resetUpToDate();
205 BoostOdeintHelper::assign(zDisturbed, z);
208 for(
size_t i=0; i<static_cast<size_t>(z.size()); ++i) {
209 zDisturbed(i)+=MBSim::epsroot;
210 system->setState(zDisturbed);
211 system->resetUpToDate();
212 auto col=boost::numeric::ublas::column(jac, i);
213 auto zd=(system->evalzd()-zd0)*MBSim::epsrootInv;
214 copy(zd.begin(), zd.end(), col.begin());
215 zDisturbed(i)-=MBSim::epsroot;
218 system->setTime(t+MBSim::epsroot);
219 system->setState(zDisturbed);
220 system->resetUpToDate();
221 BoostOdeintHelper::assign(ft, (system->evalzd()-zd0)*MBSim::epsrootInv);
224 template<
typename DOS>
231 template<
typename DOS>
243 system->setTime(tStart);
247 if(z0.size()!=system->getzSize()+system->getisSize())
248 throwError(
"BoostOdeintDOS:: size of z0 does not match, must be " + std::to_string(system->getzSize()+system->getisSize()));
249 BoostOdeintHelper::assign(zTemp, z0(fmatvec::RangeV(0,system->getzSize()-1)));
250 system->setcuris(z0(fmatvec::RangeV(system->getzSize(),z0.size()-1)));
253 BoostOdeintHelper::assign(zTemp, system->evalz0());
255 BoostOdeintHelper::assign(system->getState(), zTemp);
256 system->resetUpToDate();
257 system->computeInitialCondition();
262 svLast <<= system->evalsv();
265 dos.reset(
new DOS(aTol, rTol, dtMax));
266 dos->initialize(zTemp, tStart, dt0);
269 template<
typename DOS>
270 void BoostOdeintDOS<DOS>::subIntegrate(
double tSamplePoint) {
272 while(dos->current_time()<tSamplePoint) {
275 auto step=dos->do_step(boostOdeintSystem());
276 system->updateInternalState();
279 double curTimeAndState=dos->current_time();
280 system->setTime(dos->current_time());
281 BoostOdeintHelper::assign(system->getState(), dos->current_state());
282 system->resetUpToDate();
284 shift=signChangedWRTsvLast(system->evalsv());
288 double dt=step.second-step.first;
291 double tRoot=step.second-dt;
292 dos->calc_state(tRoot, zTemp);
293 curTimeAndState=tRoot;
294 system->setTime(tRoot);
295 BoostOdeintHelper::assign(system->getState(), zTemp);
296 system->resetUpToDate();
298 if(signChangedWRTsvLast(system->evalsv()))
303 dos->calc_state(step.second, zTemp);
304 curTimeAndState=step.second;
305 system->setTime(step.second);
306 BoostOdeintHelper::assign(system->getState(), zTemp);
307 system->resetUpToDate();
309 auto &sv=system->evalsv();
310 auto &jsv=system->getjsv();
311 for(
int i=0; i<sv.size(); ++i)
312 jsv(i)=svLast(i)*sv(i)<0;
317 while(tPlot<=step.second && tPlot<tSamplePoint) {
318 dos->calc_state(tPlot, zTemp);
319 if(curTimeAndState!=tPlot) {
320 curTimeAndState=tPlot;
321 system->setTime(tPlot);
322 BoostOdeintHelper::assign(system->getState(), zTemp);
323 system->resetUpToDate();
328 msg(Status)<<
"t = "<<tPlot<<
", dt="<<dos->current_time_step()<<
" "<<std::flush;
337 msg(Status)<<
"t = "<<step.second<<
", dt="<<dos->current_time_step()<<
" "<<std::flush;
340 if(step.second<tSamplePoint) {
343 dos->calc_state(step.second, zTemp);
344 if(curTimeAndState!=step.second) {
345 curTimeAndState=step.second;
346 system->setTime(step.second);
347 BoostOdeintHelper::assign(system->getState(), zTemp);
348 system->resetUpToDate();
355 system->resetUpToDate();
362 svLast=system->evalsv();
364 BoostOdeintHelper::assign(zTemp, system->getState());
365 dos->initialize(zTemp, step.second, dos->current_time_step());
369 bool reinitNeeded=
false;
370 if(gMax>=0 and system->positionDriftCompensationNeeded(gMax)) {
371 system->projectGeneralizedPositions(3);
372 system->projectGeneralizedVelocities(3);
375 else if(gdMax>=0 and system->velocityDriftCompensationNeeded(gdMax)) {
376 system->projectGeneralizedVelocities(3);
381 BoostOdeintHelper::assign(zTemp, system->getState());
382 dos->initialize(zTemp, step.second, dos->current_time_step());
384 system->updateStopVectorParameters();
393 dos->calc_state(tSamplePoint, zTemp);
394 curTimeAndState=tSamplePoint;
395 system->setTime(tSamplePoint);
396 BoostOdeintHelper::assign(system->getState(), zTemp);
397 dos->initialize(zTemp, tSamplePoint, dos->current_time_step());
398 system->updateStopVectorParameters();
403 template<
typename DOS>
404 void BoostOdeintDOS<DOS>::postIntegrate() {
405 msg(Info)<<std::endl;
406 msg(Info)<<
"Integration statistics:"<<std::endl;
407 msg(Info)<<
"nrSteps = "<<nrSteps<<std::endl;
408 msg(Info)<<
"nrRHS = "<<nrRHS<<std::endl;
409 msg(Info)<<
"nrJacs = "<<nrJacs<<std::endl;
410 msg(Info)<<
"nrPlots = "<<nrPlots<<std::endl;
411 msg(Info)<<
"nrSVs = "<<nrSVs<<std::endl;
412 msg(Info)<<
"nrRoots = "<<nrRoots<<std::endl;
413 msg(Info)<<
"nrDriftCorr = "<<nrDriftCorr<<std::endl;
418 template<
bool,
typename Self>
419 struct InitializeUsingXMLControlled;
422 template<
typename Self>
423 struct InitializeUsingXMLControlled<true, Self> {
424 static void call(Self
self, xercesc::DOMElement* element) {
425 xercesc::DOMElement *e;
427 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"absoluteToleranceScalar");
428 if(e)
self->setAbsoluteTolerance(MBXMLUtils::E(e)->getText<double>());
430 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"relativeToleranceScalar");
431 if(e)
self->setRelativeTolerance(MBXMLUtils::E(e)->getText<double>());
433 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"maximumStepSize");
434 if(e)
self->setMaximumStepSize(MBXMLUtils::E(e)->getText<double>());
439 template<
typename Self>
440 struct InitializeUsingXMLControlled<false, Self> {
441 static void call(Self
self, xercesc::DOMElement* element) {
442 xercesc::DOMElement *e;
446 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"absoluteToleranceScalar");
447 if(e)
self->throwError(
"absoluteToleranceScalar element used for an fixed step-size stepper.");
449 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"relativeToleranceScalar");
450 if(e)
self->throwError(
"relativeToleranceScalar element used for an fixed step-size stepper.");
452 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"maximumStepSize");
453 if(e)
self->throwError(
"maximumStepSize element used for an fixed step-size stepper.");
458 template<
typename DOS>
461 xercesc::DOMElement *e;
463 InitializeUsingXMLControlled<isControlled, decltype(
this)>::call(
this, element);
465 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"initialStepSize");
466 if(e) setInitialStepSize(MBXMLUtils::E(e)->getText<double>());
void setMaximumStepSize(double dtMax_)
Set maximum step size.
Definition: boost_odeint_integrator.h:132
Integrator with root-finding.
Definition: root_finding_integrator.h:32
void setRelativeTolerance(double rTol_)
Set relative tolerance.
Definition: boost_odeint_integrator.h:128
Definition: boost_odeint_integrator.h:69
Definition: boost_odeint_integrator.h:67
void setInitialStepSize(double dt0_)
Set initial step size.
Definition: boost_odeint_integrator.h:120
Definition: boost_odeint_integrator.h:103
void initializeUsingXML(xercesc::DOMElement *element) override
initialize integrator
Definition: boost_odeint_integrator.h:459
virtual void initializeUsingXML(xercesc::DOMElement *element)
initialize integrator
Definition: root_finding_integrator.cc:44
void setAbsoluteTolerance(double aTol_)
Set absolute tolerance.
Definition: boost_odeint_integrator.h:124
void integrate() override
start the integration of the system set by setSystem. Each class implemeting this function should cal...
Definition: boost_odeint_integrator.h:225
Definition: boost_odeint_integrator.h:75
namespace MBSim
Definition: eigenanalyzer.cc:47