4 #include <cvodes/cvodes.h>
5 #include <nvector/nvector_serial.h>
6 #include <cvodes/cvodes_dense.h>
8 namespace pt = boost::property_tree;
12 RootfindingIVP::RootfindingIVP(std::shared_ptr<WorkPiece> rhs, std::shared_ptr<WorkPiece> root, pt::ptree
const& pt, std::shared_ptr<AnyAlgebra> algebra) : ODEBase(rhs, pt, algebra), root(root), maxSteps(pt.
get<unsigned int>(
"Rootfinder.MaxSteps", (int)1e10)), maxTime(pt.
get<double>(
"Rootfinder.MaxTime", 1.0e3)), maxErrorTests(pt.
get<unsigned int>(
"Rootfinder.MaxErrorTests", 100)) {
14 assert(rhs->numInputs>0);
15 assert(
root->numInputs>0);
18 assert(
root->numOutputs>0);
28 assert(inputs.size()>=rhs->numInputs+
root->numInputs-1-(autonomous? 0 : 1));
35 DeepCopy(state, boost::any_cast<const N_Vector&>(inputs[0]));
38 auto data = std::make_shared<ODEData>(rhs,
root, inputs, autonomous, wrtIn, wrtOut);
40 const boost::any time = 0.0;
41 data->inputs.insert(data->inputs.begin(), time);
45 void* cvode_mem =
nullptr;
48 cvode_mem = CVodeCreate(multiStep, solveMethod);
49 assert(CheckFlag((
void*)cvode_mem,
"CVodeCreate", 0));
52 CreateSolverMemory(cvode_mem, state, data);
55 N_Vector *sensState =
nullptr;
58 if( wrtIn>=0 && wrtOut==0 && wrtIn<rhs->numInputs ) {
59 paramSize = algebra->Size(inputs[wrtIn]);
62 sensState = N_VCloneVectorArray_Serial(paramSize, state);
63 assert(CheckFlag((
void *)sensState,
"N_VCloneVectorArray_Serial", 0));
66 for(
int is=0; is<paramSize; ++is ) {
67 N_VConst(0.0, sensState[is]);
71 SetUpSensitivity(cvode_mem, paramSize, sensState);
74 InitializeDerivative(1, NV_LENGTH_S(state), paramSize, mode);
79 assert(CheckFlag(&flag,
"CVodeRootInit", 1));
82 flag = CVodeSetMaxNumSteps(cvode_mem,
maxSteps);
83 assert(CheckFlag(&flag,
"CVodeSetMaxNumSteps", 1));
87 assert(CheckFlag(&flag,
"CVodeSetMaxErrorTestFails", 1));
93 if( inputs.size()>rhs->numInputs+
root->numInputs-1 ) {
95 std::vector<std::pair<unsigned int, unsigned int> > timeIndices = TimeIndices(outputTimes);
98 std::pair<double, int> nextTime;
100 while( NextTime(nextTime, timeIndices, outputTimes) ) {
102 if( std::fabs(nextTime.first-t)>1.0e-14 ) {
103 flag = CVode(cvode_mem, nextTime.first, state, &t, CV_NORMAL);
104 assert(CheckFlag(&flag,
"CVode", 1));
108 if( std::fabs(nextTime.first-t)<1.0e-14 ) {
110 if( timeIndices[nextTime.second].second>1 ) {
112 DeepCopy(boost::any_cast<std::vector<N_Vector>&>(outputs[nextTime.second]) [timeIndices[nextTime.second].first-1], state);
115 DeepCopy(boost::any_cast<N_Vector&>(outputs[nextTime.second]), state);
119 if( flag==CV_ROOT_RETURN ) {
break; }
123 if( flag!=CV_ROOT_RETURN ) {
125 flag = CVode(cvode_mem,
maxTime, state, &t, CV_NORMAL);
126 assert(CheckFlag(&flag,
"CVode", 1));
130 assert(flag==CV_ROOT_RETURN);
133 outputs.insert(outputs.begin(), t);
134 outputs.insert(outputs.begin(), state);
138 flag = CVodeGetSens(cvode_mem, &t, sensState);
139 assert(CheckFlag(&flag,
"CVodeGetSens", 1));
142 DlsMat& jac = boost::any_cast<DlsMat&>(*jacobian);
143 for(
unsigned int col=0; col<paramSize; ++col ) {
144 DENSE_COL(jac, col) = NV_DATA_S(sensState[col]);
145 NV_DATA_S(sensState[col]) =
nullptr;
149 N_VDestroyVectorArray_Serial(sensState, paramSize);
153 Eigen::VectorXi rootsfound(
root->numOutputs);
154 flag = CVodeGetRootInfo(cvode_mem, rootsfound.data());
155 assert(CheckFlag(&flag,
"CVodeGetRootInfo", 1));
158 CVodeFree(&cvode_mem);
170 std::cerr << std::endl <<
"ERROR: Cannot compute derivative infomration of muq::Modeling::RootfindingIVP for outputs other than the state at the root (the first output)" << std::endl << std::endl;
176 const Eigen::VectorXi& rootsfound =
FindRoot(inputs, wrtIn, wrtOut);
177 unsigned int ind = 0;
178 for( ; ind<rootsfound.size(); ++ind ) {
179 if( rootsfound(ind)!=0 ) {
break; }
184 rhsIns.push_back(outputs[0]);
185 rhsIns.insert(rhsIns.end(), inputs.begin()+1, inputs.begin()+rhs->numInputs);
189 rootIns.push_back(outputs[0]);
190 rootIns.insert(rootIns.end(), inputs.begin()+rhs->numInputs, inputs.begin()+rhs->numInputs+
root->numInputs-1);
193 const boost::any& dgdy =
root->Jacobian(0, ind, rootIns);
194 const DlsMat& dgdyref = boost::any_cast<const DlsMat&>(dgdy);
197 const std::vector<boost::any>& f = rhs->Evaluate(rhsIns);
198 const N_Vector& fref = boost::any_cast<N_Vector>(f[0]);
199 assert(NV_LENGTH_S(fref)==dgdyref->N);
202 for(
unsigned int i=0; i<dgdyref->N; ++i ) {
203 dum += DENSE_ELEM(dgdyref, 0, i)*NV_Ith_S(fref, i);
206 if( wrtIn>=rhs->numInputs ) {
207 boost::any dgdpara =
root->Jacobian(wrtIn-rhs->numInputs+1, ind, rootIns);
208 const DlsMat& dgdpararef = boost::any_cast<const DlsMat&>(dgdpara);
209 DenseScale(-1.0/dum, dgdpararef);
211 jacobian = NewDenseMat(NV_LENGTH_S(fref), dgdpararef->N);
212 DlsMat& jac = boost::any_cast<DlsMat&>(*jacobian);
216 for(
unsigned int i=0; i<jac->M; ++i ) {
217 for(
unsigned int j=0; j<jac->N; ++j ) {
218 DENSE_ELEM(jac, i, j) = NV_Ith_S(fref, i)*DENSE_ELEM(dgdpararef, 0, j);
226 DlsMat& jac = boost::any_cast<DlsMat&>(*jacobian);
227 assert(jac->M==dgdyref->N);
228 assert(NV_LENGTH_S(fref)==jac->M);
235 DenseScale(-1.0/dum, dgdyref);
238 N_Vector dtfdic = N_VNew_Serial(jac->N);
239 for(
unsigned int i=0; i<jac->N; ++i ) {
240 NV_Ith_S(dtfdic, i) = 0.0;
241 for(
unsigned int j=0; j<jac->M; ++j ) {
242 NV_Ith_S(dtfdic, i) += DENSE_ELEM(dgdyref, 0, j)*DENSE_ELEM(jac, j, i);
247 for(
unsigned int i=0; i<jac->M; ++i ) {
248 for(
unsigned int j=0; j<jac->N; ++j ) {
249 DENSE_ELEM(jac, i, j) += NV_Ith_S(fref, i)*NV_Ith_S(dtfdic, j);
260 assert(rhs->InputType(0,
false).compare(
root->InputType(0,
false))==0);
263 assert(rhs->InputType(1,
false).compare(
root->InputType(0,
false))==0);
267 for(
auto intype :
root->InputTypes() ) {
268 if( intype.first==0 ) {
272 inputTypes[rhs->numInputs+intype.first-1] = intype.second;
276 outputTypes[0] =
root->InputType(0,
false);
279 outputTypes[1] =
typeid(double).name();
289 const boost::any& anyref = state;
293 rootins.insert(rootins.begin(), anyref);
296 const std::vector<boost::any>& result = data->
root->Evaluate(rootins);
297 assert(result.size()==data->
root->numOutputs);
298 for(
unsigned int i=0; i<result.size(); ++i ) {
299 root[i] = boost::any_cast<double>(result[i]);
A helper class for Sundial's time integration.
std::shared_ptr< ModPiece > root
A function we are trying to find the root of along an orbit of the ODE (nullptr if we are not doing a...
ref_vector< Eigen::VectorXd > inputs
std::shared_ptr< ModPiece > rhs
The right hand side of the ODE.
const unsigned int maxSteps
The maximum number of steps the timesteper can take.
std::shared_ptr< WorkPiece > root
The root function.
virtual void JacobianImpl(unsigned int const wrtIn, unsigned int const wrtOut, ref_vector< boost::any > const &inputs) override
Compute the Jacobian of the state at the root.
const unsigned int maxErrorTests
The maximum number of error test failures.
virtual ~RootfindingIVP()
Eigen::VectorXi FindRoot(ref_vector< boost::any > const &inputs, int const wrtIn=-1, int const wrtOut=-1, DerivativeMode const &mode=DerivativeMode::Jac)
Run the CVODES integrator.
const double maxTime
The maximum amount of time to integrate.
virtual void EvaluateImpl(ref_vector< boost::any > const &inputs) override
Integrate the ODE until we find a root.
static int EvaluateRoot(realtype t, N_Vector state, realtype *root, void *user_data)
Evaluate the root function.
void UpdateInputOutputTypes()
Update the input and output types based on the rhs and root muq::Modeling::WorkPiece's.
std::vector< std::reference_wrapper< const T > > ref_vector
A vector of references to something ...
auto get(const nlohmann::detail::iteration_proxy_value< IteratorType > &i) -> decltype(i.key())