/** store all the states of the graph in a matrix
* \param indx row index in the matrix
* \param startColumn the starting column
* \param DSG0 the graph of DynamicalSystem
* \param IG0 the graph of Interaction
* \param data the matrix where to save the data
* \return the last written column
*/
static inline unsigned storeAllStates(unsigned indx, unsigned startColumn, DynamicalSystemsGraph& DSG0, InteractionsGraph& IG0, SimpleMatrix& data)
{
DynamicalSystemsGraph::VIterator dsvi, dsvdend;
unsigned column = startColumn;
for (std11::tie(dsvi, dsvdend) = DSG0.vertices(); dsvi != dsvdend; ++dsvi)
{
unsigned i = column;
SiconosVector& x = *DSG0.bundle(*dsvi)->x();
for (unsigned j = 0; j < x.size(); ++i, ++j)
{
data(indx, i) = x(j);
}
column += x.size();
if (DSG0.u.hasKey(*dsvi))
{
SiconosVector& u = *DSG0.u[*dsvi];
for (unsigned j = 0; j < u.size(); ++i, ++j)
{
data(indx, i) = u(j);
}
column += u.size();
}
if (DSG0.e.hasKey(*dsvi))
{
SiconosVector& e = *DSG0.e[*dsvi];
for (unsigned j = 0; j < e.size(); ++i, ++j)
{
data(indx, i) = e(j);
}
column += e.size();
}
}
InteractionsGraph::VIterator ivi, ivdend;
for (std11::tie(ivi, ivdend) = IG0.vertices(); ivi != ivdend; ++ivi)
{
unsigned i = column;
SiconosVector& y = *IG0.bundle(*ivi)->y(0);
for (unsigned j = 0; j < y.size(); ++i, ++j)
{
data(indx, i) = y(j);
}
column += y.size();
SiconosVector& lambda = *IG0.bundle(*ivi)->lambda(0);
for (unsigned j = 0; j < lambda.size(); ++i, ++j)
{
data(indx, i) = lambda(j);
}
column += lambda.size();
}
return column;
}
void ControlLinearAdditionalTermsED::addSmoothTerms(DynamicalSystemsGraph& DSG0, const DynamicalSystemsGraph::VDescriptor& dsgVD, const double t, SiconosVector& xdot)
{
// check whether we have a system with a control input
if (DSG0.u.hasKey(dsgVD))
{
if (DSG0.B.hasKey(dsgVD))
{
prod(DSG0.B.getRef(dsgVD), DSG0.u.getRef(dsgVD), xdot, false); // xdot += B*u
}
else if (DSG0.pluginU.hasKey(dsgVD))
{
DynamicalSystem& ds = *DSG0.bundle(dsgVD);
SiconosVector& u = DSG0.u.getRef(dsgVD);
SiconosVector& tmpXdot = DSG0.tmpXdot.getRef(dsgVD);
((AdditionalTermsEDfctU)DSG0.pluginU.getRef(dsgVD).fPtr)(t, xdot.size(), ds.getx().getArray(), u.size(), u.getArray(), tmpXdot.getArray(), ds.getz().size(), ds.getz().getArray());
xdot += tmpXdot; // xdot += g(x, u)
}
else
{
RuntimeException::selfThrow("ControlLinearAdditionalTermsED :: input u but no B nor pluginU");
}
}
// check whether the DynamicalSystem is an Observer
if (DSG0.e.hasKey(dsgVD))
{
assert(DSG0.L.hasKey(dsgVD));
prod(*DSG0.L[dsgVD], *DSG0.e[dsgVD], xdot, false); // xdot += -L*e
}
}
void ControlLinearAdditionalTermsED::init(DynamicalSystemsGraph& DSG0, const Model& model)
{
DynamicalSystemsGraph::VIterator dsvi, dsvdend;
for (std11::tie(dsvi, dsvdend) = DSG0.vertices(); dsvi != dsvdend; ++dsvi)
{
DynamicalSystem& ds = *DSG0.bundle(*dsvi);
if (DSG0.pluginU.hasKey(*dsvi))
{
DSG0.tmpXdot[*dsvi].reset(new SiconosVector(ds.getx().size()));
}
if (DSG0.pluginJacgx.hasKey(*dsvi))
{
DSG0.jacgx[*dsvi].reset(new SimpleMatrix(ds.getx().size(), ds.getx().size()));
}
}
}
void LsodarOSI::computeJacobianRhs(double t, DynamicalSystemsGraph& DSG0)
{
for (DSIterator it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
{
SP::DynamicalSystem& ds = *it;
ds->computeJacobianRhsx(t);
if (_extraAdditionalTerms)
{
DynamicalSystemsGraph::VDescriptor dsgVD = DSG0.descriptor(ds);
_extraAdditionalTerms->addJacobianRhsContribution(DSG0, dsgVD, t, ds->getJacobianRhsx());
}
}
}
void ControlLinearAdditionalTermsED::addJacobianRhsContribution(DynamicalSystemsGraph& DSG0, const DynamicalSystemsGraph::VDescriptor& dsgVD, const double t, SiconosMatrix& jacRhs)
{
// check whether we have a system with a control input
if (DSG0.pluginJacgx.hasKey(dsgVD))
{
DynamicalSystem& ds = *DSG0.bundle(dsgVD);
SiconosVector& u = DSG0.u.getRef(dsgVD);
SimpleMatrix& tmpJacgx = DSG0.jacgx.getRef(dsgVD);
((AdditionalTermsEDfctU)DSG0.pluginJacgx.getRef(dsgVD).fPtr)(t, ds.getx().size(), ds.getx().getArray(), u.size(), u.getArray(), tmpJacgx.getArray(), ds.getz().size(), ds.getz().getArray());
jacRhs += tmpJacgx; // JacRhs += \nabla_x g(x, u)
}
else
{
RuntimeException::selfThrow("ControlLinearAdditionalTermsED :: input u but no B nor pluginU");
}
}
void D1MinusLinearOSI::initializeWorkVectorsForInteraction(Interaction &inter,
InteractionProperties& interProp,
DynamicalSystemsGraph & DSG)
{
DEBUG_BEGIN("D1MinusLinearOSI::initializeWorkVectorsForInteraction(Interaction &inter, InteractionProperties& interProp, DynamicalSystemsGraph & DSG)\n");
SP::DynamicalSystem ds1= interProp.source;
SP::DynamicalSystem ds2= interProp.target;
assert(ds1);
assert(ds2);
DEBUG_PRINTF("interaction number %i\n", inter.number());
VectorOfBlockVectors& DSlink = inter.linkToDSVariables();
if (!interProp.workVectors)
{
interProp.workVectors.reset(new VectorOfVectors);
interProp.workVectors->resize(D1MinusLinearOSI::WORK_INTERACTION_LENGTH);
}
if (!interProp.workBlockVectors)
{
interProp.workBlockVectors.reset(new VectorOfBlockVectors);
interProp.workBlockVectors->resize(D1MinusLinearOSI::BLOCK_WORK_LENGTH);
}
VectorOfVectors& inter_work = *interProp.workVectors;
VectorOfBlockVectors& inter_work_block = *interProp.workBlockVectors;
Relation &relation = *inter.relation();
RELATION::TYPES relationType = relation.getType();
inter_work[D1MinusLinearOSI::OSNSP_RHS].reset(new SiconosVector(inter.dimension()));
// Check if interations levels (i.e. y and lambda sizes) are compliant with the current osi.
_check_and_update_interaction_levels(inter);
// Initialize/allocate memory buffers in interaction.
inter.initializeMemory(_steps);
if (!(checkOSI(DSG.descriptor(ds1)) && checkOSI(DSG.descriptor(ds2))))
{
std::cout << "checkOSI(DSG.descriptor(ds1)): "
<< std::boolalpha
<< checkOSI(DSG.descriptor(ds1)) << std::endl;
std::cout << "checkOSI(DSG.descriptor(ds2)): "
<< std::boolalpha
<< checkOSI(DSG.descriptor(ds2)) << std::endl;
RuntimeException::selfThrow("D1MinusLinearOSI::initializeWorkVectorsForInteraction. The implementation is not correct for two different OSI for one interaction");
}
/* allocate and set work vectors for the osi */
unsigned int xfree = D1MinusLinearOSI::xfree;
DEBUG_PRINTF("ds1->number() %i\n",ds1->number());
DEBUG_PRINTF("ds2->number() %i\n",ds2->number());
if (ds1 != ds2)
{
DEBUG_PRINT("ds1 != ds2\n");
if ((!inter_work_block[xfree]) || (inter_work_block[xfree]->numberOfBlocks() !=2 ))
inter_work_block[xfree].reset(new BlockVector(2));
}
else
{
if ((!inter_work_block[xfree]) || (inter_work_block[xfree]->numberOfBlocks() !=1 ))
inter_work_block[xfree].reset(new BlockVector(1));
}
if(checkOSI(DSG.descriptor(ds1)))
{
DEBUG_PRINTF("ds1->number() %i is taken into account\n", ds1->number());
assert(DSG.properties(DSG.descriptor(ds1)).workVectors);
VectorOfVectors &workVds1 = *DSG.properties(DSG.descriptor(ds1)).workVectors;
inter_work_block[xfree]->setVectorPtr(0,workVds1[D1MinusLinearOSI::FREE]);
}
if (ds1 != ds2)
{
DEBUG_PRINT("ds1 != ds2\n");
if(checkOSI(DSG.descriptor(ds2)))
{
DEBUG_PRINTF("ds2->number() %i is taken into account\n",ds2->number());
assert(DSG.properties(DSG.descriptor(ds2)).workVectors);
VectorOfVectors &workVds2 = *DSG.properties(DSG.descriptor(ds2)).workVectors;
inter_work_block[xfree]->setVectorPtr(1,workVds2[D1MinusLinearOSI::FREE]);
}
}
DEBUG_EXPR(inter_work_block[xfree]->display(););
static inline std::pair<unsigned, std::string> getNumberOfStates(DynamicalSystemsGraph& DSG0, InteractionsGraph& IG0)
{
std::string legend;
DynamicalSystemsGraph::VIterator dsvi, dsvdend;
unsigned nb = 0;
unsigned counter = 0;
for (std11::tie(dsvi, dsvdend) = DSG0.vertices(); dsvi != dsvdend; ++dsvi)
{
SiconosVector& x = *DSG0.bundle(*dsvi)->x();
nb += x.size();
std::string nameDS;
if (DSG0.name.hasKey(*dsvi))
{
nameDS = DSG0.name[*dsvi];
}
else
{
nameDS = "unknownDS" + TO_STR(counter);
++counter;
}
for (unsigned i = 0; i < x.size(); ++i)
{
legend.append(" " + nameDS + "_" + TO_STR(i));
}
if (DSG0.u.hasKey(*dsvi))
{
unsigned sizeU = DSG0.u[*dsvi]->size();
nb += sizeU;
for (unsigned i = 0; i < sizeU; ++i)
{
legend.append(" " + nameDS + "_u_" + TO_STR(i));
}
}
if (DSG0.e.hasKey(*dsvi))
{
unsigned sizeE = DSG0.e[*dsvi]->size();
for (unsigned i = 0; i < sizeE; ++i)
{
legend.append(" " + nameDS + "_e_" + TO_STR(i));
}
nb += DSG0.e[*dsvi]->size();
}
}
InteractionsGraph::VIterator ivi, ivdend;
counter = 0;
for (std11::tie(ivi, ivdend) = IG0.vertices(); ivi != ivdend; ++ivi)
{
std::string nameInter;
if (IG0.name.hasKey(*ivi))
{
nameInter = IG0.name[*ivi];
}
else
{
nameInter = "unknownInteraction" + TO_STR(counter);
++counter;
}
SiconosVector& y = *IG0.bundle(*ivi)->y(0);
nb += y.size();
for (unsigned i = 0; i < y.size(); ++i)
{
legend.append(" " + nameInter + "_y_" + TO_STR(i));
}
SiconosVector& lambda = *IG0.bundle(*ivi)->lambda(0);
nb += lambda.size();
for (unsigned i = 0; i < lambda.size(); ++i)
{
legend.append(" " + nameInter + "_lambda_" + TO_STR(i));
}
}
return std::make_pair(nb, legend);
}
void SchatzmanPaoliOSI::initializeWorkVectorsForInteraction(Interaction &inter,
InteractionProperties& interProp,
DynamicalSystemsGraph & DSG)
{
SP::DynamicalSystem ds1= interProp.source;
SP::DynamicalSystem ds2= interProp.target;
assert(ds1);
assert(ds2);
VectorOfBlockVectors& DSlink = inter.linkToDSVariables();
if (!interProp.workVectors)
{
interProp.workVectors.reset(new VectorOfVectors);
interProp.workVectors->resize(SchatzmanPaoliOSI::WORK_INTERACTION_LENGTH);
}
if (!interProp.workBlockVectors)
{
interProp.workBlockVectors.reset(new VectorOfBlockVectors);
interProp.workBlockVectors->resize(SchatzmanPaoliOSI::BLOCK_WORK_LENGTH);
}
VectorOfVectors& inter_work = *interProp.workVectors;
VectorOfBlockVectors& inter_work_block = *interProp.workBlockVectors;
Relation &relation = *inter.relation();
inter_work[SchatzmanPaoliOSI::OSNSP_RHS].reset(new SiconosVector(inter.dimension()));
RELATION::TYPES relationType = relation.getType();
// Check if interations levels (i.e. y and lambda sizes) are compliant with the current osi.
_check_and_update_interaction_levels(inter);
// Initialize/allocate memory buffers in interaction.
inter.initializeMemory(_steps);
if (!(checkOSI(DSG.descriptor(ds1)) && checkOSI(DSG.descriptor(ds2))))
{
RuntimeException::selfThrow("SchatzmanPaoliOSI::initializeWorkVectorsForInteraction. The implementation is not correct for two different OSI for one interaction");
}
/* allocate and set work vectors for the osi */
VectorOfVectors &workVds1 = *DSG.properties(DSG.descriptor(ds1)).workVectors;
if (relationType == Lagrangian)
{
LagrangianDS& lds = *std11::static_pointer_cast<LagrangianDS> (ds1);
DSlink[LagrangianR::p0].reset(new BlockVector());
DSlink[LagrangianR::p0]->insertPtr(lds.p(0));
inter_work_block[SchatzmanPaoliOSI::xfree].reset(new BlockVector());
inter_work_block[SchatzmanPaoliOSI::xfree]->insertPtr(workVds1[SchatzmanPaoliOSI::FREE]);
}
else if (relationType == NewtonEuler)
{
inter_work_block[SchatzmanPaoliOSI::xfree].reset(new BlockVector());
inter_work_block[SchatzmanPaoliOSI::xfree]->insertPtr(workVds1[SchatzmanPaoliOSI::FREE]);
}
if (ds1 != ds2)
{
VectorOfVectors &workVds2 = *DSG.properties(DSG.descriptor(ds2)).workVectors;
if (relationType == Lagrangian)
{
inter_work_block[SchatzmanPaoliOSI::xfree]->insertPtr(workVds2[SchatzmanPaoliOSI::FREE]);
LagrangianDS& lds = *std11::static_pointer_cast<LagrangianDS> (ds2);
DSlink[LagrangianR::p0]->insertPtr(lds.p(0));
}
else if (relationType == NewtonEuler)
{
inter_work_block[SchatzmanPaoliOSI::xfree]->insertPtr(workVds2[SchatzmanPaoliOSI::FREE]);
}
}
}
