void MLCPProjectOnConstraints::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)
{
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
SP::DynamicalSystem DS1 = indexSet->properties(vd).source;
SP::DynamicalSystem DS2 = indexSet->properties(vd).target;
SP::Interaction inter = indexSet->bundle(vd);
SP::OneStepIntegrator Osi = indexSet->properties(vd).osi;
unsigned int pos1, pos2;
pos1 = indexSet->properties(vd).source_pos;
pos2 = indexSet->properties(vd).target_pos;
unsigned int sizeY = 0;
sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter);
#ifdef MLCPPROJ_DEBUG
std::cout << "\nMLCPProjectOnConstraints::computeDiagonalInteractionBlock" <<std::endl;
std::cout << "indexSetLevel()" << indexSetLevel() << std::endl;
// std::cout << "indexSet :"<< indexSet << std::endl;
// std::cout << "vd :"<< vd << std::endl;
// indexSet->display();
// std::cout << "DS1 :" << std::endl;
// DS1->display();
// std::cout << "DS2 :" << std::endl;
// DS2->display();
#endif
assert(indexSet->blockProj[vd]);
SP::SiconosMatrix currentInteractionBlock = indexSet->blockProj[vd];
#ifdef MLCPPROJ_DEBUG
// std::cout<<"MLCPProjectOnConstraints::computeDiagonalInteractionBlock "<<std::endl;
// currentInteractionBlock->display();
std::cout << "sizeY " << sizeY << std::endl;
std::cout << "blockProj " << indexSet->blockProj[vd].get() << " of edge " << vd << " of size " << currentInteractionBlock->size(0) << " x " << currentInteractionBlock->size(0) << " for interaction " << inter->number() << std::endl;
// std::cout<<"inter1->display() "<< inter1->number()<< std::endl;
//inter1->display();
// std::cout<<"inter2->display() "<< inter2->number()<< std::endl;
//inter2->display();
#endif
assert(currentInteractionBlock->size(0) == sizeY);
assert(currentInteractionBlock->size(1) == sizeY);
if (!_hasBeenUpdated)
computeOptions(inter, inter);
// Computes matrix _interactionBlocks[inter1][inter2] (and allocates memory if
// necessary) if inter1 and inter2 have commond DynamicalSystem. 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 the W and Theta maps of one of the Interaction -
// Warning: in the current version, if OSI!=MoreauJeanOSI, this fails.
// If OSI = MOREAU, centralInteractionBlocks = W if OSI = LSODAR,
// centralInteractionBlocks = M (mass matrices)
SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock, leftInteractionBlock1;
// 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.
VectorOfSMatrices& workMInter = *indexSet->properties(vd).workMatrices;
currentInteractionBlock->zero();
// loop over the common DS
bool endl = false;
unsigned int pos = pos1;
for (SP::DynamicalSystem ds = DS1; !endl; ds = DS2)
{
assert(ds == DS1 || ds == DS2);
endl = (ds == DS2);
if (Type::value(*ds) == Type::LagrangianLinearTIDS ||
Type::value(*ds) == Type::LagrangianDS)
{
if (inter->relation()->getType() != Lagrangian)
{
RuntimeException::selfThrow(
"MLCPProjectOnConstraints::computeDiagonalInteractionBlock - relation is not of type Lagrangian with a LagrangianDS.");
}
SP::LagrangianDS lds = (std11::static_pointer_cast<LagrangianDS>(ds));
unsigned int sizeDS = lds->getDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS));
inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter);
if (lds->boundaryConditions()) // V.A. Should we do that ?
{
for (std::vector<unsigned int>::iterator itindex =
lds->boundaryConditions()->velocityIndices()->begin() ;
itindex != lds->boundaryConditions()->velocityIndices()->end();
++itindex)
{
// (sizeY,sizeDS));
SP::SiconosVector coltmp(new SiconosVector(sizeY));
coltmp->zero();
leftInteractionBlock->setCol(*itindex, *coltmp);
}
}
// (inter1 == inter2)
SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
//
// std::cout<<"LinearOSNS : leftUBlock\n";
// work->display();
work->trans();
// std::cout<<"LinearOSNS::computeInteractionBlock leftInteractionBlock"<<endl;
// leftInteractionBlock->display();
if (_useMassNormalization)
{
SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
centralInteractionBlock->PLUForwardBackwardInPlace(*work);
prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
// gemm(CblasNoTrans,CblasNoTrans,1.0,*leftInteractionBlock,*work,1.0,*currentInteractionBlock);
}
else
{
prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
}
//*currentInteractionBlock *=h;
}
else if (Type::value(*ds) == Type::NewtonEulerDS)
{
if (inter->relation()->getType() != NewtonEuler)
{
RuntimeException::selfThrow("MLCPProjectOnConstraints::computeDiagonalInteractionBlock - relation is not from NewtonEulerR.");
}
SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds));
#ifdef MLCPPROJ_WITH_CT
unsigned int sizeDS = neds->getDim();
SP::SimpleMatrix T = neds->T();
SP::SimpleMatrix workT(new SimpleMatrix(*T));
workT->trans();
SP::SimpleMatrix workT2(new SimpleMatrix(6, 6));
prod(*workT, *T, *workT2, true);
leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS));
inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock);
SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
std::cout << "LinearOSNS : leftUBlock\n";
work->display();
work->trans();
std::cout << "LinearOSNS::computeInteractionBlock workT2" <<std::endl;
workT2->display();
workT2->PLUForwardBackwardInPlace(*work);
prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
#else
if (0) //(std11::static_pointer_cast<NewtonEulerR> inter->relation())->_isConstact){
{
// unsigned int sizeDS = neds->getDim();
// SP::SimpleMatrix T = neds->T();
// SP::SimpleMatrix workT(new SimpleMatrix(*T));
// workT->trans();
// SP::SimpleMatrix workT2(new SimpleMatrix(6, 6));
// prod(*workT, *T, *workT2, true);
// leftInteractionBlock1.reset(new SimpleMatrix(sizeY, sizeDS));
// inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock);
// leftInteractionBlock.reset(new SimpleMatrix(1, sizeDS));
// for (unsigned int ii = 0; ii < sizeDS; ii++)
// leftInteractionBlock->setValue(1, ii, leftInteractionBlock1->getValue(1, ii));
//
// SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
// //cout<<"LinearOSNS : leftUBlock\n";
// //work->display();
// work->trans();
// //cout<<"LinearOSNS::computeInteractionBlock workT2"<<endl;
// //workT2->display();
// workT2->PLUForwardBackwardInPlace(*work);
// prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
}
else
{
unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getqDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS));
inter->getLeftInteractionBlockForDSProjectOnConstraints(pos, leftInteractionBlock);
// #ifdef MLCPPROJ_DEBUG
// std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock - NewtonEuler case leftInteractionBlock : " << std::endl;
// leftInteractionBlock->display();
// #endif
SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
//cout<<"LinearOSNS sizeY="<<sizeY<<": leftUBlock\n";
//work->display();
work->trans();
prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
// #ifdef MLCPPROJ_DEBUG
// std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock - NewtonEuler case currentInteractionBlock : "<< std::endl;
// currentInteractionBlock->display();
// #endif
}
}
else
RuntimeException::selfThrow("MLCPProjectOnConstraints::computeDiagonalInteractionBlock - ds is not from NewtonEulerDS neither a LagrangianDS.");
#endif
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock DiaginteractionBlock " << std::endl;
currentInteractionBlock->display();
#endif
// Set pos for next loop.
pos = pos2;
}
}
