// Fill the matrix
void OSNSMatrix::fill(SP::InteractionsGraph indexSet, bool update)
{
DEBUG_BEGIN("void OSNSMatrix::fill(SP::InteractionsGraph indexSet, bool update)\n");
assert(indexSet);
if (update)
{
// Computes _dimRow and interactionBlocksPositions according to indexSet
_dimColumn = updateSizeAndPositions(indexSet);
_dimRow = _dimColumn;
}
if (_storageType == NM_DENSE)
{
// === Memory allocation, if required ===
// Mem. is allocate only if !M or if its size has changed.
if (update)
{
if (! _M1)
_M1.reset(new SimpleMatrix(_dimRow, _dimColumn));
else
{
if (_M1->size(0) != _dimRow || _M1->size(1) != _dimColumn)
_M1->resize(_dimRow, _dimColumn);
_M1->zero();
}
}
// ======> Aim: find inter1 and inter2 both in indexSet and which have
// common DynamicalSystems. Then get the corresponding matrix
// from map interactionBlocks, and copy it into M
unsigned int pos = 0, col = 0; // index position used for
// interactionBlock copy into M, see
// below.
// === Loop through "active" Interactions (ie present in
// indexSets[level]) ===
InteractionsGraph::VIterator vi, viend;
for (std11::tie(vi, viend) = indexSet->vertices();
vi != viend; ++vi)
{
SP::Interaction inter = indexSet->bundle(*vi);
pos = inter->absolutePosition();
std11::static_pointer_cast<SimpleMatrix>(_M1)
->setBlock(pos, pos, *indexSet->properties(*vi).block);
DEBUG_PRINTF("OSNSMatrix _M1: %i %i\n", _M1->size(0), _M1->size(1));
DEBUG_PRINTF("OSNSMatrix block: %i %i\n", indexSet->properties(*vi).block->size(0), indexSet->properties(*vi).block->size(1));
}
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);
pos = inter1->absolutePosition();
assert(indexSet->is_vertex(inter2));
col = inter2->absolutePosition();
assert(pos < _dimRow);
assert(col < _dimColumn);
DEBUG_PRINTF("OSNSMatrix _M1: %i %i\n", _M1->size(0), _M1->size(1));
DEBUG_PRINTF("OSNSMatrix upper: %i %i\n", indexSet->properties(*ei).upper_block->size(0), indexSet->properties(*ei).upper_block->size(1));
DEBUG_PRINTF("OSNSMatrix lower: %i %i\n", indexSet->properties(*ei).lower_block->size(0), indexSet->properties(*ei).lower_block->size(1));
assert(indexSet->properties(*ei).lower_block);
assert(indexSet->properties(*ei).upper_block);
std11::static_pointer_cast<SimpleMatrix>(_M1)
->setBlock(std::min(pos, col), std::max(pos, col),
*indexSet->properties(*ei).upper_block);
std11::static_pointer_cast<SimpleMatrix>(_M1)
->setBlock(std::max(pos, col), std::min(pos, col),
*indexSet->properties(*ei).lower_block);
}
}
else if (_storageType == NM_SPARSE_BLOCK)
{
if (! _M2)
{
DEBUG_PRINT("Reset _M2 shared pointer using new BlockCSRMatrix(indexSet) \n ");
_M2.reset(new BlockCSRMatrix(indexSet));
}
else
{
DEBUG_PRINT("fill existing _M2\n");
_M2->fill(indexSet);
}
}
if (update)
convert();
DEBUG_END("void OSNSMatrix::fill(SP::InteractionsGraph indexSet, bool update)\n");
}
// 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 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::computeInteractionBlock(const InteractionsGraph::EDescriptor& ed)
{
// 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.
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock currentInteractionBlock start " << std::endl;
#endif
// Get dimension of the NonSmoothLaw (ie dim of the interactionBlock)
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
SP::DynamicalSystem ds = indexSet->bundle(ed);
SP::Interaction inter1 = indexSet->bundle(indexSet->source(ed));
SP::Interaction inter2 = indexSet->bundle(indexSet->target(ed));
// For the edge 'ds', we need to find relative position of this ds
// in inter1 and inter2 relation matrices (--> pos1 and pos2 below)
// - find if ds is source or target in inter_i
InteractionsGraph::VDescriptor vertex_inter;
// - get the corresponding position
unsigned int pos1, pos2;
// source of inter1 :
vertex_inter = indexSet->source(ed);
VectorOfSMatrices& workMInter1 = *indexSet->properties(vertex_inter).workMatrices;
SP::OneStepIntegrator Osi = indexSet->properties(vertex_inter).osi;
SP::DynamicalSystem tmpds = indexSet->properties(vertex_inter).source;
if (tmpds == ds)
pos1 = indexSet->properties(vertex_inter).source_pos;
else
{
tmpds = indexSet->properties(vertex_inter).target;
pos1 = indexSet->properties(vertex_inter).target_pos;
}
// now, inter2
vertex_inter = indexSet->target(ed);
VectorOfSMatrices& workMInter2 = *indexSet->properties(vertex_inter).workMatrices;
tmpds = indexSet->properties(vertex_inter).source;
if (tmpds == ds)
pos2 = indexSet->properties(vertex_inter).source_pos;
else
{
tmpds = indexSet->properties(vertex_inter).target;
pos2 = indexSet->properties(vertex_inter).target_pos;
}
unsigned int index1 = indexSet->index(indexSet->source(ed));
unsigned int index2 = indexSet->index(indexSet->target(ed));
unsigned int sizeY1 = 0;
sizeY1 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter1);
unsigned int sizeY2 = 0;
sizeY2 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter2);
SP::SiconosMatrix currentInteractionBlock;
assert(index1 != index2);
if (index2 > index1) // upper block
{
// if (! indexSet->properties(ed).upper_block)
// {
// indexSet->properties(ed).upper_block.reset(new SimpleMatrix(sizeY1, sizeY2));
// }
currentInteractionBlock = indexSet->upper_blockProj[ed];
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock currentInteractionBlock " << std::endl;
// currentInteractionBlock->display();
std::cout << "sizeY1 " << sizeY1 << std::endl;
std::cout << "sizeY2 " << sizeY2 << std::endl;
std::cout << "upper_blockProj " << indexSet->upper_blockProj[ed].get() << " of edge " << ed << " of size " << currentInteractionBlock->size(0) << " x " << currentInteractionBlock->size(0) << " for interaction " << inter1->number() << " and interaction " << inter2->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) == sizeY1);
assert(currentInteractionBlock->size(1) == sizeY2);
}
else // lower block
{
// if (! indexSet->properties(ed).lower_block)
// {
// indexSet->properties(ed).lower_block.reset(new SimpleMatrix(sizeY1, sizeY2));
// }
assert(indexSet->lower_blockProj[ed]->size(0) == sizeY1);
assert(indexSet->lower_blockProj[ed]->size(1) == sizeY2);
currentInteractionBlock = indexSet->lower_blockProj[ed];
}
SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock;
RELATION::TYPES relationType1, relationType2;
// 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.
relationType1 = inter1->relation()->getType();
relationType2 = inter2->relation()->getType();
if (relationType1 == NewtonEuler &&
relationType2 == NewtonEuler)
{
assert(inter1 != inter2);
currentInteractionBlock->zero();
#ifdef MLCPPROJ_WITH_CT
unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY1, sizeDS));
inter1->getLeftInteractionBlockForDS(pos1, leftInteractionBlock);
SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds));
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);
rightInteractionBlock.reset(new SimpleMatrix(sizeY2, sizeDS));
inter2->getLeftInteractionBlockForDS(pos2, rightInteractionBlock);
rightInteractionBlock->trans();
workT2->PLUForwardBackwardInPlace(*rightInteractionBlock);
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
#else
unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getqDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY1, sizeDS));
inter1->getLeftInteractionBlockForDSProjectOnConstraints(pos1, leftInteractionBlock);
SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds));
rightInteractionBlock.reset(new SimpleMatrix(sizeY2, sizeDS));
inter2->getLeftInteractionBlockForDSProjectOnConstraints(pos2, rightInteractionBlock);
rightInteractionBlock->trans();
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
#endif
else if (relationType1 == Lagrangian &&
relationType2 == Lagrangian)
{
unsigned int sizeDS = ds->getDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY1, sizeDS));
inter1->getLeftInteractionBlockForDS(pos1, leftInteractionBlock, workMInter1);
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()) // V.A. Should we do that ?
{
for (std::vector<unsigned int>::iterator itindex =
d->boundaryConditions()->velocityIndices()->begin() ;
itindex != d->boundaryConditions()->velocityIndices()->end();
++itindex)
{
// (sizeY1,sizeDS));
SP::SiconosVector coltmp(new SiconosVector(sizeY1));
coltmp->zero();
leftInteractionBlock->setCol(*itindex, *coltmp);
}
}
}
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : leftInteractionBlock" << std::endl;
leftInteractionBlock->display();
#endif
// inter1 != inter2
rightInteractionBlock.reset(new SimpleMatrix(sizeY2, sizeDS));
inter2->getLeftInteractionBlockForDS(pos2, rightInteractionBlock, workMInter2);
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : rightInteractionBlock" << std::endl;
rightInteractionBlock->display();
#endif
// Warning: we use getLeft for Right interactionBlock
// because right = transpose(left) and because of
// size checking inside the getBlock function, a
// getRight call will fail.
SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : centralInteractionBlocks " << std::endl;
centralInteractionBlock->display();
#endif
rightInteractionBlock->trans();
if (_useMassNormalization)
{
centralInteractionBlock->PLUForwardBackwardInPlace(*rightInteractionBlock);
//*currentInteractionBlock += *leftInteractionBlock ** work;
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
else
{
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : currentInteractionBlock" << std::endl;
currentInteractionBlock->display();
#endif
}
else
RuntimeException::selfThrow("MLCPProjectOnConstraints::computeInteractionBlock not yet implemented for relation of type " + relationType1);
}
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::updateInteractionBlocksOLD()
{
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
bool isLinear = simulation()->model()->nonSmoothDynamicalSystem()->isLinear();
// std::cout<<"isLinear: "<<isLinear<<" hasTopologyChanged: "<<hasTopologyChanged<<"hasBeenUpdated: "<<_hasBeenUpdated<<endl;
if (indexSet->properties().symmetric)
{
RuntimeException::selfThrow
("MLCPProjectOnConstraints::updateInteractionBlocks() - symmetric case for the indexSet is not yet implemented");
}
else // not symmetric => follow out_edges for each vertices
{
if (!_hasBeenUpdated || !isLinear)
{
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 sizeY = 0;
sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter);
// #ifdef MLCPPROJ_DEBUG
// std::cout<<"\nMLCPProjectOnConstraints::updateInteractionBlocks()"<<endl;
// std::cout << "indexSet :"<< indexSet << std::endl;
// indexSet->display();
// std::cout << "vi :"<< *vi << std::endl;
// std::cout << "indexSet->blockProj[*vi]: before"<< indexSet->blockProj[*vi] << std::endl;
// #endif
if (! indexSet->blockProj[*vi])
{
indexSet->blockProj[*vi].reset(new SimpleMatrix(sizeY, sizeY));
}
// #ifdef MLCPPROJ_DEBUG
// std::cout << "indexSet->blockProj[*vi]: after"<< indexSet->blockProj[*vi] << std::endl;
// #endif
computeDiagonalInteractionBlock(*vi);
}
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));
unsigned int sizeY1 = 0;
unsigned int sizeY2 = 0;
sizeY1 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter1);
sizeY2 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter2);
// Memory allocation if needed
unsigned int isrc = indexSet->index(indexSet->source(*ei));
unsigned int itar = indexSet->index(indexSet->target(*ei));
if (itar > isrc) // upper block
{
if (! indexSet->upper_blockProj[*ei])
{
indexSet->upper_blockProj[*ei].reset(new SimpleMatrix(sizeY1, sizeY2));
}
}
else // lower block
{
if (! indexSet->lower_blockProj[*ei])
{
indexSet->lower_blockProj[*ei].reset(new SimpleMatrix(sizeY1, sizeY2));
}
}
// Computation of the diagonal block
computeInteractionBlock(*ei);
// allocation for transposed block
// should be avoided
if (itar > isrc) // upper block has been computed
{
// if (!indexSet->lower_blockProj[*ei])
// {
// indexSet->lower_blockProj[*ei].
// reset(new SimpleMatrix(indexSet->upper_blockProj[*ei]->size(1),
// indexSet->upper_blockProj[*ei]->size(0)));
// }
indexSet->lower_blockProj[*ei].reset(new SimpleMatrix(*(indexSet->upper_blockProj[*ei])));
indexSet->lower_blockProj[*ei]->trans();
// indexSet->lower_blockProj[*ei]->trans(*indexSet->upper_blockProj[*ei]);
}
else
{
assert(itar < isrc); // lower block has been computed
// if (!indexSet->upper_blockProj[*ei])
// {
// indexSet->upper_blockProj[*ei].
// reset(new SimpleMatrix(indexSet->lower_blockProj[*ei]->size(1),
// indexSet->lower_blockProj[*ei]->size(0)));
// }
indexSet->upper_blockProj[*ei].
reset(new SimpleMatrix(*(indexSet->lower_blockProj[*ei])));
indexSet->upper_blockProj[*ei]->trans();
}
// #ifdef MLCPPROJ_DEBUG
// printf("MLCPP upper: %i %i\n",indexSet->upper_blockProj[*ei]->size(0),indexSet->upper_blockProj[*ei]->size(1));
// printf("MLCPP lower: %i %i\n",indexSet->lower_blockProj[*ei]->size(0),indexSet->lower_blockProj[*ei]->size(1));
// #endif
}
}
}
}
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;
}
}
}