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->setcuris(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();
267 dos.reset(
new DOS(aTol, rTol, dtMax));
268 dos->initialize(zTemp, tStart, dt0);
271 template<
typename DOS>
272 void BoostOdeintDOS<DOS>::subIntegrate(
double tSamplePoint) {
274 static constexpr
double oneMinusEps = 1 - std::numeric_limits<double>::epsilon();
275 while(dos->current_time()<tSamplePoint*oneMinusEps) {
278 #if BOOST_VERSION >= 107100 280 dos->setDtMax(std::min(dtMax, tSamplePoint-dos->current_time()));
282 auto step=dos->do_step(boostOdeintSystem());
283 system->updateInternalState();
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;
342 tPlot=tStart+plotSample*dtPlot;
350 msg(Status)<<
"t = "<<step.second<<
", dt="<<dos->current_time_step()<<
" "<<std::flush;
353 #if BOOST_VERSION < 107100 354 if(step.second<tSamplePoint) {
358 dos->calc_state(step.second, zTemp);
359 if(curTimeAndState!=step.second) {
360 curTimeAndState=step.second;
361 system->setTime(step.second);
362 BoostOdeintHelper::assign(system->getState(), zTemp);
363 system->resetUpToDate();
370 system->resetUpToDate();
377 svLast=system->evalsv();
379 BoostOdeintHelper::assign(zTemp, system->getState());
380 dos->initialize(zTemp, step.second, dos->current_time_step());
384 bool reinitNeeded=
false;
385 if(gMax>=0 and system->positionDriftCompensationNeeded(gMax)) {
386 system->projectGeneralizedPositions(3);
387 system->projectGeneralizedVelocities(3);
390 else if(gdMax>=0 and system->velocityDriftCompensationNeeded(gdMax)) {
391 system->projectGeneralizedVelocities(3);
396 BoostOdeintHelper::assign(zTemp, system->getState());
397 dos->initialize(zTemp, step.second, dos->current_time_step());
399 system->updateStopVectorParameters();
401 #if BOOST_VERSION < 107100 409 dos->calc_state(tSamplePoint, zTemp);
410 curTimeAndState=tSamplePoint;
411 system->setTime(tSamplePoint);
412 BoostOdeintHelper::assign(system->getState(), zTemp);
413 dos->initialize(zTemp, tSamplePoint, dos->current_time_step());
414 system->updateStopVectorParameters();
418 #if BOOST_VERSION >= 107100 421 if(curTimeAndState!=dos->current_time()) {
422 system->setTime(dos->current_time());
423 BoostOdeintHelper::assign(system->getState(), dos->current_state());
424 system->resetUpToDate();
430 template<
typename DOS>
431 void BoostOdeintDOS<DOS>::postIntegrate() {
432 msg(Info)<<std::endl;
433 msg(Info)<<
"Integration statistics:"<<std::endl;
434 msg(Info)<<
"nrSteps = "<<nrSteps<<std::endl;
435 msg(Info)<<
"nrRHS = "<<nrRHS<<std::endl;
436 msg(Info)<<
"nrJacs = "<<nrJacs<<std::endl;
437 msg(Info)<<
"nrPlots = "<<nrPlots<<std::endl;
438 msg(Info)<<
"nrSVs = "<<nrSVs<<std::endl;
439 msg(Info)<<
"nrRoots = "<<nrRoots<<std::endl;
440 msg(Info)<<
"nrDriftCorr = "<<nrDriftCorr<<std::endl;
445 template<
bool,
typename Self>
446 struct InitializeUsingXMLControlled;
449 template<
typename Self>
450 struct InitializeUsingXMLControlled<true, Self> {
451 static void call(Self
self, xercesc::DOMElement* element) {
452 xercesc::DOMElement *e;
454 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"absoluteToleranceScalar");
455 if(e)
self->setAbsoluteTolerance(MBXMLUtils::E(e)->getText<double>());
457 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"relativeToleranceScalar");
458 if(e)
self->setRelativeTolerance(MBXMLUtils::E(e)->getText<double>());
460 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"maximumStepSize");
461 if(e)
self->setMaximumStepSize(MBXMLUtils::E(e)->getText<double>());
466 template<
typename Self>
467 struct InitializeUsingXMLControlled<false, Self> {
468 static void call(Self
self, xercesc::DOMElement* element) {
469 xercesc::DOMElement *e;
473 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"absoluteToleranceScalar");
474 if(e)
self->throwError(
"absoluteToleranceScalar element used for an fixed step-size stepper.");
476 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"relativeToleranceScalar");
477 if(e)
self->throwError(
"relativeToleranceScalar element used for an fixed step-size stepper.");
479 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"maximumStepSize");
480 if(e)
self->throwError(
"maximumStepSize element used for an fixed step-size stepper.");
485 template<
typename DOS>
488 xercesc::DOMElement *e;
490 InitializeUsingXMLControlled<isControlled, decltype(
this)>::call(
this, element);
492 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%
"initialStepSize");
493 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:486
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
namespace MBSim
Definition: bilateral_constraint.cc:29