C++ Interaction::y方法代码示例

SP::SiconosVector Simulation::y(unsigned int level, unsigned int coor)
{
  // return output(level) (ie with y[level]) for all Interactions.
  // assert(level>=0);

  DEBUG_BEGIN("Simulation::output(unsigned int level, unsigned int coor)\n");
  DEBUG_PRINTF("with level = %i and coor = %i \n", level,coor);

  InteractionsGraph::VIterator ui, uiend;
  SP::Interaction inter;
  SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0();

  SP::SiconosVector y (new SiconosVector (_nsds->topology()->indexSet0()->size() ));
  int i=0;
  for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
  {
    inter = indexSet0->bundle(*ui);
    assert(inter->lowerLevelForOutput() <= level);
    assert(inter->upperLevelForOutput() >= level);
    y->setValue(i,inter->y(level)->getValue(coor));
    i++;
  }
  DEBUG_END("Simulation::output(unsigned int level, unsigned int coor)\n");
  return y;
}

void MLCPProjectOnConstraints::computeqBlock(InteractionsGraph::VDescriptor& vertex_inter, unsigned int pos)
{
  SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
  SP::Interaction inter = indexSet->bundle(vertex_inter);
  unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
    (_M)->computeSizeForProjection(inter);
  for (unsigned int i = 0; i < sizeY; i++)
    _q->setValue(pos + i, inter->y(0)->getValue(0 + i));
#ifdef MLCPPROJ_DEBUG
  printf("MLCPProjectOnConstraints::computeqBlock, _q from y(0)\n");
  _q->display();
#endif
}

