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());
192 template<
typename DOS>
193 void BoostOdeintDOS<DOS>::jac(
const typename DOS::state_type &z, boost::numeric::ublas::matrix<double> &jac,
const double t,
194 typename DOS::state_type &ft) {
197 if(static_cast<size_t>(z.size())!=static_cast<size_t>(jac.size1()) ||
198 static_cast<size_t>(z.size())!=static_cast<size_t>(jac.size2()))
199 jac.resize(z.size(), z.size());
201 BoostOdeintHelper::assign(system->getState(), z);
202 system->resetUpToDate();
203 BoostOdeintHelper::assign(zDisturbed, z);
206 for(
size_t i=0; i<static_cast<size_t>(z.size()); ++i) {
207 zDisturbed(i)+=MBSim::epsroot;
208 system->setState(zDisturbed);
209 system->resetUpToDate();
210 auto col=boost::numeric::ublas::column(jac, i);
211 auto zd=(system->evalzd()-zd0)*MBSim::epsrootInv;
212 copy(zd.begin(), zd.end(), col.begin());
213 zDisturbed(i)-=MBSim::epsroot;
216 system->setTime(t+MBSim::epsroot);
217 system->setState(zDisturbed);
218 system->resetUpToDate();
219 BoostOdeintHelper::assign(ft, (system->evalzd()-zd0)*MBSim::epsrootInv);
222 template<
typename DOS>
229 template<
typename DOS>
241 system->setTime(tStart);
245 if(z0.size()!=system->getzSize())
246 throwError(
"BoostOdeintDOS:: size of z0 does not match, must be " + std::to_string(system->getzSize()));
247 BoostOdeintHelper::assign(zTemp, z0);
250 BoostOdeintHelper::assign(zTemp, system->evalz0());
252 system->resetUpToDate();
253 system->computeInitialCondition();
258 svLast <<= system->evalsv();
261 dos.reset(
new DOS(aTol, rTol, dtMax));
262 dos->initialize(zTemp, tStart, dt0);
265 template<
typename DOS>
266 void BoostOdeintDOS<DOS>::subIntegrate(
double tSamplePoint) {
268 while(dos->current_time()<tSamplePoint) {
271 auto step=dos->do_step(boostOdeintSystem());
274 double curTimeAndState=dos->current_time();
275 system->setTime(dos->current_time());
276 BoostOdeintHelper::assign(system->getState(), dos->current_state());
277 system->resetUpToDate();
279 shift=signChangedWRTsvLast(system->evalsv());
283 double dt=step.second-step.first;
286 double tRoot=step.second-dt;
287 dos->calc_state(tRoot, zTemp);
288 curTimeAndState=tRoot;
289 system->setTime(tRoot);
290 BoostOdeintHelper::assign(system->getState(), zTemp);
291 system->resetUpToDate();
293 if(signChangedWRTsvLast(system->evalsv()))
298 dos->calc_state(step.second, zTemp);
299 curTimeAndState=step.second;
300 system->setTime(step.second);
301 BoostOdeintHelper::assign(system->getState(), zTemp);
302 system->resetUpToDate();
304 auto &sv=system->evalsv();
305 auto &jsv=system->getjsv();
306 for(
int i=0; i<sv.size(); ++i)
307 jsv(i)=svLast(i)*sv(i)<0;
311 while(tPlot<=step.second && tPlot<tSamplePoint) {
312 dos->calc_state(tPlot, zTemp);
313 if(curTimeAndState!=tPlot) {
314 curTimeAndState=tPlot;
315 system->setTime(tPlot);
316 BoostOdeintHelper::assign(system->getState(), zTemp);
317 system->resetUpToDate();
322 msg(Status)<<
"t = "<<tPlot<<
", dt="<<dos->current_time_step()<<std::flush;
326 if(step.second<tSamplePoint) {
329 dos->calc_state(step.second, zTemp);
330 if(curTimeAndState!=step.second) {
331 curTimeAndState=step.second;
332 system->setTime(step.second);
333 BoostOdeintHelper::assign(system->getState(), zTemp);
334 system->resetUpToDate();
341 system->resetUpToDate();
348 svLast=system->evalsv();
350 BoostOdeintHelper::assign(zTemp, system->getState());
351 dos->initialize(zTemp, step.second, dos->current_time_step());
355 bool reinitNeeded=
false;
356 if(gMax>=0 and system->positionDriftCompensationNeeded(gMax)) {
357 system->projectGeneralizedPositions(3);
358 system->projectGeneralizedVelocities(3);
361 else if(gdMax>=0 and system->velocityDriftCompensationNeeded(gdMax)) {
362 system->projectGeneralizedVelocities(3);
367 BoostOdeintHelper::assign(zTemp, system->getState());
368 dos->initialize(zTemp, step.second, dos->current_time_step());
370 system->updateStopVectorParameters();
379 dos->calc_state(tSamplePoint, zTemp);
380 curTimeAndState=tSamplePoint;
381 system->setTime(tSamplePoint);
382 BoostOdeintHelper::assign(system->getState(), zTemp);
383 dos->initialize(zTemp, tSamplePoint, dos->current_time_step());
384 system->updateStopVectorParameters();
389 template<
typename DOS>
390 void BoostOdeintDOS<DOS>::postIntegrate() {
391 msg(Info)<<std::endl;
392 msg(Info)<<
"Integration statistics:"<<std::endl;
393 msg(Info)<<
"nrSteps = "<<nrSteps<<std::endl;
394 msg(Info)<<
"nrRHS = "<<nrRHS<<std::endl;
395 msg(Info)<<
"nrJacs = "<<nrJacs<<std::endl;
396 msg(Info)<<
"nrPlots = "<<nrPlots<<std::endl;
397 msg(Info)<<
"nrSVs = "<<nrSVs<<std::endl;
398 msg(Info)<<
"nrRoots = "<<nrRoots<<std::endl;
399 msg(Info)<<
"nrDriftCorr = "<<nrDriftCorr<<std::endl;
404 template<
bool,
typename Self>
405 struct InitializeUsingXMLControlled;
408 template<
typename Self>
409 struct InitializeUsingXMLControlled<true, Self> {
410 static void call(Self
self, xercesc::DOMElement* element) {
411 xercesc::DOMElement *e;
413 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"absoluteToleranceScalar");
414 if(e)
self->setAbsoluteTolerance(MBXMLUtils::E(e)->getText<double>());
416 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"relativeToleranceScalar");
417 if(e)
self->setRelativeTolerance(MBXMLUtils::E(e)->getText<double>());
419 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"maximumStepSize");
420 if(e)
self->setMaximumStepSize(MBXMLUtils::E(e)->getText<double>());
425 template<
typename Self>
426 struct InitializeUsingXMLControlled<false, Self> {
427 static void call(Self
self, xercesc::DOMElement* element) {
428 xercesc::DOMElement *e;
432 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"absoluteToleranceScalar");
433 if(e)
self->throwError(
"absoluteToleranceScalar element used for an fixed step-size stepper.");
435 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"relativeToleranceScalar");
436 if(e)
self->throwError(
"relativeToleranceScalar element used for an fixed step-size stepper.");
438 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"maximumStepSize");
439 if(e)
self->throwError(
"maximumStepSize element used for an fixed step-size stepper.");
444 template<
typename DOS>
447 xercesc::DOMElement *e;
449 InitializeUsingXMLControlled<isControlled, decltype(
this)>::call(
this, element);
451 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"initialStepSize");
452 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:445
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:223
Definition: boost_odeint_integrator.h:75
namespace MBSim
Definition: eigenanalyzer.cc:47