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 {
58 template<
typename Dst,
typename Src>
59 inline void assign(Dst &d,
const Src &s) {
60 if(static_cast<size_t>(d.size())!=static_cast<size_t>(s.size()))
62 copy(s.begin(), s.end(), d.begin());
73 template<
class SystemCategory,
class ZdFunc,
class JacFunc>
76 template<
class ZdFunc,
class JacFunc>
80 const ZdFunc& operator()()
const {
return zdFunc_; }
82 const ZdFunc& zdFunc_;
85 template<
class ZdFunc,
class JacFunc>
88 BoostOdeintSystem(
const ZdFunc& zdFunc__,
const JacFunc& jacFunc__) : implFunc_(make_pair(zdFunc__, jacFunc__)) {}
89 const std::pair<ZdFunc, JacFunc>& operator()()
const {
return implFunc_; }
91 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;
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());
193 template<
typename DOS>
194 void BoostOdeintDOS<DOS>::jac(
const typename DOS::state_type &z, boost::numeric::ublas::matrix<double> &jac,
const double t,
195 typename DOS::state_type &ft) {
198 if(static_cast<size_t>(z.size())!=static_cast<size_t>(jac.size1()) ||
199 static_cast<size_t>(z.size())!=static_cast<size_t>(jac.size2()))
200 jac.resize(z.size(), z.size());
202 BoostOdeintHelper::assign(system->getState(), z);
203 system->resetUpToDate();
204 BoostOdeintHelper::assign(zDisturbed, z);
205 zd0=system->evalzd();
207 for(
size_t i=0; i<static_cast<size_t>(z.size()); ++i) {
208 zDisturbed(i)+=MBSim::epsroot;
209 system->setState(zDisturbed);
210 system->resetUpToDate();
211 auto col=boost::numeric::ublas::column(jac, i);
212 auto zd=(system->evalzd()-zd0)*MBSim::epsrootInv;
213 copy(zd.begin(), zd.end(), col.begin());
214 zDisturbed(i)-=MBSim::epsroot;
217 system->setTime(t+MBSim::epsroot);
218 system->setState(zDisturbed);
219 system->resetUpToDate();
220 BoostOdeintHelper::assign(ft, (system->evalzd()-zd0)*MBSim::epsrootInv);
223 template<
typename DOS>
230 template<
typename DOS>
242 system->setTime(tStart);
246 if(z0.size()!=system->getzSize())
247 throwError(
"BoostOdeintDOS:: size of z0 does not match, must be " + std::to_string(system->getzSize()));
248 BoostOdeintHelper::assign(zTemp, z0);
251 BoostOdeintHelper::assign(zTemp, system->evalz0());
253 system->resetUpToDate();
254 system->computeInitialCondition();
259 svLast=system->evalsv();
262 #if BOOST_VERSION >= 106000
263 dos.reset(
new DOS(aTol, rTol, dtMax));
264 #else // boost odeint < 1.60 does not support dtMax
265 dos.reset(
new DOS(aTol, rTol));
266 msg(Warn)<<
"This build was done with boost < 1.60 which does not support a maximum step size."<<std::endl
267 <<
"Integrator will not limit the maximum step size."<<std::endl;
269 dos->initialize(zTemp, tStart, dt0);
272 template<
typename DOS>
273 void BoostOdeintDOS<DOS>::subIntegrate(
double tSamplePoint) {
275 while(dos->current_time()<tSamplePoint) {
278 auto step=dos->do_step(boostOdeintSystem());
281 double curTimeAndState=dos->current_time();
282 system->setTime(dos->current_time());
283 BoostOdeintHelper::assign(system->getState(), dos->current_state());
284 system->resetUpToDate();
286 shift=signChangedWRTsvLast(system->evalsv());
290 double dt=step.second-step.first;
293 double tRoot=step.second-dt;
294 dos->calc_state(tRoot, zTemp);
295 curTimeAndState=tRoot;
296 system->setTime(tRoot);
297 BoostOdeintHelper::assign(system->getState(), zTemp);
298 system->resetUpToDate();
300 if(signChangedWRTsvLast(system->evalsv()))
305 dos->calc_state(step.second, zTemp);
306 curTimeAndState=step.second;
307 system->setTime(step.second);
308 BoostOdeintHelper::assign(system->getState(), zTemp);
309 system->resetUpToDate();
311 auto &sv=system->evalsv();
312 auto &jsv=system->getjsv();
313 for(
int i=0; i<sv.size(); ++i)
314 jsv(i)=svLast(i)*sv(i)<0;
318 while(tPlot<=step.second && tPlot<tSamplePoint) {
319 dos->calc_state(tPlot, zTemp);
320 if(curTimeAndState!=tPlot) {
321 curTimeAndState=tPlot;
322 system->setTime(tPlot);
323 BoostOdeintHelper::assign(system->getState(), zTemp);
324 system->resetUpToDate();
329 msg(Status)<<
"t = "<<tPlot<<
", dt="<<dos->current_time_step()<<std::flush;
333 if(step.second<tSamplePoint) {
336 dos->calc_state(step.second, zTemp);
337 if(curTimeAndState!=step.second) {
338 curTimeAndState=step.second;
339 system->setTime(step.second);
340 BoostOdeintHelper::assign(system->getState(), zTemp);
341 system->resetUpToDate();
348 system->resetUpToDate();
355 svLast=system->evalsv();
357 BoostOdeintHelper::assign(zTemp, system->getState());
358 dos->initialize(zTemp, step.second, dos->current_time_step());
362 bool reinitNeeded=
false;
363 if(gMax>=0 and system->positionDriftCompensationNeeded(gMax)) {
364 system->projectGeneralizedPositions(3);
365 system->projectGeneralizedVelocities(3);
368 else if(gdMax>=0 and system->velocityDriftCompensationNeeded(gdMax)) {
369 system->projectGeneralizedVelocities(3);
374 BoostOdeintHelper::assign(zTemp, system->getState());
375 dos->initialize(zTemp, step.second, dos->current_time_step());
377 system->updateStopVectorParameters();
386 dos->calc_state(tSamplePoint, zTemp);
387 curTimeAndState=tSamplePoint;
388 system->setTime(tSamplePoint);
389 BoostOdeintHelper::assign(system->getState(), zTemp);
390 dos->initialize(zTemp, tSamplePoint, dos->current_time_step());
391 system->updateStopVectorParameters();
396 template<
typename DOS>
397 void BoostOdeintDOS<DOS>::postIntegrate() {
398 msg(Info)<<std::endl;
399 msg(Info)<<
"Integration statistics:"<<std::endl;
400 msg(Info)<<
"nrSteps = "<<nrSteps<<std::endl;
401 msg(Info)<<
"nrRHS = "<<nrRHS<<std::endl;
402 msg(Info)<<
"nrJacs = "<<nrJacs<<std::endl;
403 msg(Info)<<
"nrPlots = "<<nrPlots<<std::endl;
404 msg(Info)<<
"nrSVs = "<<nrSVs<<std::endl;
405 msg(Info)<<
"nrRoots = "<<nrRoots<<std::endl;
406 msg(Info)<<
"nrDriftCorr = "<<nrDriftCorr<<std::endl;
411 template<
bool,
typename Self>
412 struct InitializeUsingXMLControlled;
415 template<
typename Self>
416 struct InitializeUsingXMLControlled<true, Self> {
417 static void call(Self
self, xercesc::DOMElement* element) {
418 xercesc::DOMElement *e;
420 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"absoluteToleranceScalar");
421 if(e)
self->setAbsoluteTolerance(MBXMLUtils::E(e)->getText<double>());
423 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"relativeToleranceScalar");
424 if(e)
self->setRelativeTolerance(MBXMLUtils::E(e)->getText<double>());
426 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"maximumStepSize");
427 if(e)
self->setMaximumStepSize(MBXMLUtils::E(e)->getText<double>());
432 template<
typename Self>
433 struct InitializeUsingXMLControlled<false, Self> {
434 static void call(Self
self, xercesc::DOMElement* element) {
435 xercesc::DOMElement *e;
439 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"absoluteToleranceScalar");
440 if(e)
self->throwError(
"absoluteToleranceScalar element used for an fixed step-size stepper.");
442 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"relativeToleranceScalar");
443 if(e)
self->throwError(
"relativeToleranceScalar element used for an fixed step-size stepper.");
445 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"maximumStepSize");
446 if(e)
self->throwError(
"maximumStepSize element used for an fixed step-size stepper.");
451 template<
typename DOS>
454 xercesc::DOMElement *e;
456 InitializeUsingXMLControlled<isControlled, decltype(this)>::call(
this, element);
458 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"initialStepSize");
459 if(e) setInitialStepSize(MBXMLUtils::E(e)->getText<double>());
void setMaximumStepSize(double dtMax_)
Set maximum step size.
Definition: boost_odeint_integrator.h:132
casadi::DM c(const double &x)
convert double to casadi::DM
Definition: symbolic_function.h:93
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:68
Definition: boost_odeint_integrator.h:66
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:452
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:224
Definition: boost_odeint_integrator.h:74