Esempio n. 1
0
void EventDriven::initOSIs()
{
  for (OSIIterator itosi = _allOSI->begin();  itosi != _allOSI->end(); ++itosi)
  {
    // Initialize the acceleration like for NewMarkAlphaScheme
    if ((*itosi)->getType() == OSI::NEWMARKALPHAOSI)
    {
      SP::NewMarkAlphaOSI osi_NewMark =  std11::static_pointer_cast<NewMarkAlphaOSI>(*itosi);
      DynamicalSystemsGraph::VIterator dsi, dsend;
      SP::DynamicalSystemsGraph osiDSGraph = (*itosi)->dynamicalSystemsGraph();
      for (std11::tie(dsi, dsend) = osiDSGraph->vertices(); dsi != dsend; ++dsi)
      {
        if (!(*itosi)->checkOSI(dsi)) continue;
        SP::DynamicalSystem ds = osiDSGraph->bundle(*dsi);
        if ((Type::value(*ds) == Type::LagrangianDS) || (Type::value(*ds) == Type::LagrangianLinearTIDS))
        {
          SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS>(ds);
          *(d->workspace(DynamicalSystem::acce_like)) = *(d->acceleration()); // set a0 = ddotq0
          // Allocate the memory to stock coefficients of the polynomial for the dense output
          d->allocateWorkMatrix(LagrangianDS::coeffs_denseoutput, ds->dimension(), (osi_NewMark->getOrderDenseOutput() + 1));
        }
      }
    }
  }
}
Esempio n. 2
0
void SchatzmanPaoliOSI::initializeIterationMatrixW(double t, SP::DynamicalSystem ds)
{
  // This function:
  // - allocate memory for the matrix W
  // - update its content for the current (initial) state of the dynamical system, depending on its type.

  if(!ds)
    RuntimeException::selfThrow("SchatzmanPaoliOSI::initializeIterationMatrixW(t,ds) - ds == NULL");

  if(!(checkOSI(_dynamicalSystemsGraph->descriptor(ds))))
    RuntimeException::selfThrow("SchatzmanPaoliOSI::initializeIterationMatrixW(t,ds) - ds does not belong to the OSI.");

  const DynamicalSystemsGraph::VDescriptor& dsv = _dynamicalSystemsGraph->descriptor(ds);

  if(_dynamicalSystemsGraph->properties(dsv).W)
    RuntimeException::selfThrow("SchatzmanPaoliOSI::initializeIterationMatrixW(t,ds) - W(ds) is already in the map and has been initialized.");

  // Memory allocation for W
  double h = _simulation->timeStep();
  Type::Siconos dsType = Type::value(*ds);
  unsigned int sizeW = ds->dimension();
  // 1 - Lagrangian non linear systems
  if(dsType == Type::LagrangianDS)
  {
    RuntimeException::selfThrow("SchatzmanPaoliOSI::initializeIterationMatrixW - not yet implemented for Dynamical system type :" + dsType);

  }
  // 4 - Lagrangian linear systems
  else if(dsType == Type::LagrangianLinearTIDS)
  {
    SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds);
    if(d->mass())
      {
	_dynamicalSystemsGraph->properties(dsv).W.reset(new SimpleMatrix(*d->mass())); //*W = *d->mass();
      }
    else
      {
	_dynamicalSystemsGraph->properties(dsv).W.reset(new SimpleMatrix(sizeW, sizeW));
	_dynamicalSystemsGraph->properties(dsv).W->eye();
      }

    SP::SiconosMatrix K = d->K();
    SP::SiconosMatrix C = d->C();
    SP::SiconosMatrix W = _dynamicalSystemsGraph->properties(dsv).W;
    if(C)
      scal(1 / 2.0 * h * _theta, *C, *W, false); // W += 1/2.0*h*_theta *C

    if(K)
      scal(h * h * _theta * _theta, *K, *W, false); // W = h*h*_theta*_theta*K

    // WBoundaryConditions initialization
    if(d->boundaryConditions())
      initializeIterationMatrixWBoundaryConditions(d,dsv);
  }

  // === ===
  else if(dsType == Type::NewtonEulerDS)
  {
    RuntimeException::selfThrow("SchatzmanPaoliOSI::initializeIterationMatrixW - not yet implemented for Dynamical system type :" + dsType);
  }
  else RuntimeException::selfThrow("SchatzmanPaoliOSI::initializeIterationMatrixW - not yet implemented for Dynamical system type :" + dsType);

  // Remark: W is not LU-factorized nor inversed here.
  // Function PLUForwardBackward will do that if required.

}
Esempio n. 3
0
void LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)
{
  DEBUG_BEGIN("LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)\n");

  // Computes matrix _interactionBlocks[inter1][inter1] (and allocates memory if
  // necessary) one or two DS are concerned by inter1 .  How
  // _interactionBlocks are computed depends explicitely on the type of
  // Relation of each Interaction.

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

  // Get dimension of the NonSmoothLaw (ie dim of the interactionBlock)
  SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
  SP::Interaction inter = indexSet->bundle(vd);
  // Get osi property from interaction
  // We assume that all ds in vertex_inter have the same osi.
  SP::OneStepIntegrator Osi = indexSet->properties(vd).osi;
  //SP::OneStepIntegrator Osi = simulation()->integratorOfDS(ds);
  OSI::TYPES  osiType = Osi->getType();


  // At most 2 DS are linked by an Interaction
  SP::DynamicalSystem DS1;
  SP::DynamicalSystem DS2;
  unsigned int pos1, pos2;
  // --- Get the dynamical system(s) (edge(s)) connected to the current interaction (vertex) ---
  if (indexSet->properties(vd).source != indexSet->properties(vd).target)
  {
    DEBUG_PRINT("a two DS Interaction\n");
    DS1 = indexSet->properties(vd).source;
    DS2 = indexSet->properties(vd).target;
  }
  else
  {
    DEBUG_PRINT("a single DS Interaction\n");
    DS1 = indexSet->properties(vd).source;
    DS2 = DS1;
    // \warning this looks like some debug code, but it gets executed even with NDEBUG.
    // may be compiler does something smarter, but still it should be rewritten. --xhub
    InteractionsGraph::OEIterator oei, oeiend;
    for (std11::tie(oei, oeiend) = indexSet->out_edges(vd);
         oei != oeiend; ++oei)
    {
      // note : at most 4 edges
      DS2 = indexSet->bundle(*oei);
      if (DS2 != DS1)
      {
        assert(false);
        break;
      }
    }
  }
  assert(DS1);
  assert(DS2);
  pos1 = indexSet->properties(vd).source_pos;
  pos2 = indexSet->properties(vd).target_pos;

  // --- Check block size ---
  assert(indexSet->properties(vd).block->size(0) == inter->nonSmoothLaw()->size());
  assert(indexSet->properties(vd).block->size(1) == inter->nonSmoothLaw()->size());

  // --- Compute diagonal block ---
  // Block to be set in OSNS Matrix, corresponding to
  // the current interaction
  SP::SiconosMatrix currentInteractionBlock = indexSet->properties(vd).block;
  SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock;

  RELATION::TYPES relationType;
  double h = simulation()->currentTimeStep();

  // General form of the interactionBlock is : interactionBlock =
  // a*extraInteractionBlock + b * leftInteractionBlock * centralInteractionBlocks
  // * rightInteractionBlock a and b are scalars, centralInteractionBlocks a
  // matrix depending on the integrator (and on the DS), the
  // simulation type ...  left, right and extra depend on the relation
  // type and the non smooth law.
  relationType = inter->relation()->getType();
  VectorOfSMatrices& workMInter = *indexSet->properties(vd).workMatrices;

  inter->getExtraInteractionBlock(currentInteractionBlock, workMInter);

  unsigned int nslawSize = inter->nonSmoothLaw()->size();
  // loop over the DS connected to the interaction.
  bool endl = false;
  unsigned int pos = pos1;
  for (SP::DynamicalSystem ds = DS1; !endl; ds = DS2)
  {
    assert(ds == DS1 || ds == DS2);
    endl = (ds == DS2);
    unsigned int sizeDS = ds->dimension();
    // get _interactionBlocks corresponding to the current DS
    // These _interactionBlocks depends on the relation type.
    leftInteractionBlock.reset(new SimpleMatrix(nslawSize, sizeDS));
    inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter);
    DEBUG_EXPR(leftInteractionBlock->display(););
    // Computing depends on relation type -> move this in Interaction method?
    if (relationType == FirstOrder)
    {

      rightInteractionBlock.reset(new SimpleMatrix(sizeDS, nslawSize));

      inter->getRightInteractionBlockForDS(pos, rightInteractionBlock, workMInter);

      if (osiType == OSI::EULERMOREAUOSI)
      {
        if ((std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGamma() || (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGammaForRelation())
        {
          *rightInteractionBlock *= (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->gamma();
        }
      }

      // for ZOH, we have a different formula ...
      if (osiType == OSI::ZOHOSI && indexSet->properties(vd).forControl)
      {
        *rightInteractionBlock = std11::static_pointer_cast<ZeroOrderHoldOSI>(Osi)->Bd(ds);
        prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
      }
      else
      {
        // centralInteractionBlock contains a lu-factorized matrix and we solve
        // centralInteractionBlock * X = rightInteractionBlock with PLU
        SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
        centralInteractionBlock->PLUForwardBackwardInPlace(*rightInteractionBlock);
        inter->computeKhat(*rightInteractionBlock, workMInter, h); // if K is non 0

        //      integration of r with theta method removed
        //      *currentInteractionBlock += h *Theta[*itDS]* *leftInteractionBlock * (*rightInteractionBlock); //left = C, right = W.B
        //gemm(h,*leftInteractionBlock,*rightInteractionBlock,1.0,*currentInteractionBlock);
        *leftInteractionBlock *= h;
        prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
        //left = C, right = inv(W).B
      }

    }
    else if (relationType == Lagrangian ||
             relationType == NewtonEuler)
    {

      SP::BoundaryCondition bc;
      Type::Siconos dsType = Type::value(*ds);
      if (dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS)
      {
        SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
        if (d->boundaryConditions()) bc = d->boundaryConditions();
      }
      else if (dsType == Type::NewtonEulerDS)
      {
        SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
        if (d->boundaryConditions()) bc = d->boundaryConditions();
      }
      if (bc)
      {
        for (std::vector<unsigned int>::iterator itindex = bc->velocityIndices()->begin() ;
             itindex != bc->velocityIndices()->end();
             ++itindex)
        {
          // (nslawSize,sizeDS));
          SP::SiconosVector coltmp(new SiconosVector(nslawSize));
          coltmp->zero();
          leftInteractionBlock->setCol(*itindex, *coltmp);
        }
      }
      DEBUG_PRINT("leftInteractionBlock after application of boundary conditions\n");
      DEBUG_EXPR(leftInteractionBlock->display(););
void 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))->dimension();
    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->dimension();
    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);

}