Beispiel #1
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::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::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)
{
  SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());

  SP::DynamicalSystem DS1 = indexSet->properties(vd).source;
  SP::DynamicalSystem DS2 = indexSet->properties(vd).target;
  SP::Interaction inter = indexSet->bundle(vd);
  SP::OneStepIntegrator Osi = indexSet->properties(vd).osi;
  unsigned int pos1, pos2;
  pos1 = indexSet->properties(vd).source_pos;
  pos2 = indexSet->properties(vd).target_pos;

  unsigned int sizeY = 0;
  sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
    (_M)->computeSizeForProjection(inter);


#ifdef MLCPPROJ_DEBUG
  std::cout << "\nMLCPProjectOnConstraints::computeDiagonalInteractionBlock" <<std::endl;
  std::cout << "indexSetLevel()" << indexSetLevel() << std::endl;
  //   std::cout << "indexSet :"<< indexSet << std::endl;
  //   std::cout << "vd :"<< vd << std::endl;
  //  indexSet->display();
  //  std::cout << "DS1 :" << std::endl;
  // DS1->display();
  //  std::cout << "DS2 :" << std::endl;
  // DS2->display();
#endif
  assert(indexSet->blockProj[vd]);
  SP::SiconosMatrix currentInteractionBlock = indexSet->blockProj[vd];

#ifdef MLCPPROJ_DEBUG
  //     std::cout<<"MLCPProjectOnConstraints::computeDiagonalInteractionBlock  "<<std::endl;
  //    currentInteractionBlock->display();
  std::cout << "sizeY " << sizeY  << std::endl;
  std::cout <<  "blockProj " <<  indexSet->blockProj[vd].get() << " of edge " << vd << " of size " << currentInteractionBlock->size(0) << " x " << currentInteractionBlock->size(0) << " for interaction " << inter->number() <<  std::endl;
  // std::cout<<"inter1->display() "<< inter1->number()<< std::endl;
  //inter1->display();
  // std::cout<<"inter2->display() "<< inter2->number()<< std::endl;
  //inter2->display();

#endif

  assert(currentInteractionBlock->size(0) == sizeY);
  assert(currentInteractionBlock->size(1) == sizeY);

  if (!_hasBeenUpdated)
    computeOptions(inter, inter);
  // Computes matrix _interactionBlocks[inter1][inter2] (and allocates memory if
  // necessary) if inter1 and inter2 have commond DynamicalSystem.  How
  // _interactionBlocks are computed depends explicitely on the type of
  // Relation of each Interaction.

  // Warning: we suppose that at this point, all non linear
  // operators (G for lagrangian relation for example) have been
  // computed through plug-in mechanism.

  // Get the W and Theta maps of one of the Interaction -
  // Warning: in the current version, if OSI!=MoreauJeanOSI, this fails.
  // If OSI = MOREAU, centralInteractionBlocks = W if OSI = LSODAR,
  // centralInteractionBlocks = M (mass matrices)
  SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock, leftInteractionBlock1;


  // General form of the interactionBlock is : interactionBlock =
  // a*extraInteractionBlock + b * leftInteractionBlock * centralInteractionBlocks
  // * rightInteractionBlock a and b are scalars, centralInteractionBlocks a
  // matrix depending on the integrator (and on the DS), the
  // simulation type ...  left, right and extra depend on the relation
  // type and the non smooth law.


  VectorOfSMatrices& workMInter = *indexSet->properties(vd).workMatrices;

  currentInteractionBlock->zero();

  // loop over the common DS
  bool endl = false;
  unsigned int pos = pos1;
  for (SP::DynamicalSystem ds = DS1; !endl; ds = DS2)
  {
    assert(ds == DS1 || ds == DS2);
    endl = (ds == DS2);

    if (Type::value(*ds) == Type::LagrangianLinearTIDS ||
        Type::value(*ds) == Type::LagrangianDS)
    {
      if (inter->relation()->getType() != Lagrangian)
      {
        RuntimeException::selfThrow(
          "MLCPProjectOnConstraints::computeDiagonalInteractionBlock - relation is not of type Lagrangian with a LagrangianDS.");
      }


      SP::LagrangianDS lds = (std11::static_pointer_cast<LagrangianDS>(ds));
      unsigned int sizeDS = lds->getDim();
      leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS));
      inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter);

      if (lds->boundaryConditions()) // V.A. Should we do that ?
      {
        for (std::vector<unsigned int>::iterator itindex =
               lds->boundaryConditions()->velocityIndices()->begin() ;
             itindex != lds->boundaryConditions()->velocityIndices()->end();
             ++itindex)
        {
          // (sizeY,sizeDS));
          SP::SiconosVector coltmp(new SiconosVector(sizeY));
          coltmp->zero();
          leftInteractionBlock->setCol(*itindex, *coltmp);
        }
      }
      // (inter1 == inter2)
      SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
      //
      //        std::cout<<"LinearOSNS : leftUBlock\n";
      //        work->display();
      work->trans();
      //        std::cout<<"LinearOSNS::computeInteractionBlock leftInteractionBlock"<<endl;
      //        leftInteractionBlock->display();



      if (_useMassNormalization)
      {
        SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
        centralInteractionBlock->PLUForwardBackwardInPlace(*work);
        prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
        //      gemm(CblasNoTrans,CblasNoTrans,1.0,*leftInteractionBlock,*work,1.0,*currentInteractionBlock);
      }
      else
      {
        prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
      }


      //*currentInteractionBlock *=h;
    }
    else if (Type::value(*ds) == Type::NewtonEulerDS)
    {

      if (inter->relation()->getType() != NewtonEuler)
      {
        RuntimeException::selfThrow("MLCPProjectOnConstraints::computeDiagonalInteractionBlock - relation is not from NewtonEulerR.");
      }
      SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds));
