Esempio n. 1
0
const SimpleMatrix SchatzmanPaoliOSI::getWBoundaryConditions(SP::DynamicalSystem ds)
{
  assert(ds &&
         "SchatzmanPaoliOSI::getWBoundaryConditions(ds): ds == NULL.");
  //    return *(WBoundaryConditionsMap[0]);
  assert(_WBoundaryConditionsMap[ds->number()] &&
         "SchatzmanPaoliOSI::getWBoundaryConditions(ds): WBoundaryConditions[ds] == NULL.");
  return *(_WBoundaryConditionsMap[ds->number()]); // Copy !!
}
Esempio n. 2
0
void EulerMoreauOSI::setW(const SiconosMatrix& newValue, SP::DynamicalSystem ds)
{
  // Check if ds is in the OSI
  if (!OSIDynamicalSystems->isIn(ds))
    RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - ds does not belong to this Integrator ...");

  // Check dimensions consistency
  unsigned int line = newValue.size(0);
  unsigned int col  = newValue.size(1);

  if (line != col) // Check that newValue is square
    RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - newVal is not square! ");

  if (!ds)
    RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - ds == NULL.");

  unsigned int sizeW = ds->getDim(); // n for first order systems, ndof for lagrangian.
  unsigned int dsN = ds->number();
  if (line != sizeW) // check consistency between newValue and dynamical system size
    RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - unconsistent dimension between newVal and dynamical system to be integrated ");

  // Memory allocation for W, if required
  if (!WMap[dsN]) // allocate a new W if required
  {
    WMap[dsN].reset(new SimpleMatrix(newValue));
  }
  else  // or fill-in an existing one if dimensions are consistent.
  {
    if (line == WMap[dsN]->size(0) && col == WMap[dsN]->size(1))
      *(WMap[dsN]) = newValue;
    else
      RuntimeException::selfThrow("EulerMoreauOSI - setW: inconsistent dimensions with problem size for given input matrix W");
  }
}
Esempio n. 3
0
SP::SimpleMatrix EulerMoreauOSI::W(SP::DynamicalSystem ds)
{
  assert(ds && "EulerMoreauOSI::W(ds): ds == NULL.");
  //  return WMap[0];
  //  if(WMap[ds]==NULL)
  //    RuntimeException::selfThrow("EulerMoreauOSI::W(ds): W[ds] == NULL.");
  return WMap[ds->number()];
}
Esempio n. 4
0
SP::SiconosMatrix EulerMoreauOSI::WBoundaryConditions(SP::DynamicalSystem ds)
{
  assert(ds && "EulerMoreauOSI::WBoundaryConditions(ds): ds == NULL.");
  //  return WBoundaryConditionsMap[0];
  //  if(WBoundaryConditionsMap[ds]==NULL)
  //    RuntimeException::selfThrow("EulerMoreauOSI::WBoundaryConditions(ds): W[ds] == NULL.");
  return _WBoundaryConditionsMap[ds->number()];
}
Esempio n. 5
0
const SimpleMatrix EulerMoreauOSI::getW(SP::DynamicalSystem ds)
{
  int dsN = ds->number();
  assert(ds &&
         "EulerMoreauOSI::getW(ds): ds == NULL.");
  //    return *(WMap[0]);
  assert(WMap[dsN] &&
         "EulerMoreauOSI::getW(ds): W[ds] == NULL.");
  return *(WMap[dsN]); // Copy !!
}
Esempio n. 6
0
const SimpleMatrix EulerMoreauOSI::getWBoundaryConditions(SP::DynamicalSystem ds)
{
  assert(ds &&
         "EulerMoreauOSI::getWBoundaryConditions(ds): ds == NULL.");
  //    return *(WBoundaryConditionsMap[0]);
  unsigned int dsN = ds->number();
  assert(_WBoundaryConditionsMap[dsN] &&
         "EulerMoreauOSI::getWBoundaryConditions(ds): WBoundaryConditions[ds] == NULL.");
  return *(_WBoundaryConditionsMap[dsN]); // Copy !!
}
Esempio n. 7
0
const SimpleMatrix SchatzmanPaoliOSI::getW(SP::DynamicalSystem ds)
{
  assert(ds &&
         "SchatzmanPaoliOSI::getW(ds): ds == NULL.");
  //    return *(WMap[0]);
  unsigned int dsN = ds->number();
  assert(WMap[dsN] &&
         "SchatzmanPaoliOSI::getW(ds): W[ds] == NULL.");
  return *(WMap[dsN]); // Copy !!
}
Esempio n. 8
0
void EulerMoreauOSI::setWPtr(SP::SimpleMatrix newPtr, SP::DynamicalSystem ds)
{
  unsigned int line = newPtr->size(0);
  unsigned int col  = newPtr->size(1);
  if (line != col) // Check that newPtr is square
    RuntimeException::selfThrow("EulerMoreauOSI::setWPtr(newVal) - newVal is not square! ");

  if (!ds)
    RuntimeException::selfThrow("EulerMoreauOSI::setWPtr(newVal,ds) - ds == NULL.");

  unsigned int sizeW = ds->getDim(); // n for first order systems, ndof for lagrangian.
  if (line != sizeW) // check consistency between newValue and dynamical system size
    RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal) - unconsistent dimension between newVal and dynamical system to be integrated ");

  WMap[ds->number()] = newPtr;                  // link with new pointer
}
Esempio n. 9
0
SP::DynamicalSystem Topology::getDynamicalSystem(unsigned int requiredNumber)
{
  DynamicalSystemsGraph::VIterator vi, vdend;
  SP::DynamicalSystem ds;
  unsigned int currentNumber;
  for (std11::tie(vi, vdend) = _DSG[0]->vertices(); vi != vdend; ++vi)
  {
    ds = _DSG[0]->bundle(*vi);
    currentNumber = ds->number();
    if (currentNumber == requiredNumber)
      return ds;
  }

  RuntimeException::selfThrow("Topology::getDynamicalSystem(n) ds not found.");

  return ds;
}
Esempio n. 10
0
void SchatzmanPaoliOSI::display()
{
  OneStepIntegrator::display();

  std::cout << "====== SchatzmanPaoliOSI OSI display ======" <<std::endl;

  DynamicalSystemsGraph::VIterator dsi, dsend;
  for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
  {
    if (!checkOSI(dsi)) continue;
    SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
    std::cout << "--------------------------------" <<std::endl;
    std::cout << "--> W of dynamical system number " << ds->number() << ": " <<std::endl;
    if (_dynamicalSystemsGraph->properties(*dsi).W)  _dynamicalSystemsGraph->properties(*dsi).W->display();
    else std::cout << "-> NULL" <<std::endl;
    std::cout << "--> and corresponding theta is: " << _theta <<std::endl;
  }
  std::cout << "================================" <<std::endl;
}
Esempio n. 11
0
void EulerMoreauOSI::computeFreeState()
{
  // This function computes "free" states of the DS belonging to this Integrator.
  // "Free" means without taking non-smooth effects into account.
  DEBUG_PRINT("EulerMoreauOSI::computeFreeState() starts\n");

  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.

  //  Note: integration of r with a theta method has been removed
  //  SiconosVector *rold = static_cast<SiconosVector*>(d.rMemory()->getSiconosVector(0));

  // Iteration through the set of Dynamical Systems.
  //
  DSIterator it; // Iterator through the set of DS.

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

  // XXX to be removed -- xhub
  Topology& topo = *simulationLink->model()->nonSmoothDynamicalSystem()->topology();
  DynamicalSystemsGraph& DSG0 = *topo.dSG(0);


  for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
  {
    ds = *it; // the considered dynamical system

    // XXX TMP hack -- xhub
    // we have to iterate over the edges of the DSG0 -> the following won't be necessary anymore
    // Maurice will do that with subgraph :)
    DynamicalSystemsGraph::VDescriptor dsgVD = DSG0.descriptor(ds);
    VectorOfVectors& workVectors = *DSG0.properties(dsgVD).workVectors;


    dsType = Type::value(*ds); // Its type
    SiconosMatrix& W = *WMap[ds->number()]; // Its W EulerMoreauOSI matrix of iteration.

    // 1 - First Order Non Linear Systems
    if (dsType == Type::FirstOrderNonLinearDS || dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS)
    {
      // xfree =  x - W^{-1} (ResiduFree - h(1-gamma)*rold)
      // with ResiduFree = = M(x - x_k) - h*theta*f(t_{k+1}, x) - h*(1-theta)*f(t_k, x_k)

      // to be updated at current time: W, f
      // fold is f at t_k
      // not time dependant: M
      FirstOrderNonLinearDS& d = *std11::static_pointer_cast<FirstOrderNonLinearDS>(ds);

      // Get state i (previous time step) from Memories -> var. indexed with "Old"
      //    SP::SiconosVector xold = d->xMemory()->getSiconosVector(0); // xi

      SiconosVector& x = *d.x(); // x = x_k or x = x_{k+1}^{\alpha}
      // xfree gets ResiduFree at first
      SiconosVector& xfree = *workVectors[FirstOrderDS::xfree];
      xfree = *workVectors[FirstOrderDS::residuFree];

      DEBUG_PRINT("EulerMoreauOSI::computeFreeState xfree <- residuFree\n");
      DEBUG_EXPR(xfree.display());

      if (_useGamma)
      {
        SiconosVector& rold = *d.rMemory()->getSiconosVector(0);
        double coeff = -h * (1 - _gamma);
        scal(coeff, rold, xfree, false); //  xfree += -h(1-gamma)*rold
      }


      // At this point xfree = (ResiduFree - h(1-gamma)*rold)
      // -> Solve WX = xfree and set xfree = X
      W.PLUForwardBackwardInPlace(xfree);

      // at this point, xfree = W^{-1} (ResiduFree - h(1-gamma)*rold)
      // -> compute real xfree = x - W^{-1} (ResiduFree - h(1-gamma)*rold)
      xfree *= -1.0;
      xfree += x;

      DEBUG_EXPR(xfree.display());

      // now the crazy intermediate variables
      // xPartialNS was updated before this fonction call
      // It constains either 0 (first Newton iterate)
      // or g(x, \lambda, t_{k+1}) - B_{k+1}^{\alpha} \lambda - K_{k+1}^{\alpha} x
      SiconosVector& xPartialNS = *workVectors[FirstOrderDS::xPartialNS];
      DEBUG_PRINT("EulerMoreauOSI::computeFreeState xPartialNS from Interaction\n");
      DEBUG_EXPR(xPartialNS.display());

      // -> Solve WX = g(x, \lambda, t_{k+1}) - B_{k+1}^{\alpha} \lambda - K_{k+1}^{\alpha} x
      // and set xPartialNS = X
      W.PLUForwardBackwardInPlace(xPartialNS);
      scal(h, xPartialNS, xPartialNS);

      // compute real xPartialNS = xfree + ...
      xPartialNS += xfree;
      DEBUG_PRINT("EulerMoreauOSI::computeFreeState xPartialNS real value\n");
      DEBUG_EXPR(xPartialNS.display());

      // deltaxForRelation = (\widetilde{K}_{k+1}^{\alpha})^{-1} xPartialNS - x
      SiconosVector& deltaxForRelation = *workVectors[FirstOrderDS::deltaxForRelation];
      deltaxForRelation = xPartialNS;

      deltaxForRelation -= x;

      DEBUG_EXPR(deltaxForRelation.display());

      // have a look at the end of the DevNotes for this part
      if (_useGammaForRelation)
      {
        if (!(dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS))
          RuntimeException::selfThrow("EulerMoreauOSI::computeFreeState - _useGammaForRelation == true is only implemented for FirstOrderLinearDS or FirstOrderLinearTIDS");

        deltaxForRelation = xfree;

        scal(_gamma, deltaxForRelation, deltaxForRelation);
        SiconosVector& xold = *d.xMemory()->getSiconosVector(0);

        scal(1.0 - _gamma, xold, deltaxForRelation, false);
      }

      // some output
      DEBUG_EXPR(xfree.display(););
      DEBUG_EXPR(xPartialNS.display(););
Esempio n. 12
0
void EulerMoreauOSI::initW(double t, SP::DynamicalSystem ds)
{
  // This function:
  // - allocate memory for a matrix W
  // - insert this matrix into WMap with ds as a key

  if (!ds)
    RuntimeException::selfThrow("EulerMoreauOSI::initW(t,ds) - ds == NULL");

  if (!OSIDynamicalSystems->isIn(ds))
    RuntimeException::selfThrow("EulerMoreauOSI::initW(t,ds) - ds does not belong to the OSI.");
  unsigned int dsN = ds->number();
  if (WMap.find(dsN) != WMap.end())
    RuntimeException::selfThrow("EulerMoreauOSI::initW(t,ds) - W(ds) is already in the map and has been initialized.");


  unsigned int sizeW = ds->getDim(); // n for first order systems, ndof for lagrangian.
  // Memory allocation for W
  //  WMap[ds].reset(new SimpleMatrix(sizeW,sizeW));
  //   SP::SiconosMatrix W = WMap[ds];

  double h = simulationLink->timeStep();
  Type::Siconos dsType = Type::value(*ds);

  // 1 - First order non linear systems
  if (dsType == Type::FirstOrderNonLinearDS || dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS)
  {
    //    // Memory allocation for W
    //     WMap[ds].reset(new SimpleMatrix(sizeW,sizeW));
    //     SP::SiconosMatrix W = WMap[ds];

    // W =  M - h*_theta* [jacobian_x f(t,x,z)]
    SP::FirstOrderNonLinearDS d = std11::static_pointer_cast<FirstOrderNonLinearDS> (ds);

    // Copy M or I if M is Null into W


    //    SP::SiconosMatrix W = WMap[ds];

    if (d->M())
      //      *W = *d->M();
      WMap[dsN].reset(new SimpleMatrix(*d->M()));

    else
    {
      //W->eye();
      // WMap[ds].reset(new SimpleMatrix(sizeW,sizeW,Siconos::IDENTITY));
      WMap[dsN].reset(new SimpleMatrix(sizeW, sizeW)); // Warning if the Jacobian is a sparse matrix
      WMap[dsN]->eye();
    }
    SP::SiconosMatrix W = WMap[dsN];


    // d->computeJacobianfx(t); // Computation of JacxF is not required here
    // since it must have been done in OSI->initialize, before a call to this function.

    // Add -h*_theta*jacobian_XF to W
    scal(-h * _theta, *d->jacobianfx(), *W, false);
  }
  else RuntimeException::selfThrow("EulerMoreauOSI::initW - 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. 13
0
void SchatzmanPaoliOSI::computeFreeState()
{
  // This function computes "free" states of the DS belonging to this Integrator.
  // "Free" means without taking non-smooth effects into account.

  //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.

  //  Note: integration of r with a theta method has been removed
  //  SiconosVector *rold = static_cast<SiconosVector*>(d->rMemory()->getSiconosVector(0));

  // Iteration through the set of Dynamical Systems.
  //
  DSIterator it; // Iterator through the set of DS.

  SP::DynamicalSystem ds; // Current Dynamical System.
  SP::SiconosMatrix W; // W SchatzmanPaoliOSI matrix of the current DS.
  Type::Siconos dsType ; // Type of the current DS.
  for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
  {
    ds = *it; // the considered dynamical system
    dsType = Type::value(*ds); // Its type
    W = WMap[ds->number()]; // Its W SchatzmanPaoliOSI matrix of iteration.

    //1 - Lagrangian Non Linear Systems
    if (dsType == Type::LagrangianDS)
    {
      // // IN to be updated at current time: W, M, q, v, fL
      //       // IN at told: qi,vi, fLi

      //       // Note: indices i/i+1 corresponds to value at the beginning/end of the time step.
      //       // Index k stands for Newton iteration and thus corresponds to the last computed
      //       // value, ie the one saved in the DynamicalSystem.
      //       // "i" values are saved in memory vectors.

      //       // vFree = v_k,i+1 - W^{-1} ResiduFree
      //       // with
      //       // ResiduFree = M(q_k,i+1)(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)

      //       // -- 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);

      //       // --- ResiduFree computation ---
      //       // ResFree = M(v-vold) - h*[theta*forces(t) + (1-theta)*forces(told)]
      //       //
      //       // vFree pointer is used to compute and save ResiduFree in this first step.
      //       SP::SiconosVector vfree = d->workspace(DynamicalSystem::free);//workX[d];
      //       (*vfree)=*(d->workspace(DynamicalSystem::freeresidu));

      //       // -- Update W --
      //       // Note: during computeW, mass and jacobians of fL will be computed/
      //       computeW(t,d);
      //       SP::SiconosVector v = d->velocity(); // v = v_k,i+1

      // // -- vfree =  v - W^{-1} ResiduFree --
      //       // At this point vfree = residuFree
      //       // -> Solve WX = vfree and set vfree = X
      //       W->PLUForwardBackwardInPlace(*vfree);
      //       // -> compute real vfree
      //       *vfree *= -1.0;
      //       *vfree += *v;
      RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType);
    }
    // 2 - Lagrangian Linear Systems
    else if (dsType == Type::LagrangianLinearTIDS)
    {
      // IN to be updated at current time: Fext
      // IN at told: qi,vi, fext
      // IN constants: K,C

      // Note: indices i/i+1 corresponds to value at the beginning/end of the time step.
      // "i" values are saved in memory vectors.

      // vFree = v_i + W^{-1} ResiduFree    // with
      // ResiduFree = (-h*C -h^2*theta*K)*vi - h*K*qi + h*theta * Fext_i+1 + h*(1-theta)*Fext_i

      // -- 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 qold = d->qMemory()->getSiconosVector(0); // q_k
      //   SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); //v_k

      // --- ResiduFree computation ---

      // vFree pointer is used to compute and save ResiduFree in this first step.


      // Velocity free and residu. vFree = RESfree (pointer equality !!).
      SP::SiconosVector qfree = d->workspace(DynamicalSystem::free);//workX[d];
      (*qfree) = *(d->workspace(DynamicalSystem::freeresidu));

      W->PLUForwardBackwardInPlace(*qfree);
      *qfree *= -1.0;
      *qfree += *qold;

    }
    // 3 - Newton Euler Systems
    else if (dsType == Type::NewtonEulerDS)
    {
      // // IN to be updated at current time: W, M, q, v, fL
      // // IN at told: qi,vi, fLi

      // // Note: indices i/i+1 corresponds to value at the beginning/end of the time step.
      // // Index k stands for Newton iteration and thus corresponds to the last computed
      // // value, ie the one saved in the DynamicalSystem.
      // // "i" values are saved in memory vectors.

      // // vFree = v_k,i+1 - W^{-1} ResiduFree
      // // with
      // // ResiduFree = M(q_k,i+1)(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)

      // // -- Convert the DS into a Lagrangian one.
      // SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
      // computeW(t,d);
      // // 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);

      // // --- ResiduFree computation ---
      // // ResFree = M(v-vold) - h*[theta*forces(t) + (1-theta)*forces(told)]
      // //
      // // vFree pointer is used to compute and save ResiduFree in this first step.
      // SP::SiconosVector vfree = d->workspace(DynamicalSystem::free);//workX[d];
      // (*vfree)=*(d->workspace(DynamicalSystem::freeresidu));
      // //*(d->vPredictor())=*(d->workspace(DynamicalSystem::freeresidu));

      // // -- Update W --
      // // Note: during computeW, mass and jacobians of fL will be computed/
      // SP::SiconosVector v = d->velocity(); // v = v_k,i+1

      // // -- vfree =  v - W^{-1} ResiduFree --
      // // At this point vfree = residuFree
      // // -> Solve WX = vfree and set vfree = X
      // //   std::cout<<"SchatzmanPaoliOSI::computeFreeState residu free"<<endl;
      // //   vfree->display();
      // d->luW()->PLUForwardBackwardInPlace(*vfree);
      // //   std::cout<<"SchatzmanPaoliOSI::computeFreeState -WRfree"<<endl;
      // //   vfree->display();
      // //   scal(h,*vfree,*vfree);
      // // -> compute real vfree
      // *vfree *= -1.0;
      // *vfree += *v;
      RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType);
    }
    else
      RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType);
  }

}
Esempio n. 14
0
void SchatzmanPaoliOSI::computeW(double t, SP::DynamicalSystem ds)
{
  // Compute W matrix of the Dynamical System ds, at time t and for the current ds state.

  // When this function is called, WMap[ds] is supposed to exist and not to be null
  // Memory allocation has been done during initW.

  assert(ds &&
         "SchatzmanPaoliOSI::computeW(t,ds) - ds == NULL");
  unsigned int dsN = ds->number();
  assert((WMap.find(dsN) != WMap.end()) &&
         "SchatzmanPaoliOSI::computeW(t,ds) - W(ds) does not exists. Maybe you forget to initialize the osi?");

  //double h = simulationLink->timeStep();
  Type::Siconos dsType = Type::value(*ds);

  SP::SiconosMatrix W = WMap[dsN];

  // 1 - Lagrangian non linear systems
  if (dsType == Type::LagrangianDS)
  {
    // SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
    // SP::SiconosMatrix K = d->jacobianqForces(); // jacobian according to q
    // SP::SiconosMatrix C = d->jacobianqDotForces(); // jacobian according to velocity

    // d->computeMass();
    // *W = *d->mass();

    // if (C)
    // {
    //   d->computeJacobianqDotForces(t);
    //   scal(-h*_theta, *C,*W,false); // W -= h*_theta*C
    // }

    // if (K)
    // {
    //   d->computeJacobianqForces(t);
    //   scal(-h*h*_theta*_theta,*K,*W,false); //*W -= h*h*_theta*_theta**K;
    // }
    RuntimeException::selfThrow("SchatzmanPaoliOSI::computeW - not yet implemented for Dynamical system type :" + dsType);

  }
  // 4 - Lagrangian linear systems
  else if (dsType == Type::LagrangianLinearTIDS)
  {
    // Nothing: W does not depend on time.
  }

  // === ===
  else if (dsType == Type::NewtonEulerDS)
  {
    // SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
    // d->computeJacobianvFL(t);
    // double thetaFL=_theta;
    // *(d->luW())=*(d->jacobianvFL());
    // scal(h*thetaFL,*(d->jacobianvFL()),*(d->luW()),true);
    // *(d->luW())+=*(d->massMatrix());

    // //cout<<"SchatzmanPaoliOSI::computeW luW before LUFact\n";
    // //d->luW()->display();

    // d->luW()->PLUFactorizationInPlace();
    RuntimeException::selfThrow("SchatzmanPaoliOSI::computeW - not yet implemented for Dynamical system type :" + dsType);
  }
  else RuntimeException::selfThrow("SchatzmanPaoliOSI::computeW - not yet implemented for Dynamical system type :" + dsType);

  // Remark: W is not LU-factorized here.
  // Function PLUForwardBackward will do that if required.
}
Esempio n. 15
0
void SchatzmanPaoliOSI::initW(double t, SP::DynamicalSystem ds)
{
  // This function:
  // - allocate memory for a matrix W
  // - insert this matrix into WMap with ds as a key

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

  if (!OSIDynamicalSystems->isIn(ds))
    RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - ds does not belong to the OSI.");

  unsigned int dsN = ds->number();
  if (WMap.find(dsN) != WMap.end())
    RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - W(ds) is already in the map and has been initialized.");


  //unsigned int sizeW = ds->getDim(); // n for first order systems, ndof for lagrangian.
  // Memory allocation for W
  //  WMap[ds].reset(new SimpleMatrix(sizeW,sizeW));
  //   SP::SiconosMatrix W = WMap[ds];

  double h = simulationLink->timeStep();
  Type::Siconos dsType = Type::value(*ds);


  // 1 - Lagrangian non linear systems
  if (dsType == Type::LagrangianDS)
  {
    // SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
    // SP::SiconosMatrix K = d->jacobianqForces(); // jacobian according to q
    // SP::SiconosMatrix C = d->jacobianqDotForces(); // jacobian according to velocity
    // WMap[ds].reset(new SimpleMatrix(*d->mass())); //*W = *d->mass();

    // SP::SiconosMatrix W = WMap[ds];

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

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

    // // WBoundaryConditions initialization
    // if (d->boundaryConditions())
    //   initWBoundaryConditions(d);
    RuntimeException::selfThrow("SchatzmanPaoliOSI::initW - 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);
    SP::SiconosMatrix K = d->K();
    SP::SiconosMatrix C = d->C();
    WMap[dsN].reset(new SimpleMatrix(*d->mass())); //*W = *d->mass();
    SP::SiconosMatrix W = WMap[dsN];

    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())
      initWBoundaryConditions(d);


  }

  // === ===
  else if (dsType == Type::NewtonEulerDS)
  {
    //WMap[ds].reset(new SimpleMatrix(3,3));
    RuntimeException::selfThrow("SchatzmanPaoliOSI::initW - not yet implemented for Dynamical system type :" + dsType);
  }
  else RuntimeException::selfThrow("SchatzmanPaoliOSI::initW - 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. 16
0
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);
      //       SP::SiconosVector v = d->velocity();
      // #ifdef SCHATZMANPAOLI_NE_DEBUG
      //       std::cout<<"SchatzmanPaoliOSI::updatestate prev v"<<endl;
      //       v->display();
      // #endif

      //       /*d->p has been fill by the Relation->computeInput, it contains
      //            B \lambda _{k+1}*/
      //       *v = *d->p(level); // v = p
      //       d->luW()->PLUForwardBackwardInPlace(*v);

      // #ifdef SCHATZMANPAOLI_NE_DEBUG
      //       std::cout<<"SchatzmanPaoliOSI::updatestate hWB lambda"<<endl;
      //       v->display();
      // #endif

      //       *v +=  * ds->workspace(DynamicalSystem::free);

      // #ifdef SCHATZMANPAOLI_NE_DEBUG
      //       std::cout<<"SchatzmanPaoliOSI::updatestate work free"<<endl;
      //       ds->workspace(DynamicalSystem::free)->display();
      //       std::cout<<"SchatzmanPaoliOSI::updatestate new v"<<endl;
      //       v->display();
      // #endif
      //       //compute q
      //       //first step consists in computing  \dot q.
      //       //second step consists in updating q.
      //       //
      //       SP::SiconosMatrix T = d->T();
      //       SP::SiconosVector dotq = d->dotq();
      //       prod(*T,*v,*dotq,true);
      //       // std::cout<<"SchatzmanPaoliOSI::updateState v"<<endl;
      //       // v->display();
      //       // std::cout<<"SchatzmanPaoliOSI::updateState dotq"<<endl;
      //       // dotq->display();




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

      //       //  -> get previous time step state
      //       SP::SiconosVector dotqold = d->dotqMemory()->getSiconosVector(0);
      //       SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
      //       // *q = *qold + h*(theta * *v +(1.0 - theta)* *vold)
      //       double coeff = h*_theta;
      //       scal(coeff, *dotq, *q) ; // q = h*theta*v
      //       coeff = h*(1-_theta);
      //       scal(coeff,*dotqold,*q,false); // q += h(1-theta)*vold
      //       *q += *qold;
      // #ifdef SCHATZMANPAOLI_NE_DEBUG
      //       std::cout<<"new q before normalizing"<<endl;
      //       q->display();
      // #endif

      //       //q[3:6] must be normalized
      //       d->normalizeq();
      //       dotq->setValue(3,(q->getValue(3)-qold->getValue(3))/h);
      //       dotq->setValue(4,(q->getValue(4)-qold->getValue(4))/h);
      //       dotq->setValue(5,(q->getValue(5)-qold->getValue(5))/h);
      //       dotq->setValue(6,(q->getValue(6)-qold->getValue(6))/h);
      //       d->updateT();
      RuntimeException::selfThrow("SchatzmanPaoliOSI::updateState - not yet implemented for Dynamical system type: " + dsType);
    }
    else RuntimeException::selfThrow("SchatzmanPaoliOSI::updateState - not yet implemented for Dynamical system type: " + dsType);
  }
}