C++ DynamicalSystem::workspace方法代码示例

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 SchatzmanPaoliOSI::updateState(const unsigned int level)
{

  double h = simulationLink->timeStep();

  double RelativeTol = simulationLink->relativeConvergenceTol();
  bool useRCC = simulationLink->useRelativeConvergenceCriteron();
  if (useRCC)
    simulationLink->setRelativeConvergenceCriterionHeld(true);

  DSIterator it;
  SP::SiconosMatrix W;
  for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
  {
    SP::DynamicalSystem ds = *it;
    W = WMap[ds->number()];
    // Get the DS type

    Type::Siconos dsType = Type::value(*ds);

    // 1 - Lagrangian Systems
    if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
    {
      // get dynamical system
      SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);

      //    SiconosVector *vfree = d->velocityFree();
      SP::SiconosVector q = d->q();
      bool baux = dsType == Type::LagrangianDS && useRCC && simulationLink->relativeConvergenceCriterionHeld();
      if (level != LEVELMAX)
      {
        // To compute q, we solve W(q - qfree) = p
        if (d->p(level))
        {
          *q = *d->p(level); // q = p
          W->PLUForwardBackwardInPlace(*q);
        }

        // if (d->boundaryConditions())
        //   for (vector<unsigned int>::iterator
        //        itindex = d->boundaryConditions()->velocityIndices()->begin() ;
        //        itindex != d->boundaryConditions()->velocityIndices()->end();
        //        ++itindex)
        //     v->setValue(*itindex, 0.0);
        *q +=  * ds->workspace(DynamicalSystem::free);

      }
      else
        *q =  * ds->workspace(DynamicalSystem::free);



      // Computation of the velocity

      SP::SiconosVector v = d->velocity();
      SP::SiconosVector q_k_1 = d->qMemory()->getSiconosVector(1); // q_{k-1}

      //  std::cout << "SchatzmanPaoliOSI::updateState - q_k_1 =" <<std::endl;
      // q_k_1->display();
      //  std::cout << "SchatzmanPaoliOSI::updateState - q =" <<std::endl;
      // q->display();

      *v = 1.0 / (2.0 * h) * (*q - *q_k_1);
      //  std::cout << "SchatzmanPaoliOSI::updateState - v =" <<std::endl;
      // v->display();

      // int bc=0;
      // SP::SiconosVector columntmp(new SiconosVector(ds->getDim()));

      // if (d->boundaryConditions())
      // {
      //   for (vector<unsigned int>::iterator  itindex = d->boundaryConditions()->velocityIndices()->begin() ;
      //        itindex != d->boundaryConditions()->velocityIndices()->end();
      //        ++itindex)
      //   {
      //     _WBoundaryConditionsMap[ds]->getCol(bc,*columntmp);
      //     /*\warning we assume that W is symmetric in the Lagrangian case*/
      //     double value = - inner_prod(*columntmp, *v);
      //     value += (d->p(level))->getValue(*itindex);
      //     /* \warning the computation of reactionToBoundaryConditions take into
      //        account the contact impulse but not the external and internal forces.
      //        A complete computation of the residue should be better */
      //     d->reactionToBoundaryConditions()->setValue(bc,value) ;
      //     bc++;
      //   }

      if (baux)
      {
        ds->subWorkVector(q, DynamicalSystem::local_buffer);
        double aux = ((ds->workspace(DynamicalSystem::local_buffer))->norm2()) / (ds->normRef());
        if (aux > RelativeTol)
          simulationLink->setRelativeConvergenceCriterionHeld(false);
      }

    }
    //2 - Newton Euler Systems
    else if (dsType == Type::NewtonEulerDS)
    {
      //  // get dynamical system
      //       SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
//.........这里部分代码省略.........

double SchatzmanPaoliOSI::computeResidu()
{

  // This function is used to compute the residu for each "SchatzmanPaoliOSI-discretized" dynamical system.
  // It then computes the norm of each of them and finally return the maximum
  // value for those norms.
  //
  // The state values used are those saved in the DS, ie the last computed ones.
  //  $\mathcal R(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) - h r$
  //  $\mathcal R_{free}(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) $

  double t = simulationLink->nextTime(); // End of the time step
  double told = simulationLink->startingTime(); // Beginning of the time step
  double h = t - told; // time step length

  // Operators computed at told have index i, and (i+1) at t.

  // Iteration through the set of Dynamical Systems.
  //
  DSIterator it;
  SP::DynamicalSystem ds; // Current Dynamical System.
  Type::Siconos dsType ; // Type of the current DS.

  double maxResidu = 0;
  double normResidu = maxResidu;

  for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
  {
    ds = *it; // the considered dynamical system
    dsType = Type::value(*ds); // Its type
    SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu);

    // 1 - Lagrangian Non Linear Systems
    if (dsType == Type::LagrangianDS)
    {
      // // residu = M(q*)(v_k,i+1 - v_i) - h*theta*forces(t,v_k,i+1, q_k,i+1) - h*(1-theta)*forces(ti,vi,qi) - pi+1

      //       // -- Convert the DS into a Lagrangian one.
      //       SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);

      //       // Get state i (previous time step) from Memories -> var. indexed with "Old"
      //       SP::SiconosVector qold =d->qMemory()->getSiconosVector(0);
      //       SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0);

      //       SP::SiconosVector q =d->q();


      //       d->computeMass();
      //       SP::SiconosMatrix M = d->mass();
      //       SP::SiconosVector v = d->velocity(); // v = v_k,i+1
      //       //residuFree->zero();


      //       //    std::cout << "(*v-*vold)->norm2()" << (*v-*vold).norm2() << std::endl;

      //       prod(*M, (*v-*vold), *residuFree); // residuFree = M(v - vold)


      //       if (d->forces())  // if fL exists
      //       {
      //         // computes forces(ti,vi,qi)
      //         d->computeForces(told,qold,vold);
      //         double coef = -h*(1-_theta);
      //         // residuFree += coef * fL_i
      //         scal(coef, *d->forces(), *residuFree, false);

      //         // computes forces(ti+1, v_k,i+1, q_k,i+1) = forces(t,v,q)
      //         //d->computeForces(t);
      //         // or  forces(ti+1, v_k,i+1, q(v_k,i+1))
      //         //or
      //         SP::SiconosVector qbasedonv(new SiconosVector(*qold));
      //         *qbasedonv +=  h*( (1-_theta)* *vold + _theta * *v );
      //         d->computeForces(t,qbasedonv,v);
      //         coef = -h*_theta;
      //         // residuFree += coef * fL_k,i+1
      //         scal(coef, *d->forces(), *residuFree, false);
      //       }

      //       if (d->boundaryConditions())
      //       {

      //         d->boundaryConditions()->computePrescribedVelocity(t);

      //         unsigned int columnindex=0;
      //         SP::SimpleMatrix WBoundaryConditions = _WBoundaryConditionsMap[ds];
      //         SP::SiconosVector columntmp(new SiconosVector(ds->getDim()));

      //         for (vector<unsigned int>::iterator  itindex = d->boundaryConditions()->velocityIndices()->begin() ;
      //              itindex != d->boundaryConditions()->velocityIndices()->end();
      //              ++itindex)
      //         {

      //           double DeltaPrescribedVelocity =
      //             d->boundaryConditions()->prescribedVelocity()->getValue(columnindex)
      //             - vold->getValue(columnindex);

      //           WBoundaryConditions->getCol(columnindex,*columntmp);
      //           *residuFree -= *columntmp * (DeltaPrescribedVelocity);

      //           residuFree->setValue(*itindex, columntmp->getValue(*itindex)   *(DeltaPrescribedVelocity));
//.........这里部分代码省略.........

double SchatzmanPaoliOSI::computeResidu()
{

  // This function is used to compute the residu for each "SchatzmanPaoliOSI-discretized" dynamical system.
  // It then computes the norm of each of them and finally return the maximum
  // value for those norms.
  //
  // The state values used are those saved in the DS, ie the last computed ones.
  //  $\mathcal R(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) - h r$
  //  $\mathcal R_{free}(x,r) = x - x_{k} -h\theta f( x , t_{k+1}) - h(1-\theta)f(x_k,t_k) $

  double t = _simulation->nextTime(); // End of the time step
  double told = _simulation->startingTime(); // Beginning of the time step
  double h = t - told; // time step length

  // Operators computed at told have index i, and (i+1) at t.

  // Iteration through the set of Dynamical Systems.
  //
  SP::DynamicalSystem ds; // Current Dynamical System.
  Type::Siconos dsType ; // Type of the current DS.

  double maxResidu = 0;
  double normResidu = maxResidu;

  DynamicalSystemsGraph::VIterator dsi, dsend;
  for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
  {
    if (!checkOSI(dsi)) continue;
    SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
    dsType = Type::value(*ds); // Its type
    SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu);

    // 1 - Lagrangian Non Linear Systems
    if (dsType == Type::LagrangianDS)
    {
      RuntimeException::selfThrow("SchatzmanPaoliOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
    }
    // 2 - Lagrangian Linear Systems
    else if (dsType == Type::LagrangianLinearTIDS)
    {
      // ResiduFree =  M(-q_{k}+q_{k-1})  + h^2 (K q_k)+  h^2 C (\theta \Frac{q_k-q_{k-1}}{2h}+ (1-\theta) v_k))  (1)
      // This formulae is only valid for the first computation of the residual for q = q_k
      // otherwise the complete formulae must be applied, that is
      // ResiduFree   M(q-2q_{k}+q_{k-1})  + h^2 (K(\theta q+ (1-\theta) q_k)))+  h^2 C (\theta \Frac{q-q_{k-1}}{2h}+ (1-\theta) v_k))  (2)
      // for q != q_k, the formulae (1) is wrong.
      // in the sequel, only the equation (1) is implemented

      // -- Convert the DS into a Lagrangian one.
      SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds);

      // Get state i (previous time step) from Memories -> var. indexed with "Old"
      SP::SiconosVector q_k = d->qMemory()->getSiconosVector(0); // q_k
      SP::SiconosVector q_k_1 = d->qMemory()->getSiconosVector(1); // q_{k-1}
      SP::SiconosVector v_k = d->velocityMemory()->getSiconosVector(0); //v_k
      //  std::cout << "SchatzmanPaoliOSI::computeResidu - q_k_1 =" <<std::endl;
      // q_k_1->display();
      //  std::cout << "SchatzmanPaoliOSI::computeResidu - q_k =" <<std::endl;
      // q_k->display();
      //  std::cout << "SchatzmanPaoliOSI::computeResidu - v_k =" <<std::endl;
      // v_k->display();

      // --- ResiduFree computation Equation (1) ---
      residuFree->zero();
      double coeff;
      // -- No need to update W --

      //SP::SiconosVector v = d->velocity(); // v = v_k,i+1

      SP::SiconosMatrix M = d->mass();
      prod(*M, (*q_k_1 - *q_k), *residuFree); // residuFree = M(-q_{k}+q_{k-1})

      SP::SiconosMatrix K = d->K();
      if (K)
      {
        prod(h * h, *K, *q_k, *residuFree, false); // residuFree += h^2*K*qi
      }

      SP::SiconosMatrix C = d->C();
      if (C)
        prod(h * h, *C, (1.0 / (2.0 * h)*_theta * (*q_k - *q_k_1) + (1.0 - _theta)* *v_k)  , *residuFree, false);
      // residufree += h^2 C (\theta \Frac{q-q_{k-1}}{2h}+ (1-\theta) v_k))


      SP::SiconosVector Fext = d->fExt();
      if (Fext)
      {
        // computes Fext(ti)
        d->computeFExt(told);
        coeff = -h * h * (1 - _theta);
        scal(coeff, *Fext, *residuFree, false); // residufree -= h^2*(1-_theta) * fext(ti)
        // computes Fext(ti+1)
        d->computeFExt(t);
        coeff = -h * h * _theta;
        scal(coeff, *Fext, *residuFree, false); // residufree -= h^2*_theta * fext(ti+1)
      }



      //  std::cout << "SchatzmanPaoliOSI::ComputeResidu LagrangianLinearTIDS residufree :"  << std::endl;
//.........这里部分代码省略.........