Exemple #1
0
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;
}
void MLCPProjectOnConstraints::postCompute()
{
  _hasBeenUpdated = true;
  // This function is used to set y/lambda values using output from
  // lcp_driver (w,z).  Only Interactions (ie Interactions) of
  // indexSet(leveMin) are concerned.

  // === Get index set from Topology ===
  SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());

  // y and lambda vectors
  SP::SiconosVector lambda;
  SP::SiconosVector y;

  // === Loop through "active" Interactions (ie present in
  // indexSets[1]) ===
  /** We chose to do a small step _alpha in view of stabilized the algorithm.*/
#ifdef MLCPPROJ_DEBUG
  printf("MLCPProjectOnConstraints::postCompute damping value = %f\n", _alpha);
#endif
  (*_z) *= _alpha;
  unsigned int pos = 0;
#ifdef MLCPPROJ_DEBUG
  printf("MLCPProjectOnConstraints::postCompute _z\n");
  _z->display();
  display();
#endif



  InteractionsGraph::VIterator ui, uiend;

  for (std11::tie(ui, uiend) = indexSet->vertices(); ui != uiend; ++ui)
  {

    SP::Interaction inter = indexSet->bundle(*ui);
    // Get the relative position of inter-interactionBlock in the vector w
    // or z
    pos = _M->getPositionOfInteractionBlock(*inter);
    RELATION::TYPES relationType = inter->relation()->getType();
    if (relationType == NewtonEuler)
    {
      postComputeNewtonEulerR(inter, pos);
    }
    else if (relationType == Lagrangian)
    {
      postComputeLagrangianR(inter, pos);
    }
    else
    {
      RuntimeException::selfThrow("MLCPProjectOnConstraints::computeInteractionBlock - relation type is not from Lagrangian type neither NewtonEuler.");
    }

  }



}
Exemple #3
0
std::vector<SP::Interaction> interactions(SP::InteractionsGraph dsg)
{
    std::vector<SP::Interaction> r = std::vector<SP::Interaction>();
    InteractionsGraph::VIterator vi, viend;
    for (boost::tie(vi, viend) = dsg->vertices(); vi != viend; ++vi)
    {
        r.push_back(dsg->bundle(*vi));
    };
    return r;
};
void FrictionContact::updateMu()
{
  _mu->clear();
  SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
  InteractionsGraph::VIterator ui, uiend;
  for (std11::tie(ui, uiend) = indexSet->vertices(); ui != uiend; ++ui)
  {
    _mu->push_back(std11::static_pointer_cast<NewtonImpactFrictionNSL>
                   (indexSet->bundle(*ui)->nonSmoothLaw())->mu());
  }
}
Exemple #5
0
unsigned int Topology::numberOfInvolvedDS(unsigned int inumber)
{
   if (inumber >= _IG.size())
  {
    RuntimeException::selfThrow("index number must be inferior to the number of indexSets");
  }

  /* on an adjoint graph a dynamical system may be on several edges */
  std::map<SP::DynamicalSystem, bool> flag;

  unsigned int return_value = 0;

  SP::InteractionsGraph indexSet = _IG[inumber];

  InteractionsGraph::VIterator vi, viend;
  for(std11::tie(vi, viend) = indexSet->vertices();
      vi != viend; ++vi)
  {
    if(indexSet->properties(*vi).source)
    {
      if (flag.find(indexSet->properties(*vi).source) == flag.end())
      {
        flag[indexSet->properties(*vi).source] = true;
        return_value++;
      }
    }
    if(indexSet->properties(*vi).target)
    {
      if (flag.find(indexSet->properties(*vi).target) == flag.end())
      {
        flag[indexSet->properties(*vi).target] = true;
        return_value++;
      }
    }
  }

  InteractionsGraph::EIterator ei, eiend;

  for(std11::tie(ei, eiend) = indexSet->edges();
      ei != eiend; ++ei)
  {
    if (flag.find(indexSet->bundle(*ei)) == flag.end())
    {
      flag[indexSet->bundle(*ei)] = true;
      return_value++;
    }
  }

  return return_value;
}
void FrictionContact::initialize(SP::Simulation sim)
{
  // - Checks memory allocation for main variables (M,q,w,z)
  // - Formalizes the problem if the topology is time-invariant

  // This function performs all steps that are time-invariant

  // General initialize for OneStepNSProblem
  LinearOSNS::initialize(sim);

  // Connect to the right function according to dim. of the problem

  // get topology
  SP::Topology topology =
    simulation()->model()->nonSmoothDynamicalSystem()->topology();

  // Note that interactionBlocks is up to date since updateInteractionBlocks
  // has been called during OneStepNSProblem::initialize()

  // Fill vector of friction coefficients
  int sizeMu = simulation()->model()->nonSmoothDynamicalSystem()
               ->topology()->indexSet(0)->size();
  _mu->reserve(sizeMu);

  // If the topology is TimeInvariant ie if M structure does not
  // change during simulation:

  if (topology->indexSet0()->size()>0)
  {
    // Get index set from Simulation
    SP::InteractionsGraph indexSet =
      simulation()->indexSet(indexSetLevel());
    InteractionsGraph::VIterator ui, uiend;
    for (std11::tie(ui, uiend) = indexSet->vertices(); ui != uiend; ++ui)
    {
      _mu->push_back(std11::static_pointer_cast<NewtonImpactFrictionNSL>
                     (indexSet->bundle(*ui)->nonSmoothLaw())->mu());
    }
  }
}
Exemple #7
0
// 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(););
Exemple #8
0
// 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");
}
Exemple #9
0
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::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;
    }

  }
}