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;
42 inline int size(
const fmatvec::Vec& v) {
49 namespace BoostOdeintHelper {
55 inline void assign(fmatvec::Vec &d,
const fmatvec::Vec &s) {
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 std::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;
166 typename DOS::state_type zTemp;
169 fmatvec::Vec zDisturbed;
183 template<
typename DOS>
184 void BoostOdeintDOS<DOS>::zd(
const typename DOS::state_type &z,
typename DOS::state_type &zd,
const double t) {
188 BoostOdeintHelper::assign(system->getState(), z);
189 system->resetUpToDate();
190 BoostOdeintHelper::assign(zd, system->evalzd());
195 template<
typename DOS>
196 void BoostOdeintDOS<DOS>::jac(
const typename DOS::state_type &z, boost::numeric::ublas::matrix<double> &jac,
const double t,
197 typename DOS::state_type &ft) {
200 if(static_cast<size_t>(z.size())!=static_cast<size_t>(jac.size1()) ||
201 static_cast<size_t>(z.size())!=static_cast<size_t>(jac.size2()))
202 jac.resize(z.size(), z.size());
204 BoostOdeintHelper::assign(system->getState(), z);
205 system->resetUpToDate();
206 BoostOdeintHelper::assign(zDisturbed, z);
207 fmatvec::Vec zd0=system->evalzd();
209 for(
size_t i=0; i<static_cast<size_t>(z.size()); ++i) {
210 zDisturbed(i)+=MBSim::epsroot;
211 system->setState(zDisturbed);
212 system->resetUpToDate();
213 auto col=boost::numeric::ublas::column(jac, i);
214 auto zd=(system->evalzd()-zd0)*MBSim::epsrootInv;
215 std::copy(zd.begin(), zd.end(), col.begin());
216 zDisturbed(i)-=MBSim::epsroot;
219 system->setTime(t+MBSim::epsroot);
220 system->setState(zDisturbed);
221 system->resetUpToDate();
222 BoostOdeintHelper::assign(ft, (system->evalzd()-zd0)*MBSim::epsrootInv);
225 template<
typename DOS>
232 template<
typename DOS>
244 system->setTime(tStart);
248 if(z0.size()!=system->getzSize()+system->getisSize())
249 throwError(
"BoostOdeintDOS:: size of z0 does not match, must be " + std::to_string(system->getzSize()+system->getisSize()));
250 BoostOdeintHelper::assign(zTemp, z0(fmatvec::RangeV(0,system->getzSize()-1)));
251 system->setInternalState(z0(fmatvec::RangeV(system->getzSize(),z0.size()-1)));
254 BoostOdeintHelper::assign(zTemp, system->evalz0());
256 BoostOdeintHelper::assign(system->getState(), zTemp);
257 system->resetUpToDate();
258 system->computeInitialCondition();
262 tPlot=tStart+plotSample*dtPlot;
264 svLast <<= system->evalsv();
265 BoostOdeintHelper::assign(zTemp, system->getState());
268 dos.reset(
new DOS(aTol, rTol, dtMax));
269 dos->initialize(zTemp, tStart, dt0);
272 template<
typename DOS>
273 void BoostOdeintDOS<DOS>::subIntegrate(
double tSamplePoint) {
275 static constexpr
double oneMinusEps = 1 - std::numeric_limits<double>::epsilon();
276 while(dos->current_time()<tSamplePoint*oneMinusEps) {
279 #if BOOST_VERSION >= 107100
281 dos->setDtMax(std::min(dtMax, tSamplePoint-dos->current_time()));
283 auto step=dos->do_step(boostOdeintSystem());
286 double curTimeAndState=dos->current_time();
287 system->setTime(dos->current_time());
288 BoostOdeintHelper::assign(system->getState(), dos->current_state());
289 system->resetUpToDate();
291 shift=signChangedWRTsvLast(system->evalsv());
295 double dt=step.second-step.first;
298 double tRoot=step.second-dt;
299 dos->calc_state(tRoot, zTemp);
300 curTimeAndState=tRoot;
301 system->setTime(tRoot);
302 BoostOdeintHelper::assign(system->getState(), zTemp);
303 system->resetUpToDate();
305 if(signChangedWRTsvLast(system->evalsv()))
310 dos->calc_state(step.second, zTemp);
311 curTimeAndState=step.second;
312 system->setTime(step.second);
313 BoostOdeintHelper::assign(system->getState(), zTemp);
314 system->resetUpToDate();
316 auto &sv=system->evalsv();
317 auto &jsv=system->getjsv();
318 for(
int i=0; i<sv.size(); ++i)
319 jsv(i)=svLast(i)*sv(i)<0;
324 static constexpr
double onePlusEps = 1 + std::numeric_limits<double>::epsilon();
325 while(tPlot<=step.second*onePlusEps
326 #
if BOOST_VERSION < 107100
327 && tPlot<=tSamplePoint*onePlusEps
330 dos->calc_state(tPlot, zTemp);
331 if(curTimeAndState!=tPlot) {
332 curTimeAndState=tPlot;
333 system->setTime(tPlot);
334 BoostOdeintHelper::assign(system->getState(), zTemp);
335 system->resetUpToDate();
340 msg(Status)<<
"t = "<<tPlot<<
", dt="<<dos->current_time_step()<<
" "<<std::flush;
341 system->updateInternalState();
343 tPlot=tStart+plotSample*dtPlot;
351 msg(Status)<<
"t = "<<step.second<<
", dt="<<dos->current_time_step()<<
" "<<std::flush;
354 #if BOOST_VERSION < 107100
355 if(step.second<tSamplePoint) {
359 dos->calc_state(step.second, zTemp);
360 if(curTimeAndState!=step.second) {
361 curTimeAndState=step.second;
362 system->setTime(step.second);
363 BoostOdeintHelper::assign(system->getState(), zTemp);
364 system->resetUpToDate();
371 system->resetUpToDate();
378 svLast=system->evalsv();
380 BoostOdeintHelper::assign(zTemp, system->getState());
381 dos->initialize(zTemp, step.second, dos->current_time_step());
386 if((gMax>=0 || gdMax>=0) && curTimeAndState!=step.second) {
387 curTimeAndState=step.second;
388 system->setTime(step.second);
389 BoostOdeintHelper::assign(system->getState(), dos->current_state());
390 system->resetUpToDate();
393 bool reinitNeeded=
false;
394 if(gMax>=0 and system->positionDriftCompensationNeeded(gMax)) {
395 system->projectGeneralizedPositions(3);
396 system->projectGeneralizedVelocities(3);
399 else if(gdMax>=0 and system->velocityDriftCompensationNeeded(gdMax)) {
400 system->projectGeneralizedVelocities(3);
405 BoostOdeintHelper::assign(zTemp, system->getState());
406 dos->initialize(zTemp, step.second, dos->current_time_step());
408 system->updateStopVectorParameters();
410 #if BOOST_VERSION < 107100
418 dos->calc_state(tSamplePoint, zTemp);
419 curTimeAndState=tSamplePoint;
420 system->setTime(tSamplePoint);
421 BoostOdeintHelper::assign(system->getState(), zTemp);
422 system->resetUpToDate();
423 dos->initialize(zTemp, tSamplePoint, dos->current_time_step());
424 system->updateStopVectorParameters();
428 #if BOOST_VERSION >= 107100
431 if(curTimeAndState!=dos->current_time()) {
432 system->setTime(dos->current_time());
433 BoostOdeintHelper::assign(system->getState(), dos->current_state());
434 system->resetUpToDate();
437 system->updateInternalState();
441 template<
typename DOS>
442 void BoostOdeintDOS<DOS>::postIntegrate() {
443 msg(Info)<<std::endl;
444 msg(Info)<<
"Integration statistics:"<<std::endl;
445 msg(Info)<<
"nrSteps = "<<nrSteps<<std::endl;
446 msg(Info)<<
"nrRHS = "<<nrRHS<<std::endl;
447 msg(Info)<<
"nrJacs = "<<nrJacs<<std::endl;
448 msg(Info)<<
"nrPlots = "<<nrPlots<<std::endl;
449 msg(Info)<<
"nrSVs = "<<nrSVs<<std::endl;
450 msg(Info)<<
"nrRoots = "<<nrRoots<<std::endl;
451 msg(Info)<<
"nrDriftCorr = "<<nrDriftCorr<<std::endl;
456 template<
bool,
typename Self>
457 struct InitializeUsingXMLControlled;
460 template<
typename Self>
461 struct InitializeUsingXMLControlled<true, Self> {
462 static void call(Self
self, xercesc::DOMElement* element) {
463 xercesc::DOMElement *e;
465 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"absoluteToleranceScalar");
466 if(e)
self->setAbsoluteTolerance(MBXMLUtils::E(e)->getText<double>());
468 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"relativeToleranceScalar");
469 if(e)
self->setRelativeTolerance(MBXMLUtils::E(e)->getText<double>());
471 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"maximumStepSize");
472 if(e)
self->setMaximumStepSize(MBXMLUtils::E(e)->getText<double>());
477 template<
typename Self>
478 struct InitializeUsingXMLControlled<false, Self> {
479 static void call(Self
self, xercesc::DOMElement* element) {
480 xercesc::DOMElement *e;
484 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"absoluteToleranceScalar");
485 if(e)
self->throwError(
"absoluteToleranceScalar element used for an fixed step-size stepper.");
487 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"relativeToleranceScalar");
488 if(e)
self->throwError(
"relativeToleranceScalar element used for an fixed step-size stepper.");
490 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"maximumStepSize");
491 if(e)
self->throwError(
"maximumStepSize element used for an fixed step-size stepper.");
496 template<
typename DOS>
499 xercesc::DOMElement *e;
501 InitializeUsingXMLControlled<isControlled, decltype(this)>::call(
this, element);
503 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"initialStepSize");
504 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:497
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:226
Definition: boost_odeint_integrator.h:75