#ifdef MLCPPROJ_WITH_CT
      unsigned int sizeDS = neds->getDim();
      SP::SimpleMatrix T = neds->T();
      SP::SimpleMatrix workT(new SimpleMatrix(*T));
      workT->trans();
      SP::SimpleMatrix workT2(new SimpleMatrix(6, 6));
      prod(*workT, *T, *workT2, true);
      leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS));
      inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock);
      SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
      std::cout << "LinearOSNS : leftUBlock\n";
      work->display();
      work->trans();
      std::cout << "LinearOSNS::computeInteractionBlock workT2" <<std::endl;
      workT2->display();
      workT2->PLUForwardBackwardInPlace(*work);
      prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
#else
      if (0) //(std11::static_pointer_cast<NewtonEulerR> inter->relation())->_isConstact){
      {
//        unsigned int sizeDS = neds->getDim();
//        SP::SimpleMatrix T = neds->T();
//        SP::SimpleMatrix workT(new SimpleMatrix(*T));
//        workT->trans();
//        SP::SimpleMatrix workT2(new SimpleMatrix(6, 6));
//        prod(*workT, *T, *workT2, true);
//        leftInteractionBlock1.reset(new SimpleMatrix(sizeY, sizeDS));
//        inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock);
//        leftInteractionBlock.reset(new SimpleMatrix(1, sizeDS));
//        for (unsigned int ii = 0; ii < sizeDS; ii++)
//          leftInteractionBlock->setValue(1, ii, leftInteractionBlock1->getValue(1, ii));
//
//        SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
//        //cout<<"LinearOSNS : leftUBlock\n";
//        //work->display();
//        work->trans();
//        //cout<<"LinearOSNS::computeInteractionBlock workT2"<<endl;
//        //workT2->display();
//        workT2->PLUForwardBackwardInPlace(*work);
//        prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
      }
      else
      {
        unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getqDim();
        leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS));
        inter->getLeftInteractionBlockForDSProjectOnConstraints(pos, leftInteractionBlock);
        // #ifdef MLCPPROJ_DEBUG
        //          std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock - NewtonEuler case leftInteractionBlock : " << std::endl;
        //         leftInteractionBlock->display();
        // #endif

        SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
        //cout<<"LinearOSNS sizeY="<<sizeY<<": leftUBlock\n";
        //work->display();
        work->trans();
        prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
        // #ifdef MLCPPROJ_DEBUG
        //          std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock - NewtonEuler case currentInteractionBlock : "<< std::endl;
        //         currentInteractionBlock->display();
        // #endif


      }
      
    }
    else
      RuntimeException::selfThrow("MLCPProjectOnConstraints::computeDiagonalInteractionBlock - ds is not from NewtonEulerDS neither a LagrangianDS.");



#endif
#ifdef MLCPPROJ_DEBUG
    std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock DiaginteractionBlock " << std::endl;
    currentInteractionBlock->display();
#endif
    // Set pos for next loop. 
    pos = pos2;
     
  }
  
}