C++ LagrangianDS::dimension方法代码示例

void MoreauJeanDirectProjectionOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds)
{
  DEBUG_BEGIN("MoreauJeanDirectProjectionOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds) \n");
  MoreauJeanOSI::initializeWorkVectorsForDS(t, ds);
  const DynamicalSystemsGraph::VDescriptor& dsv = _dynamicalSystemsGraph->descriptor(ds);
  VectorOfVectors& workVectors = *_dynamicalSystemsGraph->properties(dsv).workVectors;
  Type::Siconos dsType = Type::value(*ds);
  if(dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
  {
    SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
    workVectors[MoreauJeanOSI::QTMP].reset(new SiconosVector(d->dimension()));
  }
  else if(dsType == Type::NewtonEulerDS)
  {
    SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS>(ds);
    workVectors[MoreauJeanOSI::QTMP].reset(new SiconosVector(d->getqDim()));
  }
  else
  {
    RuntimeException::selfThrow("MoreauJeanDirectProjectionOSI::initialize() - DS not of the right type");
  }
  for (unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++)
  {
    DEBUG_PRINTF("ds->initializeNonSmoothInput(%i)\n", k);
    ds->initializeNonSmoothInput(k);
    DEBUG_EXPR_WE(
      SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
      if (d->p(k))
        std::cout << "d->p(" << k <<" ) exists" << std::endl;
      );

  }

void D1MinusLinearOSI::initializeWorkVectorsForDS(double t, SP::DynamicalSystem ds)
{
  // Get work buffers from the graph
  VectorOfVectors& ds_work_vectors = *_initializeDSWorkVectors(ds);

  // Check dynamical system type
  Type::Siconos dsType = Type::value(*ds);
  assert(dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS || dsType == Type::NewtonEulerDS);
  
  if(dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
  {
    SP::LagrangianDS lds = std11::static_pointer_cast<LagrangianDS> (ds);
    lds->init_generalized_coordinates(2); // acceleration is required for the ds
    lds->init_inverse_mass(); // invMass required to update post-impact velocity

    ds_work_vectors.resize(D1MinusLinearOSI::WORK_LENGTH);
    ds_work_vectors[D1MinusLinearOSI::RESIDU_FREE].reset(new SiconosVector(lds->dimension()));
    ds_work_vectors[D1MinusLinearOSI::FREE].reset(new SiconosVector(lds->dimension()));
    ds_work_vectors[D1MinusLinearOSI::FREE_TDG].reset(new SiconosVector(lds->dimension()));
    // Update dynamical system components (for memory swap).
    lds->computeForces(t, lds->q(), lds->velocity());
    lds->swapInMemory();
  }
  else if(dsType == Type::NewtonEulerDS)
  {
    SP::NewtonEulerDS neds = std11::static_pointer_cast<NewtonEulerDS> (ds);
    neds->init_inverse_mass(); // invMass required to update post-impact velocity
    ds_work_vectors.resize(D1MinusLinearOSI::WORK_LENGTH);
    ds_work_vectors[D1MinusLinearOSI::RESIDU_FREE].reset(new SiconosVector(neds->dimension()));
    ds_work_vectors[D1MinusLinearOSI::FREE].reset(new SiconosVector(neds->dimension()));
    ds_work_vectors[D1MinusLinearOSI::FREE_TDG].reset(new SiconosVector(neds->dimension()));
    //Compute a first value of the forces to store it in _forcesMemory
    neds->computeForces(t, neds->q(), neds->twist());
    neds->swapInMemory();
  }
  else
    RuntimeException::selfThrow("D1MinusLinearOSI::initialize - not implemented for Dynamical system type: " + dsType);

  for (unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++)
  {
    ds->initializeNonSmoothInput(k);
  }

}

double D1MinusLinearOSI::computeResiduHalfExplicitAccelerationLevel()
{
  DEBUG_BEGIN("\n D1MinusLinearOSI::computeResiduHalfExplicitAccelerationLevel()\n");

  double t = _simulation->nextTime(); // end of the time step
  double told = _simulation->startingTime(); // beginning of the time step
  double h = _simulation->timeStep(); // time step length

  SP::OneStepNSProblems allOSNS  = _simulation->oneStepNSProblems(); // all OSNSP
  SP::Topology topo =  _simulation->nonSmoothDynamicalSystem()->topology();
  SP::InteractionsGraph indexSet2 = topo->indexSet(2);

  /**************************************************************************************************************
   *  Step 1-  solve a LCP at acceleration level for lambda^+_{k} for the last set indices
   *   if index2 is empty we should skip this step
   **************************************************************************************************************/

  DEBUG_PRINT("\nEVALUATE LEFT HAND SIDE\n");

  DEBUG_EXPR(std::cout<< "allOSNS->empty()   " << std::boolalpha << allOSNS->empty() << std::endl << std::endl);
  DEBUG_EXPR(std::cout<< "allOSNS->size()   "  << allOSNS->size() << std::endl << std::endl);

// -- LEFT SIDE --
  DynamicalSystemsGraph::VIterator dsi, dsend;
  for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
  {
    if (!checkOSI(dsi)) continue;
    SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);

    Type::Siconos dsType = Type::value(*ds);
    SP::SiconosVector accFree;
    SP::SiconosVector work_tdg;
    SP::SiconosMatrix Mold;
    DEBUG_EXPR((*it)->display());

    if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
    {
      SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
      accFree = d->workspace(DynamicalSystem::free); /* POINTER CONSTRUCTOR : will contain
                                                       * the acceleration without contact force */
      accFree->zero();

      // get left state from memory
      SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
      SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
      Mold = d->mass();

      DEBUG_EXPR(accFree->display());
      DEBUG_EXPR(qold->display());
      DEBUG_EXPR(vold->display());
      DEBUG_EXPR(Mold->display());

      if (! d->workspace(DynamicalSystem::free_tdg))
      {
        d->allocateWorkVector(DynamicalSystem::free_tdg, d->dimension()) ;
      }
      work_tdg = d->workspace(DynamicalSystem::free_tdg);
      work_tdg->zero();
      DEBUG_EXPR(work_tdg->display());

      if (d->forces())
      {
        d->computeForces(told, qold, vold);
        DEBUG_EXPR(d->forces()->display());

        *accFree += *(d->forces());
      }
      Mold->PLUForwardBackwardInPlace(*accFree); // contains left (right limit) acceleration without contact force
      d->addWorkVector(accFree,DynamicalSystem::free_tdg); // store the value in WorkFreeFree
    }
    else if(dsType == Type::NewtonEulerDS)
    {
      SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
      accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
      accFree->zero();

      // get left state from memory
      SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
      SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
      //Mold = d->mass();
      assert(!d->mass()->isPLUInversed());
      Mold.reset(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization
      DEBUG_EXPR(accFree->display());
      DEBUG_EXPR(qold->display());
      DEBUG_EXPR(vold->display());
      DEBUG_EXPR(Mold->display());

      if (! d->workspace(DynamicalSystem::free_tdg))
      {
        d->allocateWorkVector(DynamicalSystem::free_tdg, d->dimension()) ;
      }

      work_tdg = d->workspace(DynamicalSystem::free_tdg);
      work_tdg->zero();
      DEBUG_EXPR(work_tdg->display());

      if (d->forces())
      {
        d->computeForces(told, qold, vold);
        DEBUG_EXPR(d->forces()->display());
//.........这里部分代码省略.........

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->dimension();
      leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS));
      inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter);

      if (lds->boundaryConditions()) // V.A. Should we do that ?
//.........这里部分代码省略.........