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(););
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::displayBlocks(SP::InteractionsGraph indexSet)
{
std::cout << "MLCPProjectOnConstraints::displayBlocks(SP::InteractionsGraph indexSet) " << std::endl;
std::cout << " indexSet :" << indexSet << std::endl;
InteractionsGraph::VIterator vi, viend;
for (std11::tie(vi, viend) = indexSet->vertices();
vi != viend; ++vi)
{
SP::Interaction inter = indexSet->bundle(*vi);
std::cout << " vertex :" << *vi << std::endl;
std::cout << " bundle :" << indexSet->bundle(*vi) << std::endl;
if (indexSet->blockProj[*vi])
{
std::cout << " blockProj ";
indexSet->blockProj[*vi]->display();
}
InteractionsGraph::OEIterator oei, oeiend;
for (std11::tie(oei, oeiend) = indexSet->out_edges(*vi);
oei != oeiend; ++oei)
{
unsigned int isrc = indexSet->index(indexSet->source(*oei));
unsigned int itar = indexSet->index(indexSet->target(*oei));
std::cout << " isrc :" << isrc << std::endl;
std::cout << " itar :" << itar << std::endl;
InteractionsGraph::EDescriptor ed1, ed2;
std::cout << " outedges :" << *oei << std::endl;
std11::tie(ed1, ed2) = indexSet->edges(indexSet->source(*oei), indexSet->target(*oei));
std::cout << " edges(ed1,ed2) :" << ed1 << " " << ed2 << std::endl;
std::cout << " (ed1)->upper_blockProj : ";
if (indexSet->upper_blockProj[ed1])
{
std::cout << indexSet->upper_blockProj[ed1] << " :" ;
indexSet->upper_blockProj[ed1]->display();
}
else
std::cout << "NULL " << std::endl;
std::cout << " (ed1)->lower_blockProj : ";
if (indexSet->lower_blockProj[ed1])
{
std::cout << indexSet->lower_blockProj[ed1] << " :" ;
indexSet->lower_blockProj[ed1]->display();
}
else
std::cout << "NULL " << std::endl;
std::cout << " (ed2)->upper_blockProj : ";
if (indexSet->upper_blockProj[ed2])
{
std::cout << indexSet->upper_blockProj[ed2] << " :" ;
indexSet->upper_blockProj[ed2]->display();
}
else
std::cout << "NULL" << std::endl;
std::cout << " (ed2)->lower_blockProj : ";
if (indexSet->lower_blockProj[ed2])
{
std::cout << indexSet->lower_blockProj[ed2] << " :" ;
indexSet->lower_blockProj[ed2]->display();
}
else
std::cout << "NULL" << std::endl;
}
}
}
void MLCPProjectOnConstraints::updateInteractionBlocks()
{
// 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.
//
#ifdef MLCPPROJ_DEBUG
std::cout << " " << std::endl;
std::cout << "===================================================" << std::endl;
std::cout << "MLCPProjectOnConstraints::updateInteractionBlocks()" << std::endl;
#endif
// Get index set from Simulation
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
// It seems that index() in not update in Index(0)
// see comment in void Simulation::updateIndexSets()
if (indexSetLevel() == 0)
{
indexSet->update_vertices_indices();
indexSet->update_edges_indices();
}
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)
{
RuntimeException::selfThrow
(" MLCPProjectOnConstraints::updateInteractionBlocks() - not yet implemented for symmetric case");
}
else // not symmetric => follow out_edges for each vertices
{
if (!_hasBeenUpdated)
{
// printf("MLCPProjectOnConstraints::updateInteractionBlocks must be updated.\n");
_n = 0;
_m = 0;
_curBlock = 0;
}
InteractionsGraph::VIterator vi, viend;
for (std11::tie(vi, viend) = indexSet->vertices();
vi != viend; ++vi)
{
SP::Interaction inter = indexSet->bundle(*vi);
unsigned int nslawSize = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter);
#ifdef MLCPPROJ_DEBUG
std::cout << " " << std::endl;
std::cout << "Start to work on Interaction " << inter->number() << "of vertex" << *vi << std::endl;
#endif
if (! indexSet->blockProj[*vi])
{
#ifdef MLCPPROJ_DEBUG
std::cout << "Allocation of blockProj of size " << nslawSize << " x " << nslawSize << " for interaction " << inter->number() << std::endl;
#endif
indexSet->blockProj[*vi].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->upper_blockProj[ed1])
{
initialized[indexSet->upper_blockProj[ed1]] = false;
}
// if(indexSet->upper_blockProj[ed2])
// {
// initialized[indexSet->upper_blockProj[ed1]] = false;
// }
if (indexSet->lower_blockProj[ed1])
{
initialized[indexSet->lower_blockProj[ed2]] = false;
}
// if(indexSet->lower_blockProj[ed2])
// {
// initialized[indexSet->lower_blockProj[ed2]] = false;
// }
}
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));
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 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter1);
unsigned int nslawSize2 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter2);
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->upper_blockProj[ed1])
{
indexSet->upper_blockProj[ed1].reset(new SimpleMatrix(nslawSize1, nslawSize2));
initialized[indexSet->upper_blockProj[ed1]] = false;
#ifdef MLCPPROJ_DEBUG
std::cout << "Allocation of upper_blockProj " << indexSet->upper_blockProj[ed1].get() << " of edge " << ed1 << " of size " << nslawSize1 << " x " << nslawSize2 << " for interaction " << inter1->number() << " and interaction " << inter2->number() << std::endl;
#endif
if (ed2 != ed1)
indexSet->upper_blockProj[ed2] = indexSet->upper_blockProj[ed1];
}
#ifdef MLCPPROJ_DEBUG
else
std::cout << "No Allocation of upper_blockProj of size " << nslawSize1 << " x " << nslawSize2 << std::endl;
#endif
currentInteractionBlock = indexSet->upper_blockProj[ed1];
#ifdef MLCPPROJ_DEBUG
std::cout << "currentInteractionBlock->size(0)" << currentInteractionBlock->size(0) << std::endl;
std::cout << "currentInteractionBlock->size(1)" << currentInteractionBlock->size(1) << std::endl;
std::cout << "inter1->display() " << inter1->number() << std::endl;
//inter1->display();
std::cout << "inter2->display() " << inter2->number() << std::endl;
//inter2->display();
#endif
}
else // lower block
{
if (! indexSet->lower_blockProj[ed1])
{
#ifdef MLCPPROJ_DEBUG
std::cout << "Allocation of lower_blockProj of size " << nslawSize1 << " x " << nslawSize2 << " for interaction " << inter1->number() << " and interaction " << inter2->number() << std::endl;
#endif
indexSet->lower_blockProj[ed1].reset(new SimpleMatrix(nslawSize1, nslawSize2));
initialized[indexSet->lower_blockProj[ed1]] = false;
if (ed2 != ed1)
indexSet->lower_blockProj[ed2] = indexSet->lower_blockProj[ed1];
}
#ifdef MLCPPROJ_DEBUG
else
std::cout << "No Allocation of lower_blockProj of size " << nslawSize1 << " x " << nslawSize2 << std::endl;
#endif
currentInteractionBlock = indexSet->lower_blockProj[ed1];
#ifdef MLCPPROJ_DEBUG
std::cout << "currentInteractionBlock->size(0)" << currentInteractionBlock->size(0) << std::endl;
std::cout << "currentInteractionBlock->size(1)" << currentInteractionBlock->size(1) << std::endl;
std::cout << "inter1->display() " << inter1->number() << std::endl;
//inter1->display();
std::cout << "inter2->display() " << inter2->number() << std::endl;
//inter2->display();
#endif
}
//assert(indexSet->index(ed1));
if (!initialized[currentInteractionBlock])
{
initialized[currentInteractionBlock] = true;
currentInteractionBlock->zero();
}
if (!isLinear || !_hasBeenUpdated)
{
if (isrc != itar)
computeInteractionBlock(*oei);
}
}
}
}
#ifdef MLCPPROJ_DEBUG
displayBlocks(indexSet);
#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");
}