void MLCPProjectOnConstraints::postComputeNewtonEulerR(SP::Interaction inter, unsigned int pos)
{
  SP::NewtonEulerR ner = (std11::static_pointer_cast<NewtonEulerR>(inter->relation()));
  SP::SiconosVector lambda = inter->lambda(0);
  SP::SiconosVector y = inter->y(0);
  unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
    (_M)->computeSizeForProjection(inter);
  // Copy _w/_z values, starting from index pos into y/lambda.

  //setBlock(*_w, y, sizeY, pos, 0);
  setBlock(*_z, lambda, sizeY, pos, 0);

}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
double EventDriven::detectEvents(bool updateIstate)
{
  double _minResiduOutput = 0.0; // maximum of g_i with i running over all activated or deactivated contacts
  // Loop over all interactions to detect whether some constraints are activated or deactivated
  bool _IsContactClosed = false;
  bool _IsContactOpened = false;
  bool _IsFirstTime = true;
  InteractionsGraph::VIterator ui, uiend;
  SP::SiconosVector y, ydot, lambda;
  SP::Topology topo = _nsds->topology();
  SP::InteractionsGraph indexSet2 = topo->indexSet(2);
  //
#ifdef DEBUG_MESSAGES
  cout << "======== In EventDriven::detectEvents =========" <<endl;
#endif
  for (std11::tie(ui, uiend) = _indexSet0->vertices(); ui != uiend; ++ui)
  {
    SP::Interaction inter = _indexSet0->bundle(*ui);
    double nsLawSize = inter->nonSmoothLaw()->size();
    if (nsLawSize != 1)
    {
      RuntimeException::selfThrow("In EventDriven::detectEvents, the interaction size > 1 has not been implemented yet!!!");
    }
    y = inter->y(0);   // output y at this Interaction
    ydot = inter->y(1); // output of level 1 at this Interaction
    lambda = inter->lambda(2); // input of level 2 at this Interaction
    if (!(indexSet2->is_vertex(inter))) // if Interaction is not in the indexSet[2]
    {
      if ((*y)(0) < _TOL_ED) // gap at the current interaction <= 0
      {
        _IsContactClosed = true;
      }

      if (_IsFirstTime)
      {
        _minResiduOutput = (*y)(0);
        _IsFirstTime = false;
      }
      else
      {
        if (_minResiduOutput > (*y)(0))
        {
          _minResiduOutput = (*y)(0);
        }
      }
    }
    else // If interaction is in the indexSet[2]
    {
      if ((*lambda)(0) < _TOL_ED) // normal force at the current interaction <= 0
      {
        _IsContactOpened = true;
      }

      if (_IsFirstTime)
      {
        _minResiduOutput = (*lambda)(0);
        _IsFirstTime = false;
      }
      else
      {
        if (_minResiduOutput > (*lambda)(0))
        {
          _minResiduOutput = (*lambda)(0);
        }
      }
    }
    //
#ifdef DEBUG_MESSAGES
    cout.precision(15);
    cout << "Contact number: " << inter->number() <<endl;
    cout << "Contact gap: " << (*y)(0) <<endl;
    cout << "Contact force: " << (*lambda)(0) <<endl;
    cout << "Is contact is closed: " << _IsContactClosed <<endl;
    cout << "Is contact is opened: " << _IsContactOpened <<endl;
#endif
    //
  }
  //
  if (updateIstate)
  {
    if ((!_IsContactClosed) && (!_IsContactOpened))
    {
      _istate = 2; //no event is detected
    }
    else if ((_IsContactClosed) && (!_IsContactOpened))
    {
      _istate = 3; // Only some contacts are closed
    }
    else if ((!_IsContactClosed) && (_IsContactOpened))
    {
      _istate = 4; // Only some contacts are opened
    }
    else
    {
      _istate = 5; // Some contacts are closed AND some contacts are opened
    }
  }
  //
  return  _minResiduOutput;
//.........这里部分代码省略.........

void EventDriven::computeg(SP::OneStepIntegrator osi,
                           integer * sizeOfX, doublereal* time,
                           doublereal* x, integer * ng,
                           doublereal * gOut)
{
  assert(_nsds);
  assert(_nsds->topology());
  InteractionsGraph::VIterator ui, uiend;
  SP::Topology topo = _nsds->topology();
  SP::InteractionsGraph indexSet2 = topo->indexSet(2);
  unsigned int nsLawSize, k = 0 ;
  SP::SiconosVector y, ydot, yddot, lambda;
  SP::LsodarOSI lsodar = std11::static_pointer_cast<LsodarOSI>(osi);
  // Solve LCP at acceleration level to calculate the lambda[2] at Interaction of indexSet[2]
  lsodar->fillXWork(sizeOfX, x);
  //
  double t = *time;
  if (!_allNSProblems->empty())
  {
    if (((*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->hasInteractions()))
    {
      (*_allNSProblems)[SICONOS_OSNSP_ED_SMOOTH_ACC]->compute(t);
    }
  };
  /*
     double * xdottmp = (double *)malloc(*sizeOfX*sizeof(double));
     computef(osi, sizeOfX,time,x,xdottmp);
     free(xdottmp);
     */
  // Update the output from level 0 to level 1
  _nsds->updateOutput(t,0);
  _nsds->updateOutput(t,1);
  _nsds->updateOutput(t,2);
  //
  for (std11::tie(ui, uiend) = _indexSet0->vertices(); ui != uiend; ++ui)
  {
    SP::Interaction inter = _indexSet0->bundle(*ui);
    nsLawSize = inter->nonSmoothLaw()->size();
    y = inter->y(0);   // output y at this Interaction
    ydot = inter->y(1); // output of level 1 at this Interaction
    yddot = inter->y(2);
    lambda = inter->lambda(2); // input of level 2 at this Interaction
    if (!(indexSet2->is_vertex(inter))) // if Interaction is not in the indexSet[2]
    {
      for (unsigned int i = 0; i < nsLawSize; ++i)
      {
        if ((*y)(i) > _TOL_ED)
        {
          gOut[k] = (*y)(i);
        }
        else
        {
          if ((*ydot)(i) > -_TOL_ED)
          {
            gOut[k] = 100 * _TOL_ED;
          }
          else
          {
            gOut[k] = (*y)(i);
          }
        }
        k++;
      }
    }
    else // If Interaction is in the indexSet[2]
    {
      for (unsigned int i = 0; i < nsLawSize; ++i)
      {
        if ((*lambda)(i) > _TOL_ED)
        {
          gOut[k] = (*lambda)(i); // g = lambda[2]
        }
        else
        {
          if ((*yddot)(i) > _TOL_ED)
          {
            gOut[k] = (*lambda)(i);
          }
          else
          {
            gOut[k] = 100 * _TOL_ED;
          }
        }
        k++;
      }
    }

  }
}

int main(int argc, char* argv[])
{
  try
  {

    // ================= Creation of the model =======================

    // User-defined main parameters
    unsigned int nDof = 3;           // degrees of freedom for the ball
    double t0 = 0;                   // initial computation time
    double T = 2.0;                  // final computation time
    double h = 0.0005;                // time step
    double position_init = 1.0;      // initial position for lowest bead.
    double velocity_init = 0.0;      // initial velocity for lowest bead.
    double theta = 0.5;              // theta for MoreauJeanOSI integrator
    double R = 0.1; // Ball radius
    double m = 1; // Ball mass
    double g = 9.81; // Gravity
    // -------------------------
    // --- Dynamical systems ---
    // -------------------------

    cout << "====> Model loading ..." << endl << endl;

    // Number of Beads
    unsigned int nBeads = 10;
    double initialGap = 0.25;
    double alert = 0.02;

    SP::SiconosMatrix Mass(new SimpleMatrix(nDof, nDof));
    (*Mass)(0, 0) = m;
    (*Mass)(1, 1) = m;
    (*Mass)(2, 2) = 3. / 5 * m * R * R;

    // -- Initial positions and velocities --
    std::vector<SP::SiconosVector> q0(nBeads);
    std::vector<SP::SiconosVector> v0(nBeads);

    for (unsigned int i = 0; i < nBeads; i++)
    {
      (q0[i]).reset(new SiconosVector(nDof));
      (v0[i]).reset(new SiconosVector(nDof));
      (q0[i])->setValue(0, position_init + i * initialGap);
      (v0[i])->setValue(0, velocity_init);
    }

    // -- The dynamical system --
    SP::SiconosVector weight(new SiconosVector(nDof));
    (*weight)(0) = -m * g;


    std::vector<SP::LagrangianLinearTIDS> beads(nBeads);
    for (unsigned int i = 0; i < nBeads; i++)
    {
      beads[i].reset(new LagrangianLinearTIDS(q0[i], v0[i], Mass));
      // -- Set external forces (weight) --
      beads[i]->setFExtPtr(weight);
    }


    // --------------------
    // --- Interactions ---
    // --------------------

    // -- nslaw --
    double e = 0.9;

    // Interaction ball-floor
    //
    SP::SimpleMatrix H(new SimpleMatrix(1, nDof));
    (*H)(0, 0) = 1.0;
    SP::SiconosVector b(new SiconosVector(1));
    (*b)(0) = -R;

    SP::NonSmoothLaw nslaw(new NewtonImpactNSL(e));
    SP::Relation relation(new LagrangianLinearTIR(H, b));

    SP::Interaction inter;


    // beads/beads interactions
    SP::SimpleMatrix HOfBeads(new SimpleMatrix(1, 2 * nDof));
    (*HOfBeads)(0, 0) = -1.0;
    (*HOfBeads)(0, 3) = 1.0;
    SP::SiconosVector bOfBeads(new SiconosVector(1));
    (*bOfBeads)(0) = -2 * R;

    // This doesn't work !!!

    //SP::Relation relationOfBeads(new LagrangianLinearTIR(HOfBeads));
    //std::vector<SP::Interaction > interOfBeads(nBeads-1);
    // for (unsigned int i =0; i< nBeads-1; i++)
    // {
    //   interOfBeads[i].reset(new Interaction(1, nslaw, relationOfBeads));
    // }

    // This works !!
    std::vector<SP::Relation > relationOfBeads(nBeads - 1);
    std::vector<SP::Interaction > interOfBeads(nBeads - 1);
    // for (unsigned int i =0; i< nBeads-1; i++)
//.........这里部分代码省略.........

void MLCPProjectOnConstraints::postComputeLagrangianR(SP::Interaction inter, unsigned int pos)
{
  SP::LagrangianR  lr = std11::static_pointer_cast<LagrangianR>(inter->relation());
#ifdef MLCPPROJ_DEBUG
  printf("MLCPProjectOnConstraints::postComputeLagrangian inter->y(0)\n");
  inter->y(0)->display();
  printf("MLCPProjectOnConstraints::postComputeLagrangian lr->jachq \n");
  lr->jachq()->display();
  printf("MLCPProjectOnConstraints::postComputeLagrangianR q before update\n");

  
  SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
  InteractionsGraph::VDescriptor ui = indexSet->descriptor(inter);
  InteractionsGraph::OEIterator oei, oeiend;
    for(std11::tie(oei, oeiend) = indexSet->out_edges(ui);
        oei != oeiend; ++oei)
    {
      
      SP::LagrangianDS lds =  std11::static_pointer_cast<LagrangianDS>(indexSet->bundle(*oei));
      lds->q()->display();
  }
#endif



  //unsigned int sizeY = inter->nonSmoothLaw()->size();

  // y and lambda vectors
  SP::SiconosVector lambda = inter->lambda(0);
  SP::SiconosVector y = inter->y(0);
  unsigned int sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
    (_M)->computeSizeForProjection(inter);
  // Copy _w/_z values, starting from index pos into y/lambda.

  //setBlock(*_w, y, sizeY, pos, 0);
  setBlock(*_z, lambda, sizeY, pos, 0);

#ifdef MLCPPROJ_DEBUG
  printf("MLCPP lambda of Interaction is pos =%i :\n", pos);
  //  aBuff->display();
  lambda->display();
  unsigned int nslawsize = inter->nonSmoothLaw()->size();
  SP::SiconosVector aBuff(new SiconosVector(nslawsize));
  setBlock(*_z, aBuff, sizeY, pos, 0);
  SP::SiconosMatrix J = lr->jachq();
  SP::SimpleMatrix aux(new SimpleMatrix(*J));
  aux->trans();
  // SP::SiconosVector tmp(new SiconosVector(*(lr->q())));
  // prod(*aux, *aBuff, *(tmp), false);
  // //prod(*aux,*lambda,*(lr->q()),false);
  // std:: std::cout << " tmp =  tmp + J^T * lambda" << std::endl;
  // tmp->display();
#endif



  // // WARNING : Must not be done here. and should be called with the correct time.
  // // compute p(0)
  // inter->computeInput(0.0 ,0);

  // // \warning aBuff should normally be in lambda[0]
  // // The update of the position in DS should be made
  // //  in MoreauJeanOSI::upateState or ProjectedMoreauJeanOSI::updateState
  // SP::SiconosMatrix J=lr->jachq();
  // SP::SimpleMatrix aux(new SimpleMatrix(*J));
  // aux->trans();

  // SP::SiconosVector tmp (new SiconosVector(*(lr->q())));
  // std:: std::cout << " tmp ="<<std::endl;
  // tmp->display();
  // std:: std::cout << " lr->q() ="<<std::endl;
  // lr->q()->display();

  // //prod(*aux,*lambda,*(lr->q()),false);
  // prod(*aux,*aBuff,*(tmp),false);
  // std:: std::cout << " tmp =  tmp + J * lambda"<<std::endl;
  // tmp->display();


  // // The following step should be done on MoreauJeanOSI::upateState or ProjectedMoreauJeanOSI::updateState
  // DSIterator itDS = inter->dynamicalSystemsBegin();
  // while(itDS!=inter->dynamicalSystemsEnd())
  // {
  //   Type::Siconos dsType = Type::value(**itDS);
  //   if((dsType !=Type::LagrangianDS) and
  //      (dsType !=Type::LagrangianLinearTIDS) )
  //   {
  //     RuntimeException::selfThrow("MLCPProjectOnConstraint::postCompute- ds is not of Lagrangian DS type.");
  //   }

  //   SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (*itDS);
  //   SP::SiconosVector q = d->q();

  //   *q +=  *d->p(0);
  //    std::cout << " q=" << std::endl;
  //   q->display();
  //   itDS++;
  // }

  // if ((*lr->q() - *tmp).normInf() > 1e-12)
//.........这里部分代码省略.........