unsigned OSNSMatrix::updateSizeAndPositions(SP::InteractionsGraph indexSet)
{
// === Description ===
// For an interactionBlock (diagonal or extra diagonal) corresponding to
// an Interaction, we need to know the position of its first
// element in the full-matrix M. This position depends on the
// previous interactionBlocks sizes.
//
// Note FP: at the time positions are saved in the Interaction
// but this is wrong (I think) since it prevents the inter
// to be present in several different osns.
//
// Computes real size of the current matrix = sum of the dim. of all
// Interactionin indexSet
unsigned dim = 0;
InteractionsGraph::VIterator vd, vdend;
for (std11::tie(vd, vdend) = indexSet->vertices(); vd != vdend; ++vd)
{
assert(indexSet->descriptor(indexSet->bundle(*vd)) == *vd);
indexSet->bundle(*vd)->setAbsolutePosition(dim);
dim += (indexSet->bundle(*vd)->nonSmoothLaw()->size());
assert(indexSet->bundle(*vd)->absolutePosition() < dim);
}
return dim;
}
// 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(););
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");
}