void visit(const NewtonImpactNSL& nslaw)
{
double e;
e = nslaw.e();
Index subCoord(4);
subCoord[0] = 0;
subCoord[1] = _inter->nonSmoothLaw()->size();
subCoord[2] = 0;
subCoord[3] = subCoord[1];
subscal(e, *_inter->yOld(_osnsp->inputOutputLevel()), *(_inter->yForNSsolver()), subCoord, false); // q = q + e * q
}
void visit(const NewtonImpactNSL& nslaw)
{
double e;
e = nslaw.e();
Index subCoord(4);
subCoord[0] = 0;
subCoord[1] = _inter->nonSmoothLaw()->size();
subCoord[2] = 0;
subCoord[3] = subCoord[1];
// Only the normal part is multiplied by e
SP::SiconosVector y_k_1 ;
y_k_1 = _inter->yMemory(_osnsp->inputOutputLevel())->getSiconosVector(1);
// std::cout << "y_k_1 " << std::endl;
// y_k_1->display();
subscal(e, *y_k_1, *(_inter->yForNSsolver()), subCoord, false);
}
void visit(const NewtonImpactNSL& nslaw)
{
double e;
e = nslaw.e();
Index subCoord(4);
subCoord[0] = 0;
subCoord[1] = _inter->nonSmoothLaw()->size();
subCoord[2] = 0;
subCoord[3] = subCoord[1];
// Only the normal part is multiplied by e
const SiconosVector& y_k_1(
_inter->yMemory(_osnsp->inputOutputLevel()).getSiconosVector(1));
DEBUG_PRINTF("_osnsp->inputOutputLevel() = %i \n ",_osnsp->inputOutputLevel() );
DEBUG_EXPR(y_k_1.display());;
SiconosVector & osnsp_rhs = *(*_interProp.workVectors)[SchatzmanPaoliOSI::OSNSP_RHS];
subscal(e, y_k_1, osnsp_rhs, subCoord, false);
}
void SchatzmanPaoliOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp)
{
DEBUG_BEGIN("SchatzmanPaoliOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp)\n");
/** \warning: ensures that it can also work with two different osi for two different ds ?
*/
SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel());
SP::Interaction inter = indexSet->bundle(vertex_inter);
SP::OneStepNSProblems allOSNS = _simulation->oneStepNSProblems();
VectorOfBlockVectors& inter_work_block = *indexSet->properties(vertex_inter).workBlockVectors;
// Get relation and non smooth law types
RELATION::TYPES relationType = inter->relation()->getType();
RELATION::SUBTYPES relationSubType = inter->relation()->getSubType();
unsigned int sizeY = inter->nonSmoothLaw()->size();
unsigned int relativePosition = 0;
Index coord(8);
coord[0] = relativePosition;
coord[1] = relativePosition + sizeY;
coord[2] = 0;
coord[4] = 0;
coord[6] = 0;
coord[7] = sizeY;
SP::SiconosMatrix C;
SP::SiconosMatrix D;
SP::SiconosMatrix F;
SP::BlockVector deltax;
SiconosVector& osnsp_rhs = *(*indexSet->properties(vertex_inter).workVectors)[SchatzmanPaoliOSI::OSNSP_RHS];
SP::SiconosVector e;
SP::BlockVector Xfree = inter_work_block[SchatzmanPaoliOSI::xfree];;
assert(Xfree);
SP::Interaction mainInteraction = inter;
assert(mainInteraction);
assert(mainInteraction->relation());
DEBUG_EXPR(inter->display(););
// Fill the SparseMat
void BlockCSRMatrix::fill(SP::InteractionsGraph indexSet)
{
// ======> Aim: find inter1 and inter2 both in indexSets[level] and which
// have common DynamicalSystems. Then get the corresponding matrix
// from map blocks.
assert(indexSet);
// Number of blocks in a row = number of active constraints.
_nr = indexSet->size();
// (re)allocate memory for ublas matrix
_blockCSR->resize(_nr, _nr, false);
_diagsize0->resize(_nr);
_diagsize1->resize(_nr);
// === Loop through "active" Interactions (ie present in
// indexSets[level]) ===
int sizeV = 0;
InteractionsGraph::VIterator vi, viend;
for (std11::tie(vi, viend) = indexSet->vertices();
vi != viend; ++vi)
{
SP::Interaction inter = indexSet->bundle(*vi);
assert(inter->nonSmoothLaw()->size() > 0);
sizeV += inter->nonSmoothLaw()->size();
(*_diagsize0)[indexSet->index(*vi)] = sizeV;
(*_diagsize1)[indexSet->index(*vi)] = sizeV;
assert((*_diagsize0)[indexSet->index(*vi)] > 0);
assert((*_diagsize1)[indexSet->index(*vi)] > 0);
(*_blockCSR)(indexSet->index(*vi), indexSet->index(*vi)) =
indexSet->properties(*vi).block->getArray();
}
InteractionsGraph::EIterator ei, eiend;
for (std11::tie(ei, eiend) = indexSet->edges();
ei != eiend; ++ei)
{
InteractionsGraph::VDescriptor vd1 = indexSet->source(*ei);
InteractionsGraph::VDescriptor vd2 = indexSet->target(*ei);
SP::Interaction inter1 = indexSet->bundle(vd1);
SP::Interaction inter2 = indexSet->bundle(vd2);
assert(indexSet->index(vd1) < _nr);
assert(indexSet->index(vd2) < _nr);
assert(indexSet->is_vertex(inter2));
assert(vd2 == indexSet->descriptor(inter2));
assert(indexSet->index(vd2) == indexSet->index(indexSet->descriptor(inter2)));
unsigned int pos = indexSet->index(vd1);
unsigned int col = indexSet->index(vd2);
assert(pos != col);
(*_blockCSR)(std::min(pos, col), std::max(pos, col)) =
indexSet->properties(*ei).upper_block->getArray();
(*_blockCSR)(std::max(pos, col), std::min(pos, col)) =
indexSet->properties(*ei).lower_block->getArray();
}
DEBUG_EXPR(display(););
SetupLevels(SP::Simulation s, SP::Interaction inter,
SP::DynamicalSystem ds) :
_parent(s), _interaction(inter), _ds(ds)
{
_nonSmoothLaw = inter->nonSmoothLaw();
};
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
double EventDriven::detectEvents(bool updateIstate)
{
double _minResiduOutput = 0.0; // maximum of g_i with i running over all activated or deactivated contacts
// Loop over all interactions to detect whether some constraints are activated or deactivated
bool _IsContactClosed = false;
bool _IsContactOpened = false;
bool _IsFirstTime = true;
InteractionsGraph::VIterator ui, uiend;
SP::SiconosVector y, ydot, lambda;
SP::Topology topo = _nsds->topology();
SP::InteractionsGraph indexSet2 = topo->indexSet(2);
//
#ifdef DEBUG_MESSAGES
cout << "======== In EventDriven::detectEvents =========" <<endl;
#endif
for (std11::tie(ui, uiend) = _indexSet0->vertices(); ui != uiend; ++ui)
{
SP::Interaction inter = _indexSet0->bundle(*ui);
double nsLawSize = inter->nonSmoothLaw()->size();
if (nsLawSize != 1)
{
RuntimeException::selfThrow("In EventDriven::detectEvents, the interaction size > 1 has not been implemented yet!!!");
}
y = inter->y(0); // output y at this Interaction
ydot = inter->y(1); // output of level 1 at this Interaction
lambda = inter->lambda(2); // input of level 2 at this Interaction
if (!(indexSet2->is_vertex(inter))) // if Interaction is not in the indexSet[2]
{
if ((*y)(0) < _TOL_ED) // gap at the current interaction <= 0
{
_IsContactClosed = true;
}
if (_IsFirstTime)
{
_minResiduOutput = (*y)(0);
_IsFirstTime = false;
}
else
{
if (_minResiduOutput > (*y)(0))
{
_minResiduOutput = (*y)(0);
}
}
}
else // If interaction is in the indexSet[2]
{
if ((*lambda)(0) < _TOL_ED) // normal force at the current interaction <= 0
{
_IsContactOpened = true;
}
if (_IsFirstTime)
{
_minResiduOutput = (*lambda)(0);
_IsFirstTime = false;
}
else
{
if (_minResiduOutput > (*lambda)(0))
{
_minResiduOutput = (*lambda)(0);
}
}
}
//
#ifdef DEBUG_MESSAGES
cout.precision(15);
cout << "Contact number: " << inter->number() <<endl;
cout << "Contact gap: " << (*y)(0) <<endl;
cout << "Contact force: " << (*lambda)(0) <<endl;
cout << "Is contact is closed: " << _IsContactClosed <<endl;
cout << "Is contact is opened: " << _IsContactOpened <<endl;
#endif
//
}
//
if (updateIstate)
{
if ((!_IsContactClosed) && (!_IsContactOpened))
{
_istate = 2; //no event is detected
}
else if ((_IsContactClosed) && (!_IsContactOpened))
{
_istate = 3; // Only some contacts are closed
}
else if ((!_IsContactClosed) && (_IsContactOpened))
{
_istate = 4; // Only some contacts are opened
}
else
{
_istate = 5; // Some contacts are closed AND some contacts are opened
}
}
//
return _minResiduOutput;
}
void EventDriven::computeg(SP::OneStepIntegrator osi,
integer * sizeOfX, doublereal* time,
doublereal* x, integer * ng,
doublereal * gOut)
{
assert(_nsds);
assert(_nsds->topology());
InteractionsGraph::VIterator ui, uiend;
SP::Topology topo = _nsds->topology();
SP::InteractionsGraph indexSet2 = topo->indexSet(2);
unsigned int nsLawSize, k = 0 ;
SP::SiconosVector y, ydot, yddot, lambda;
SP::LsodarOSI lsodar = std11::static_pointer_cast<LsodarOSI>(osi);
// Solve LCP at acceleration level to calculate the lambda[2] at Interaction of indexSet[2]
lsodar->fillXWork(sizeOfX, x);
//
double t = *time;
if (!_allNSProblems->empty())
{
if (((*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->hasInteractions()))
{
(*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->compute(t);
}
};
/*
double * xdottmp = (double *)malloc(*sizeOfX*sizeof(double));
computef(osi, sizeOfX,time,x,xdottmp);
free(xdottmp);
*/
// Update the output from level 0 to level 1
_nsds->updateOutput(t,0);
_nsds->updateOutput(t,1);
_nsds->updateOutput(t,2);
//
for (std11::tie(ui, uiend) = _indexSet0->vertices(); ui != uiend; ++ui)
{
SP::Interaction inter = _indexSet0->bundle(*ui);
nsLawSize = inter->nonSmoothLaw()->size();
y = inter->y(0); // output y at this Interaction
ydot = inter->y(1); // output of level 1 at this Interaction
yddot = inter->y(2);
lambda = inter->lambda(2); // input of level 2 at this Interaction
if (!(indexSet2->is_vertex(inter))) // if Interaction is not in the indexSet[2]
{
for (unsigned int i = 0; i < nsLawSize; ++i)
{
if ((*y)(i) > _TOL_ED)
{
gOut[k] = (*y)(i);
}
else
{
if ((*ydot)(i) > -_TOL_ED)
{
gOut[k] = 100 * _TOL_ED;
}
else
{
gOut[k] = (*y)(i);
}
}
k++;
}
}
else // If Interaction is in the indexSet[2]
{
for (unsigned int i = 0; i < nsLawSize; ++i)
{
if ((*lambda)(i) > _TOL_ED)
{
gOut[k] = (*lambda)(i); // g = lambda[2]
}
else
{
if ((*yddot)(i) > _TOL_ED)
{
gOut[k] = (*lambda)(i);
}
else
{
gOut[k] = 100 * _TOL_ED;
}
}
k++;
}
}
}
}
void LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)
{
DEBUG_BEGIN("LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)\n");
// Computes matrix _interactionBlocks[inter1][inter1] (and allocates memory if
// necessary) one or two DS are concerned by inter1 . How
// _interactionBlocks are computed depends explicitely on the type of
// Relation of each Interaction.
// Warning: we suppose that at this point, all non linear
// operators (G for lagrangian relation for example) have been
// computed through plug-in mechanism.
// Get dimension of the NonSmoothLaw (ie dim of the interactionBlock)
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
SP::Interaction inter = indexSet->bundle(vd);
// Get osi property from interaction
// We assume that all ds in vertex_inter have the same osi.
SP::OneStepIntegrator Osi = indexSet->properties(vd).osi;
//SP::OneStepIntegrator Osi = simulation()->integratorOfDS(ds);
OSI::TYPES osiType = Osi->getType();
// At most 2 DS are linked by an Interaction
SP::DynamicalSystem DS1;
SP::DynamicalSystem DS2;
unsigned int pos1, pos2;
// --- Get the dynamical system(s) (edge(s)) connected to the current interaction (vertex) ---
if (indexSet->properties(vd).source != indexSet->properties(vd).target)
{
DEBUG_PRINT("a two DS Interaction\n");
DS1 = indexSet->properties(vd).source;
DS2 = indexSet->properties(vd).target;
}
else
{
DEBUG_PRINT("a single DS Interaction\n");
DS1 = indexSet->properties(vd).source;
DS2 = DS1;
// \warning this looks like some debug code, but it gets executed even with NDEBUG.
// may be compiler does something smarter, but still it should be rewritten. --xhub
InteractionsGraph::OEIterator oei, oeiend;
for (std11::tie(oei, oeiend) = indexSet->out_edges(vd);
oei != oeiend; ++oei)
{
// note : at most 4 edges
DS2 = indexSet->bundle(*oei);
if (DS2 != DS1)
{
assert(false);
break;
}
}
}
assert(DS1);
assert(DS2);
pos1 = indexSet->properties(vd).source_pos;
pos2 = indexSet->properties(vd).target_pos;
// --- Check block size ---
assert(indexSet->properties(vd).block->size(0) == inter->nonSmoothLaw()->size());
assert(indexSet->properties(vd).block->size(1) == inter->nonSmoothLaw()->size());
// --- Compute diagonal block ---
// Block to be set in OSNS Matrix, corresponding to
// the current interaction
SP::SiconosMatrix currentInteractionBlock = indexSet->properties(vd).block;
SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock;
RELATION::TYPES relationType;
double h = simulation()->currentTimeStep();
// General form of the interactionBlock is : interactionBlock =
// a*extraInteractionBlock + b * leftInteractionBlock * centralInteractionBlocks
// * rightInteractionBlock a and b are scalars, centralInteractionBlocks a
// matrix depending on the integrator (and on the DS), the
// simulation type ... left, right and extra depend on the relation
// type and the non smooth law.
relationType = inter->relation()->getType();
VectorOfSMatrices& workMInter = *indexSet->properties(vd).workMatrices;
inter->getExtraInteractionBlock(currentInteractionBlock, workMInter);
unsigned int nslawSize = inter->nonSmoothLaw()->size();
// loop over the DS connected to the interaction.
bool endl = false;
unsigned int pos = pos1;
for (SP::DynamicalSystem ds = DS1; !endl; ds = DS2)
{
assert(ds == DS1 || ds == DS2);
endl = (ds == DS2);
unsigned int sizeDS = ds->dimension();
// get _interactionBlocks corresponding to the current DS
// These _interactionBlocks depends on the relation type.
leftInteractionBlock.reset(new SimpleMatrix(nslawSize, sizeDS));
inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter);
DEBUG_EXPR(leftInteractionBlock->display(););
// Computing depends on relation type -> move this in Interaction method?
if (relationType == FirstOrder)
{
rightInteractionBlock.reset(new SimpleMatrix(sizeDS, nslawSize));
inter->getRightInteractionBlockForDS(pos, rightInteractionBlock, workMInter);
if (osiType == OSI::EULERMOREAUOSI)
{
if ((std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGamma() || (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGammaForRelation())
{
*rightInteractionBlock *= (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->gamma();
}
}
// for ZOH, we have a different formula ...
if (osiType == OSI::ZOHOSI && indexSet->properties(vd).forControl)
{
*rightInteractionBlock = std11::static_pointer_cast<ZeroOrderHoldOSI>(Osi)->Bd(ds);
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
else
{
// centralInteractionBlock contains a lu-factorized matrix and we solve
// centralInteractionBlock * X = rightInteractionBlock with PLU
SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
centralInteractionBlock->PLUForwardBackwardInPlace(*rightInteractionBlock);
inter->computeKhat(*rightInteractionBlock, workMInter, h); // if K is non 0
// integration of r with theta method removed
// *currentInteractionBlock += h *Theta[*itDS]* *leftInteractionBlock * (*rightInteractionBlock); //left = C, right = W.B
//gemm(h,*leftInteractionBlock,*rightInteractionBlock,1.0,*currentInteractionBlock);
*leftInteractionBlock *= h;
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
//left = C, right = inv(W).B
}
}
else if (relationType == Lagrangian ||
relationType == NewtonEuler)
{
SP::BoundaryCondition bc;
Type::Siconos dsType = Type::value(*ds);
if (dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS)
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
if (d->boundaryConditions()) bc = d->boundaryConditions();
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
if (d->boundaryConditions()) bc = d->boundaryConditions();
}
if (bc)
{
for (std::vector<unsigned int>::iterator itindex = bc->velocityIndices()->begin() ;
itindex != bc->velocityIndices()->end();
++itindex)
{
// (nslawSize,sizeDS));
SP::SiconosVector coltmp(new SiconosVector(nslawSize));
coltmp->zero();
leftInteractionBlock->setCol(*itindex, *coltmp);
}
}
DEBUG_PRINT("leftInteractionBlock after application of boundary conditions\n");
DEBUG_EXPR(leftInteractionBlock->display(););
std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor>
Topology::addInteractionInIndexSet0(SP::Interaction inter, SP::DynamicalSystem ds1, SP::DynamicalSystem ds2)
{
// !! Private function !!
//
// Add inter and ds into IG/DSG
// Compute number of constraints
unsigned int nsLawSize = inter->nonSmoothLaw()->size();
unsigned int m = inter->getSizeOfY() / nsLawSize;
if (m > 1)
RuntimeException::selfThrow("Topology::addInteractionInIndexSet0 - m > 1. Obsolete !");
_numberOfConstraints += nsLawSize;
SP::DynamicalSystem ds2_ = ds2;
// _DSG is the hyper forest : (vertices : dynamical systems, edges :
// Interactions)
//
// _IG is the hyper graph : (vertices : Interactions, edges :
// dynamical systems)
assert(_DSG[0]->edges_number() == _IG[0]->size());
// _IG = L(_DSG), L is the line graph transformation
// vector of the Interaction
DynamicalSystemsGraph::VDescriptor dsgv1, dsgv2;
dsgv1 = _DSG[0]->add_vertex(ds1);
SP::VectorOfVectors workVds1 = _DSG[0]->properties(dsgv1).workVectors;
SP::VectorOfVectors workVds2;
if (!workVds1)
{
workVds1.reset(new VectorOfVectors());
_DSG[0]->properties(dsgv1).workMatrices.reset(new VectorOfMatrices());
ds1->initWorkSpace(*workVds1, *_DSG[0]->properties(dsgv1).workMatrices);
}
if(ds2)
{
dsgv2 = _DSG[0]->add_vertex(ds2);
workVds2 = _DSG[0]->properties(dsgv2).workVectors;
if (!workVds2)
{
workVds2.reset(new VectorOfVectors());
_DSG[0]->properties(dsgv2).workMatrices.reset(new VectorOfMatrices());
ds2->initWorkSpace(*workVds2, *_DSG[0]->properties(dsgv2).workMatrices);
}
}
else
{
dsgv2 = dsgv1;
ds2_ = ds1;
workVds2 = workVds1;
}
// this may be a multi edges graph
assert(!_DSG[0]->is_edge(dsgv1, dsgv2, inter));
assert(!_IG[0]->is_vertex(inter));
InteractionsGraph::VDescriptor ig_new_ve;
DynamicalSystemsGraph::EDescriptor new_ed;
std11::tie(new_ed, ig_new_ve) = _DSG[0]->add_edge(dsgv1, dsgv2, inter, *_IG[0]);
InteractionProperties& interProp = _IG[0]->properties(ig_new_ve);
interProp.DSlink.reset(new VectorOfBlockVectors);
interProp.workVectors.reset(new VectorOfVectors);
interProp.workMatrices.reset(new VectorOfSMatrices);
unsigned int nslawSize = inter->nonSmoothLaw()->size();
interProp.block.reset(new SimpleMatrix(nslawSize, nslawSize));
inter->setDSLinkAndWorkspace(interProp, *ds1, *workVds1, *ds2_, *workVds2);
// add self branches in vertex properties
// note : boost graph SEGFAULT on self branch removal
// see https://svn.boost.org/trac/boost/ticket/4622
_IG[0]->properties(ig_new_ve).source = ds1;
_IG[0]->properties(ig_new_ve).source_pos = 0;
if(!ds2)
{
_IG[0]->properties(ig_new_ve).target = ds1;
_IG[0]->properties(ig_new_ve).target_pos = 0;
}
else
{
_IG[0]->properties(ig_new_ve).target = ds2;
_IG[0]->properties(ig_new_ve).target_pos = ds1->getDim();
}
assert(_IG[0]->bundle(ig_new_ve) == inter);
assert(_IG[0]->is_vertex(inter));
assert(_DSG[0]->is_edge(dsgv1, dsgv2, inter));
assert(_DSG[0]->edges_number() == _IG[0]->size());
return std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor>(new_ed, ig_new_ve);
}
void LsodarOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp)
{
SP::OneStepNSProblems allOSNS = simulationLink->oneStepNSProblems();
SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel());
SP::Interaction inter = indexSet->bundle(vertex_inter);
VectorOfBlockVectors& DSlink = *indexSet->properties(vertex_inter).DSlink;
// Get relation and non smooth law types
RELATION::TYPES relationType = inter->relation()->getType();
RELATION::SUBTYPES relationSubType = inter->relation()->getSubType();
unsigned int sizeY = inter->nonSmoothLaw()->size();
unsigned int relativePosition = 0;
SP::Interaction mainInteraction = inter;
Index coord(8);
coord[0] = relativePosition;
coord[1] = relativePosition + sizeY;
coord[2] = 0;
coord[4] = 0;
coord[6] = 0;
coord[7] = sizeY;
SP::SiconosMatrix C;
// SP::SiconosMatrix D;
// SP::SiconosMatrix F;
SiconosVector& yForNSsolver = *inter->yForNSsolver();
SP::BlockVector Xfree;
// All of these values should be stored in the node corrseponding to the Interactionwhen a MoreauJeanOSI scheme is used.
/* V.A. 10/10/2010
* Following the type of OSNS we need to retrieve the velocity or the acceleration
* This tricks is not very nice but for the moment the OSNS do not known if
* it is in accelaration of not
*/
//SP::OneStepNSProblems allOSNS = _simulation->oneStepNSProblems();
if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp)
{
if (relationType == Lagrangian)
{
Xfree = DSlink[LagrangianR::xfree];
}
// else if (relationType == NewtonEuler)
// {
// Xfree = inter->data(NewtonEulerR::free);
// }
assert(Xfree);
// std::cout << "Computeqblock Xfree (Gamma)========" << std::endl;
// Xfree->display();
}
else if (((*allOSNS)[SICONOS_OSNSP_ED_IMPACT]).get() == osnsp)
{
Xfree = DSlink[LagrangianR::q1];
// std::cout << "Computeqblock Xfree (Velocity)========" << std::endl;
// Xfree->display();
}
else
RuntimeException::selfThrow(" computeqBlock for Event Event-driven is wrong ");
if (relationType == Lagrangian)
{
C = mainInteraction->relation()->C();
if (C)
{
assert(Xfree);
coord[3] = C->size(1);
coord[5] = C->size(1);
subprod(*C, *Xfree, yForNSsolver, coord, true);
}
SP::SiconosMatrix ID(new SimpleMatrix(sizeY, sizeY));
ID->eye();
Index xcoord(8);
xcoord[0] = 0;
xcoord[1] = sizeY;
xcoord[2] = 0;
xcoord[3] = sizeY;
xcoord[4] = 0;
xcoord[5] = sizeY;
xcoord[6] = 0;
xcoord[7] = sizeY;
// For the relation of type LagrangianRheonomousR
if (relationSubType == RheonomousR)
{
if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp)
{
RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not yet implemented for LCP at acceleration level with LagrangianRheonomousR");
}
else if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY]).get() == osnsp)
{
SiconosVector q = *DSlink[LagrangianR::q0];
SiconosVector z = *DSlink[LagrangianR::z];
std11::static_pointer_cast<LagrangianRheonomousR>(inter->relation())->computehDot(simulation()->getTkp1(), q, z);
*DSlink[LagrangianR::z] = z;
subprod(*ID, *(std11::static_pointer_cast<LagrangianRheonomousR>(inter->relation())->hDot()), yForNSsolver, xcoord, false); // y += hDot
}
else
RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not implemented for SICONOS_OSNSP ");
}
// For the relation of type LagrangianScleronomousR
if (relationSubType == ScleronomousR)
{
if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp)
{
std11::static_pointer_cast<LagrangianScleronomousR>(inter->relation())->computedotjacqhXqdot(simulation()->getTkp1(), *inter, DSlink);
subprod(*ID, *(std11::static_pointer_cast<LagrangianScleronomousR>(inter->relation())->dotjacqhXqdot()), yForNSsolver, xcoord, false); // y += NonLinearPart
}
}
}
else
RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not yet implemented for Relation of type " + relationType);
if (((*allOSNS)[SICONOS_OSNSP_ED_IMPACT]).get() == osnsp)
{
if (inter->relation()->getType() == Lagrangian || inter->relation()->getType() == NewtonEuler)
{
SP::SiconosVisitor nslEffectOnFreeOutput(new _NSLEffectOnFreeOutput(osnsp, inter));
inter->nonSmoothLaw()->accept(*nslEffectOnFreeOutput);
}
}
}
void SchatzmanPaoliOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp)
{
/** \warning: ensures that it can also work with two different osi for two different ds ?
*/
SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel());
SP::Interaction inter = indexSet->bundle(vertex_inter);
SP::OneStepNSProblems allOSNS = simulationLink->oneStepNSProblems();
VectorOfBlockVectors& DSlink = *indexSet->properties(vertex_inter).DSlink;
// Get relation and non smooth law types
RELATION::TYPES relationType = inter->relation()->getType();
RELATION::SUBTYPES relationSubType = inter->relation()->getSubType();
unsigned int sizeY = inter->nonSmoothLaw()->size();
unsigned int relativePosition = 0;
Index coord(8);
coord[0] = relativePosition;
coord[1] = relativePosition + sizeY;
coord[2] = 0;
coord[4] = 0;
coord[6] = 0;
coord[7] = sizeY;
SP::SiconosMatrix C;
SP::SiconosMatrix D;
SP::SiconosMatrix F;
SP::BlockVector deltax;
SiconosVector& yForNSsolver = *inter->yForNSsolver();
SP::SiconosVector e;
SP::BlockVector Xfree;
if (relationType == NewtonEuler)
{
Xfree = DSlink[NewtonEulerR::xfree];
}
else if (relationType == Lagrangian)
{
Xfree = DSlink[LagrangianR::xfree];
}
assert(Xfree);
assert(Xfree);
SP::Interaction mainInteraction = inter;
assert(mainInteraction);
assert(mainInteraction->relation());
if (relationSubType == LinearTIR)
{
if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY]).get() != osnsp)
RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeOutput not yet implemented for SICONOS_OSNSP ");
C = mainInteraction->relation()->C();
if (C)
{
assert(Xfree);
coord[3] = C->size(1);
coord[5] = C->size(1);
// creates a POINTER link between workX[ds] (xfree) and the
// corresponding interactionBlock in each Interactionfor each ds of the
// current Interaction.
if (_useGammaForRelation)
{
assert(deltax);
subprod(*C, *deltax, yForNSsolver, coord, true);
}
else
{
subprod(*C, *Xfree, yForNSsolver, coord, true);
// subprod(*C,*(*(mainInteraction->dynamicalSystemsBegin()))->workspace(DynamicalSystem::free),*Yp,coord,true);
// if (mainInteraction->dynamicalSystems()->size() == 2)
// {
// subprod(*C,*(*++(mainInteraction->dynamicalSystemsBegin()))->workspace(DynamicalSystem::free),*Yp,coord,false);
// }
}
}
SP::LagrangianLinearTIR ltir = std11::static_pointer_cast<LagrangianLinearTIR> (mainInteraction->relation());
e = ltir->e();
if (e)
{
yForNSsolver += *e;
}
}
else
RuntimeException::selfThrow("SchatzmanPaoliOSI::ComputeFreeOutput not yet implemented for relation of Type : " + relationType);
if (inter->relation()->getSubType() == LinearTIR)
{
SP::SiconosVisitor nslEffectOnFreeOutput(new _NSLEffectOnFreeOutput(osnsp, inter));
inter->nonSmoothLaw()->accept(*nslEffectOnFreeOutput);
}
}
void OneStepNSProblem::updateInteractionBlocks()
{
DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks() starts\n");
// The present functions checks various conditions and possibly
// compute interactionBlocks matrices.
//
// Let interi and interj be two Interactions.
//
// Things to be checked are:
// 1 - is the topology time invariant?
// 2 - does interactionBlocks[interi][interj] already exists (ie has been
// computed in a previous time step)?
// 3 - do we need to compute this interactionBlock? A interactionBlock is
// to be computed if interi and interj are in IndexSet1 AND if interi and
// interj have common DynamicalSystems.
//
// The possible cases are:
//
// - If 1 and 2 are true then it does nothing. 3 is not checked.
// - If 1 == true, 2 == false, 3 == false, it does nothing.
// - If 1 == true, 2 == false, 3 == true, it computes the
// interactionBlock.
// - If 1==false, 2 is not checked, and the interactionBlock is
// computed if 3==true.
//
// Get index set from Simulation
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
bool isLinear = simulation()->model()->nonSmoothDynamicalSystem()->isLinear();
// we put diagonal informations on vertices
// self loops with bgl are a *nightmare* at the moment
// (patch 65198 on standard boost install)
if (indexSet->properties().symmetric)
{
DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Symmetric case");
InteractionsGraph::VIterator vi, viend;
for (std11::tie(vi, viend) = indexSet->vertices();
vi != viend; ++vi)
{
SP::Interaction inter = indexSet->bundle(*vi);
unsigned int nslawSize = inter->nonSmoothLaw()->size();
if (! indexSet->properties(*vi).block)
{
indexSet->properties(*vi).block.reset(new SimpleMatrix(nslawSize, nslawSize));
}
if (!isLinear || !_hasBeenUpdated)
{
computeDiagonalInteractionBlock(*vi);
}
}
/* interactionBlock must be zeroed at init */
std::vector<bool> initialized;
initialized.resize(indexSet->edges_number());
std::fill(initialized.begin(), initialized.end(), false);
InteractionsGraph::EIterator ei, eiend;
for (std11::tie(ei, eiend) = indexSet->edges();
ei != eiend; ++ei)
{
SP::Interaction inter1 = indexSet->bundle(indexSet->source(*ei));
SP::Interaction inter2 = indexSet->bundle(indexSet->target(*ei));
/* on adjoint graph there is at most 2 edges between source and target */
InteractionsGraph::EDescriptor ed1, ed2;
std11::tie(ed1, ed2) = indexSet->edges(indexSet->source(*ei), indexSet->target(*ei));
assert(*ei == ed1 || *ei == ed2);
/* the first edge has the lower index */
assert(indexSet->index(ed1) <= indexSet->index(ed2));
// Memory allocation if needed
unsigned int nslawSize1 = inter1->nonSmoothLaw()->size();
unsigned int nslawSize2 = inter2->nonSmoothLaw()->size();
unsigned int isrc = indexSet->index(indexSet->source(*ei));
unsigned int itar = indexSet->index(indexSet->target(*ei));
SP::SiconosMatrix currentInteractionBlock;
if (itar > isrc) // upper block
{
if (! indexSet->properties(ed1).upper_block)
{
indexSet->properties(ed1).upper_block.reset(new SimpleMatrix(nslawSize1, nslawSize2));
if (ed2 != ed1)
indexSet->properties(ed2).upper_block = indexSet->properties(ed1).upper_block;
}
currentInteractionBlock = indexSet->properties(ed1).upper_block;
}
else // lower block
{
if (! indexSet->properties(ed1).lower_block)
{
indexSet->properties(ed1).lower_block.reset(new SimpleMatrix(nslawSize1, nslawSize2));
if (ed2 != ed1)
indexSet->properties(ed2).lower_block = indexSet->properties(ed1).lower_block;
}
currentInteractionBlock = indexSet->properties(ed1).lower_block;
}
if (!initialized[indexSet->index(ed1)])
{
initialized[indexSet->index(ed1)] = true;
currentInteractionBlock->zero();
}
if (!isLinear || !_hasBeenUpdated)
{
{
computeInteractionBlock(*ei);
}
// allocation for transposed block
// should be avoided
if (itar > isrc) // upper block has been computed
{
if (!indexSet->properties(ed1).lower_block)
{
indexSet->properties(ed1).lower_block.
reset(new SimpleMatrix(indexSet->properties(ed1).upper_block->size(1),
indexSet->properties(ed1).upper_block->size(0)));
}
indexSet->properties(ed1).lower_block->trans(*indexSet->properties(ed1).upper_block);
indexSet->properties(ed2).lower_block = indexSet->properties(ed1).lower_block;
}
else
{
assert(itar < isrc); // lower block has been computed
if (!indexSet->properties(ed1).upper_block)
{
indexSet->properties(ed1).upper_block.
reset(new SimpleMatrix(indexSet->properties(ed1).lower_block->size(1),
indexSet->properties(ed1).lower_block->size(0)));
}
indexSet->properties(ed1).upper_block->trans(*indexSet->properties(ed1).lower_block);
indexSet->properties(ed2).upper_block = indexSet->properties(ed1).upper_block;
}
}
}
}
else // not symmetric => follow out_edges for each vertices
{
DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Non symmetric case\n");
InteractionsGraph::VIterator vi, viend;
for (std11::tie(vi, viend) = indexSet->vertices();
vi != viend; ++vi)
{
DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Computation of diaganal block\n");
SP::Interaction inter = indexSet->bundle(*vi);
unsigned int nslawSize = inter->nonSmoothLaw()->size();
if (! indexSet->properties(*vi).block)
{
indexSet->properties(*vi).block.reset(new SimpleMatrix(nslawSize, nslawSize));
}
if (!isLinear || !_hasBeenUpdated)
{
computeDiagonalInteractionBlock(*vi);
}
/* on a undirected graph, out_edges gives all incident edges */
InteractionsGraph::OEIterator oei, oeiend;
/* interactionBlock must be zeroed at init */
std::map<SP::SiconosMatrix, bool> initialized;
for (std11::tie(oei, oeiend) = indexSet->out_edges(*vi);
oei != oeiend; ++oei)
{
/* on adjoint graph there is at most 2 edges between source and target */
InteractionsGraph::EDescriptor ed1, ed2;
std11::tie(ed1, ed2) = indexSet->edges(indexSet->source(*oei), indexSet->target(*oei));
if (indexSet->properties(ed1).upper_block)
{
initialized[indexSet->properties(ed1).upper_block] = false;
}
// if(indexSet->properties(ed2).upper_block)
// {
// initialized[indexSet->properties(ed2).upper_block] = false;
// }
if (indexSet->properties(ed1).lower_block)
{
initialized[indexSet->properties(ed1).lower_block] = false;
}
// if(indexSet->properties(ed2).lower_block)
// {
// initialized[indexSet->properties(ed2).lower_block] = false;
// }
}
for (std11::tie(oei, oeiend) = indexSet->out_edges(*vi);
oei != oeiend; ++oei)
{
DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Computation of extra-diaganal block\n");
/* on adjoint graph there is at most 2 edges between source and target */
InteractionsGraph::EDescriptor ed1, ed2;
std11::tie(ed1, ed2) = indexSet->edges(indexSet->source(*oei), indexSet->target(*oei));
assert(*oei == ed1 || *oei == ed2);
/* the first edge as the lower index */
assert(indexSet->index(ed1) <= indexSet->index(ed2));
SP::Interaction inter1 = indexSet->bundle(indexSet->source(*oei));
SP::Interaction inter2 = indexSet->bundle(indexSet->target(*oei));
// Memory allocation if needed
unsigned int nslawSize1 = inter1->nonSmoothLaw()->size();
unsigned int nslawSize2 = inter2->nonSmoothLaw()->size();
unsigned int isrc = indexSet->index(indexSet->source(*oei));
unsigned int itar = indexSet->index(indexSet->target(*oei));
SP::SiconosMatrix currentInteractionBlock;
if (itar > isrc) // upper block
{
if (! indexSet->properties(ed1).upper_block)
{
indexSet->properties(ed1).upper_block.reset(new SimpleMatrix(nslawSize1, nslawSize2));
initialized[indexSet->properties(ed1).upper_block] = false;
if (ed2 != ed1)
indexSet->properties(ed2).upper_block = indexSet->properties(ed1).upper_block;
}
currentInteractionBlock = indexSet->properties(ed1).upper_block;
}
else // lower block
{
if (! indexSet->properties(ed1).lower_block)
{
indexSet->properties(ed1).lower_block.reset(new SimpleMatrix(nslawSize1, nslawSize2));
initialized[indexSet->properties(ed1).lower_block] = false;
if (ed2 != ed1)
indexSet->properties(ed2).lower_block = indexSet->properties(ed1).lower_block;
}
currentInteractionBlock = indexSet->properties(ed1).lower_block;
}
if (!initialized[currentInteractionBlock])
{
initialized[currentInteractionBlock] = true;
currentInteractionBlock->zero();
}
if (!isLinear || !_hasBeenUpdated)
{
if (isrc != itar)
computeInteractionBlock(*oei);
}
}
}
}
DEBUG_EXPR(displayBlocks(indexSet););
void MLCPProjectOnConstraints::computeOptions(SP::Interaction inter1, SP::Interaction inter2)
{
// printf("MLCPProjectOnConstraints::computeOptions\n");
// Get dimension of the NonSmoothLaw (ie dim of the interactionBlock)
RELATION::TYPES relationType1;
relationType1 = inter1->relation()->getType();
// Retrieve size of Y (projected variable)
unsigned int sizeY1;
sizeY1 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter1);
// Compute the number of equalities
unsigned int equalitySize1 = sizeY1; //default behavior
if (Type::value(*(inter1->nonSmoothLaw())) == Type::NewtonImpactFrictionNSL ||
Type::value(*(inter1->nonSmoothLaw())) == Type::NewtonImpactNSL)
{
if (_doProjOnEquality)
{
equalitySize1 = sizeY1;
}
else
{
equalitySize1 = 0;
}
}
else if (Type::value(*(inter1->nonSmoothLaw()))
== Type::MixedComplementarityConditionNSL)
{
equalitySize1 = std11::static_pointer_cast<MixedComplementarityConditionNSL>(inter1->nonSmoothLaw())->getEqualitySize();
}
// Compute the number of inequalities
unsigned int inequalitySize1 = sizeY1 - equalitySize1;
if (inter1 == inter2)
{
//inter1->getExtraInteractionBlock(currentInteractionBlock);
_m += inequalitySize1;
_n += equalitySize1;
// _m=0;
//_n=6;
if (_curBlock > MLCP_NB_BLOCKS - 2)
printf("MLCP.cpp : number of block to small, memory crach below!!!\n");
/*add an equality block.*/
// #ifdef MLCPPROJ_DEBUG
// printf("MLCPProjectOnConstraints::computeOptions()\n");
// #endif
if (equalitySize1 > 0)
{
_numerics_problem.blocksRows[_curBlock + 1] = _numerics_problem.blocksRows[_curBlock] + equalitySize1;
_numerics_problem.blocksIsComp[_curBlock] = 0;
// #ifdef MLCPPROJ_DEBUG
// std::cout << "_curBlock : " << _curBlock <<std::endl;
// std::cout << "_numerics_problem.blocksRows["<<_curBlock+1 <<" ] : " << _numerics_problem.blocksRows[_curBlock+1] <<std::endl;
// std::cout << "_numerics_problem.blocksIsComp["<<_curBlock <<" ] : " << _numerics_problem.blocksIsComp[_curBlock] <<std::endl;
// #endif
_curBlock++;
}
/*add a complementarity block.*/
if (inequalitySize1 > 0)
{
_numerics_problem.blocksRows[_curBlock + 1] = _numerics_problem.blocksRows[_curBlock] + inequalitySize1;
_numerics_problem.blocksIsComp[_curBlock] = 1;
// #ifdef MLCPPROJ_DEBUG
// std::cout << "_curBlock : " << _curBlock <<std::endl;
// std::cout << "_numerics_problem.blocksRows["<<_curBlock+1<< "] : " << _numerics_problem.blocksRows[_curBlock+1] <<std::endl;
// std::cout << "_numerics_problem.blocksIsComp["<<_curBlock<< "] : " << _numerics_problem.blocksIsComp[_curBlock] <<std::endl;
// #endif
_curBlock++;
}
}
// #ifdef MLCPPROJ_DEBUG
// std::cout << "_m : " << _m <<std::endl;
// std::cout << "_n : " << _n <<std::endl;
// #endif
}
void MLCPProjectOnConstraints::postComputeLagrangianR(SP::Interaction inter, unsigned int pos)
{
SP::LagrangianR lr = std11::static_pointer_cast<LagrangianR>(inter->relation());
#ifdef MLCPPROJ_DEBUG
printf("MLCPProjectOnConstraints::postComputeLagrangian inter->y(0)\n");
inter->y(0)->display();
printf("MLCPProjectOnConstraints::postComputeLagrangian lr->jachq \n");
lr->jachq()->display();
printf("MLCPProjectOnConstraints::postComputeLagrangianR q before update\n");
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
InteractionsGraph::VDescriptor ui = indexSet->descriptor(inter);
InteractionsGraph::OEIterator oei, oeiend;
for(std11::tie(oei, oeiend) = indexSet->out_edges(ui);
oei != oeiend; ++oei)
{
SP::LagrangianDS lds = std11::static_pointer_cast<LagrangianDS>(indexSet->bundle(*oei));
lds->q()->display();
}
#endif
//unsigned int sizeY = inter->nonSmoothLaw()->size();
// y and lambda vectors
SP::SiconosVector lambda = inter->lambda(0);
SP::SiconosVector y = inter->y(0);
unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter);
// Copy _w/_z values, starting from index pos into y/lambda.
//setBlock(*_w, y, sizeY, pos, 0);
setBlock(*_z, lambda, sizeY, pos, 0);
#ifdef MLCPPROJ_DEBUG
printf("MLCPP lambda of Interaction is pos =%i :\n", pos);
// aBuff->display();
lambda->display();
unsigned int nslawsize = inter->nonSmoothLaw()->size();
SP::SiconosVector aBuff(new SiconosVector(nslawsize));
setBlock(*_z, aBuff, sizeY, pos, 0);
SP::SiconosMatrix J = lr->jachq();
SP::SimpleMatrix aux(new SimpleMatrix(*J));
aux->trans();
// SP::SiconosVector tmp(new SiconosVector(*(lr->q())));
// prod(*aux, *aBuff, *(tmp), false);
// //prod(*aux,*lambda,*(lr->q()),false);
// std:: std::cout << " tmp = tmp + J^T * lambda" << std::endl;
// tmp->display();
#endif
// // WARNING : Must not be done here. and should be called with the correct time.
// // compute p(0)
// inter->computeInput(0.0 ,0);
// // \warning aBuff should normally be in lambda[0]
// // The update of the position in DS should be made
// // in MoreauJeanOSI::upateState or ProjectedMoreauJeanOSI::updateState
// SP::SiconosMatrix J=lr->jachq();
// SP::SimpleMatrix aux(new SimpleMatrix(*J));
// aux->trans();
// SP::SiconosVector tmp (new SiconosVector(*(lr->q())));
// std:: std::cout << " tmp ="<<std::endl;
// tmp->display();
// std:: std::cout << " lr->q() ="<<std::endl;
// lr->q()->display();
// //prod(*aux,*lambda,*(lr->q()),false);
// prod(*aux,*aBuff,*(tmp),false);
// std:: std::cout << " tmp = tmp + J * lambda"<<std::endl;
// tmp->display();
// // The following step should be done on MoreauJeanOSI::upateState or ProjectedMoreauJeanOSI::updateState
// DSIterator itDS = inter->dynamicalSystemsBegin();
// while(itDS!=inter->dynamicalSystemsEnd())
// {
// Type::Siconos dsType = Type::value(**itDS);
// if((dsType !=Type::LagrangianDS) and
// (dsType !=Type::LagrangianLinearTIDS) )
// {
// RuntimeException::selfThrow("MLCPProjectOnConstraint::postCompute- ds is not of Lagrangian DS type.");
// }
// SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (*itDS);
// SP::SiconosVector q = d->q();
// *q += *d->p(0);
// std::cout << " q=" << std::endl;
// q->display();
// itDS++;
// }
// if ((*lr->q() - *tmp).normInf() > 1e-12)
// {
// RuntimeException::selfThrow("youyou");
// }
#ifdef MLCPPROJ_DEBUG
printf("MLCPProjectOnConstraints::postComputeLagrangianR _z\n");
_z->display();
printf("MLCPProjectOnConstraints::postComputeLagrangianR updated\n");
VectorOfBlockVectors& DSlink = *(indexSet->properties(ui)).DSlink;
// (*DSlink[LagrangianR::q0]).display();
// (lr->q())->display();
#endif
//RuntimeException::selfThrow("MLCPProjectOnConstraints::postComputeLagrangianR() - not yet implemented");
}