Exemplo n.º 1
0
SP::SiconosVector Simulation::lambda(unsigned int level, unsigned int coor)
{
  // return input(level) (ie with lambda[level]) for all Interactions.
  // assert(level>=0);

  DEBUG_BEGIN("Simulation::input(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 lambda (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);
    lambda->setValue(i,inter->lambda(level)->getValue(coor));
    i++;
  }
  DEBUG_END("Simulation::input(unsigned int level, unsigned int coor)\n");
  return lambda;
}
Exemplo n.º 2
0
void EventDriven::updateIndexSetsWithDoubleCondition()
{

  assert(_nsds);
  assert(_nsds->topology());

  // for all Interactions in indexSet[i-1], compute y[i-1] and
  // update the indexSet[i]

  SP::Topology topo = _nsds->topology();

  SP::InteractionsGraph indexSet2 = topo->indexSet(2);

  InteractionsGraph::VIterator ui, uiend, vnext;
  std11::tie(ui, uiend) = indexSet2->vertices();

  for (vnext = ui; ui != uiend; ui = vnext)
  {
    ++vnext;

    SP::Interaction inter = indexSet2->bundle(*ui);
    double gamma = inter->getYRef(2);
    double F     = inter->getLambdaRef(2);
    if (fabs(F) < _TOL_ED)
      indexSet2->remove_vertex(inter);
    else if ((gamma < -_TOL_ED) || (F < -_TOL_ED))
      RuntimeException::selfThrow("EventDriven::updateIndexSetsWithDoubleCondition(), output[2] and lambda[2] for Interactionof indexSet[2] must be higher or equal to zero.");
    else if (((fabs(gamma) > _TOL_ED) && (fabs(F) > _TOL_ED)))
      RuntimeException::selfThrow("EventDriven::updateIndexSetsWithDoubleCondition(), something is wrong for the LCP resolution.");
  }
}
Exemplo n.º 3
0
std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor>
Topology::link(SP::Interaction inter, SP::DynamicalSystem ds, SP::DynamicalSystem ds2)
{
  DEBUG_PRINTF("Topology::link : inter %p, ds1 %p, ds2 %p\n", &*inter, &*ds,
               &*ds2);
  if (indexSet0()->is_vertex(inter))
  {
    removeInteractionFromIndexSet(inter);
  }

  unsigned int sumOfDSSizes = 0, sumOfZSizes = 0;

  sumOfDSSizes += ds->getDim();
  if(ds->z())
    sumOfZSizes += ds->z()->size();

  if(ds2)
  {
    sumOfDSSizes += ds2->getDim();
    if(ds->z())
      sumOfZSizes += ds2->z()->size();
    inter->setHas2Bodies(true);
  }
  DEBUG_PRINTF("sumOfDSSizes = %i\t, sumOfZSizes = %i\n ", sumOfDSSizes, sumOfZSizes);

  inter->setDSSizes(sumOfDSSizes, sumOfZSizes);

  return addInteractionInIndexSet0(inter, ds, ds2);
}
Exemplo n.º 4
0
void MLCPProjectOnConstraints::postCompute()
{
  _hasBeenUpdated = true;
  // This function is used to set y/lambda values using output from
  // lcp_driver (w,z).  Only Interactions (ie Interactions) of
  // indexSet(leveMin) are concerned.

  // === Get index set from Topology ===
  SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());

  // y and lambda vectors
  SP::SiconosVector lambda;
  SP::SiconosVector y;

  // === Loop through "active" Interactions (ie present in
  // indexSets[1]) ===
  /** We chose to do a small step _alpha in view of stabilized the algorithm.*/
#ifdef MLCPPROJ_DEBUG
  printf("MLCPProjectOnConstraints::postCompute damping value = %f\n", _alpha);
#endif
  (*_z) *= _alpha;
  unsigned int pos = 0;
#ifdef MLCPPROJ_DEBUG
  printf("MLCPProjectOnConstraints::postCompute _z\n");
  _z->display();
  display();
#endif



  InteractionsGraph::VIterator ui, uiend;

  for (std11::tie(ui, uiend) = indexSet->vertices(); ui != uiend; ++ui)
  {

    SP::Interaction inter = indexSet->bundle(*ui);
    // Get the relative position of inter-interactionBlock in the vector w
    // or z
    pos = _M->getPositionOfInteractionBlock(*inter);
    RELATION::TYPES relationType = inter->relation()->getType();
    if (relationType == NewtonEuler)
    {
      postComputeNewtonEulerR(inter, pos);
    }
    else if (relationType == Lagrangian)
    {
      postComputeLagrangianR(inter, pos);
    }
    else
    {
      RuntimeException::selfThrow("MLCPProjectOnConstraints::computeInteractionBlock - relation type is not from Lagrangian type neither NewtonEuler.");
    }

  }



}
Exemplo n.º 5
0
  void visit(const Hem5OSI&)
  {
    unsigned int lowerLevelForOutput = LEVELMAX;
    unsigned int upperLevelForOutput = 0;
    unsigned int lowerLevelForInput = LEVELMAX;
    unsigned int upperLevelForInput = 0;

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

    /** there is only a test on the dstype and simulation since  we assume that
     * we implicitely the nonsmooth law when a DS type is chosen
     */

    if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS || dsType == Type::NewtonEulerDS)
    {
      if (Type::value(*_parent) == Type::EventDriven)
      {
        Type::Siconos nslType = Type::value(*_nonSmoothLaw);

        if (nslType == Type::NewtonImpactNSL || nslType == Type::MultipleImpactNSL)
        {
          lowerLevelForOutput = 0;
          upperLevelForOutput = 2 ;
          lowerLevelForInput = 1;
          upperLevelForInput = 2;
        }
        else if (nslType ==  Type::NewtonImpactFrictionNSL)
        {
          lowerLevelForOutput = 0;
          upperLevelForOutput = 4;
          lowerLevelForInput = 1;
          upperLevelForInput = 2;
          RuntimeException::selfThrow("Simulation::SetupLevels::visit - simulation of type: " + Type::name(*_parent) + " not yet implemented for nonsmooth law of type NewtonImpactFrictionNSL");
        }
        else
        {
          RuntimeException::selfThrow("Simulation::SetupLevels::visit - simulation of type: " + Type::name(*_parent) + "not yet implemented  for nonsmooth of type");
        }
      }
      else
        RuntimeException::selfThrow("Simulation::SetupLevels::visit - unknown simulation type: " + Type::name(*_parent));
    }
    else RuntimeException::selfThrow("Simulation::SetupLevels::visit - not yet implemented for Dynamical system type :" + dsType);

    _parent->_levelMinForInput = std::min<int>(lowerLevelForInput, _parent->_levelMinForInput);
    _parent->_levelMaxForInput = std::max<int>(upperLevelForInput, _parent->_levelMaxForInput);
    _parent->_levelMinForOutput = std::min<int>(lowerLevelForOutput, _parent->_levelMinForInput);
    _parent->_levelMaxForOutput = std::max<int>(upperLevelForOutput, _parent->_levelMaxForInput);
    _parent->_numberOfIndexSets = std::max<int>(_parent->_levelMaxForOutput + 1, _parent->_numberOfIndexSets);
    _interaction->setLowerLevelForOutput(lowerLevelForOutput);
    _interaction->setUpperLevelForOutput(upperLevelForOutput);

    _interaction->setLowerLevelForInput(lowerLevelForInput);
    _interaction->setUpperLevelForInput(upperLevelForInput);

    _interaction->setSteps(1);
  };
Exemplo n.º 6
0
  void visit(const NewtonImpactFrictionNSL& nslaw)
  {
    double e;
    e = nslaw.en();
    // Only the normal part is multiplied by e
    SP::SiconosVector y_k_1 ;
    y_k_1 = _inter->yMemory(_osnsp->inputOutputLevel())->getSiconosVector(1);
    (*_inter->yForNSsolver())(0) +=  e * (*y_k_1)(0);

  }
Exemplo n.º 7
0
 void visit(const NewtonImpactNSL& nslaw)
 {
   double e;
   e = nslaw.e();
   Index subCoord(4);
   subCoord[0] = 0;
   subCoord[1] = _inter->nonSmoothLaw()->size();
   subCoord[2] = 0;
   subCoord[3] = subCoord[1];
   subscal(e, *_inter->yOld(_osnsp->inputOutputLevel()), *(_inter->yForNSsolver()), subCoord, false); // q = q + e * q
 }
Exemplo n.º 8
0
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
}
Exemplo n.º 9
0
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);

}
Exemplo n.º 10
0
  void visit(const MoreauJeanCombinedProjectionOSI& moreauCPOSI)
  {
    unsigned int lowerLevelForOutput = LEVELMAX;
    unsigned int upperLevelForOutput = 0;
    unsigned int lowerLevelForInput = LEVELMAX;
    unsigned int upperLevelForInput = 0;

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

    if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS || dsType == Type::NewtonEulerDS)
    {
      if (Type::value(*_parent) == Type::TimeStepping)
      {
        lowerLevelForOutput = 0;
        upperLevelForOutput = 1;
        lowerLevelForInput = 1;
        upperLevelForInput = 1;
      }
      else if (Type::value(*_parent) == Type::TimeSteppingCombinedProjection)
      {
        // Warning : we never enter this case !!!
        lowerLevelForOutput = 0;
        upperLevelForOutput = 1 ;
        lowerLevelForInput = 0;
        upperLevelForInput = 1;
      }
      else
      {
        RuntimeException::selfThrow("Simulation::SetupLevels::visit(const MoreauJeanCombinedProjectionOSI) - unknown simulation type: " + Type::name(*_parent));
      }
    }
    else RuntimeException::selfThrow("Simulation::SetupLevels::visit(const MoreauJeanCombinedProjectionOSI) - not yet implemented for Dynamical system type :" + dsType);

    _parent->_levelMinForInput = std::min<int>(lowerLevelForInput, _parent->_levelMinForInput);
    _parent->_levelMaxForInput = std::max<int>(upperLevelForInput, _parent->_levelMaxForInput);
    _parent->_levelMinForOutput = std::min<int>(lowerLevelForOutput, _parent->_levelMinForInput);
    _parent->_levelMaxForOutput = std::max<int>(upperLevelForOutput, _parent->_levelMaxForInput);
    _parent->_numberOfIndexSets = std::max<int>(_parent->_levelMaxForOutput + 1, _parent->_numberOfIndexSets);


    _interaction->setLowerLevelForOutput(lowerLevelForOutput);
    _interaction->setUpperLevelForOutput(upperLevelForOutput);

    _interaction->setLowerLevelForInput(lowerLevelForInput);
    _interaction->setUpperLevelForInput(upperLevelForInput);

    _interaction->setSteps(1);

  };
Exemplo n.º 11
0
  void visit(const D1MinusLinearOSI& d1OSI)
  {
    unsigned int lowerLevelForOutput = LEVELMAX;
    unsigned int upperLevelForOutput = 0;
    unsigned int lowerLevelForInput = LEVELMAX;
    unsigned int upperLevelForInput = 0;

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

    /** there is only a test on the dstype and simulation since  we assume that
     * we implicitely the nonsmooth law when a DS type is chosen
     */

    if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS || dsType == Type::NewtonEulerDS)
    {


      if (Type::value(*_parent) == Type::TimeSteppingD1Minus)
      {
        lowerLevelForOutput = 0;
        upperLevelForOutput = 2 ;
        lowerLevelForInput = 1;
        upperLevelForInput = 2;
      }
      else
        RuntimeException::selfThrow("Simulation::SetupLevels::visit(const D1MinusLinearOSI&) - unknown simulation type: " + Type::name(*_parent));
    }
    else RuntimeException::selfThrow("Simulation::SetupLevels::visit(const D1MinusLinearOSI&) - not yet implemented for Dynamical system type :" + dsType);

    _parent->_levelMinForInput = std::min<int>(lowerLevelForInput, _parent->_levelMinForInput);
    _parent->_levelMaxForInput = std::max<int>(upperLevelForInput, _parent->_levelMaxForInput);
    _parent->_levelMinForOutput = std::min<int>(lowerLevelForOutput, _parent->_levelMinForInput);
    _parent->_levelMaxForOutput = std::max<int>(upperLevelForOutput, _parent->_levelMaxForInput);

    /* Get the number of required index sets. */

    unsigned int nbIndexSets =  d1OSI.numberOfIndexSets();

    _parent->_numberOfIndexSets = std::max<int>(nbIndexSets, _parent->_numberOfIndexSets);

    _interaction->setLowerLevelForOutput(lowerLevelForOutput);
    _interaction->setUpperLevelForOutput(upperLevelForOutput);

    _interaction->setLowerLevelForInput(lowerLevelForInput);
    _interaction->setUpperLevelForInput(upperLevelForInput);

    _interaction->setSteps(2); // Two evaluations of lambda(2) are made for each time--step
  };
Exemplo n.º 12
0
  void visit(const NewtonImpactNSL& nslaw)
  {
    double e;
    e = nslaw.e();
    Index subCoord(4);
    subCoord[0] = 0;
    subCoord[1] = _inter->nonSmoothLaw()->size();
    subCoord[2] = 0;
    subCoord[3] = subCoord[1];
    // Only the normal part is multiplied by e
    SP::SiconosVector y_k_1 ;
    y_k_1 = _inter->yMemory(_osnsp->inputOutputLevel())->getSiconosVector(1);


    //  std::cout << "y_k_1 " << std::endl;
    // y_k_1->display();
    subscal(e, *y_k_1, *(_inter->yForNSsolver()), subCoord, false);
  }
Exemplo n.º 13
0
  void visit(const NewtonImpactNSL& nslaw)
  {
    double e;
    e = nslaw.e();
    Index subCoord(4);
    subCoord[0] = 0;
    subCoord[1] = _inter->nonSmoothLaw()->size();
    subCoord[2] = 0;
    subCoord[3] = subCoord[1];
    // Only the normal part is multiplied by e
    const SiconosVector& y_k_1(
      _inter->yMemory(_osnsp->inputOutputLevel()).getSiconosVector(1));

    DEBUG_PRINTF("_osnsp->inputOutputLevel() = %i \n ",_osnsp->inputOutputLevel() );
    DEBUG_EXPR(y_k_1.display());;
    SiconosVector & osnsp_rhs = *(*_interProp.workVectors)[SchatzmanPaoliOSI::OSNSP_RHS];
    subscal(e, y_k_1, osnsp_rhs, subCoord, false);
  }
Exemplo n.º 14
0
  void visit(const NewtonImpactFrictionNSL& nslaw)
  {
    double e;
    e = nslaw.en();
    // Only the normal part is multiplied by e
    const SiconosVector& y_k_1(
      _inter->yMemory(_osnsp->inputOutputLevel()).getSiconosVector(1));
    SiconosVector & osnsp_rhs = *(*_interProp.workVectors)[SchatzmanPaoliOSI::OSNSP_RHS];
    osnsp_rhs (0) +=  e * (y_k_1)(0);

  }
Exemplo n.º 15
0
void Simulation::updateOutput(unsigned int level)
{
  // To compute output(level) (ie with y[level]) for all Interactions.
  //  assert(level>=0);

  DEBUG_PRINTF("Simulation::updateOutput(unsigned int level) starts for level = %i\n", level);
  double time = model()->currentTime();
  InteractionsGraph::VIterator ui, uiend;
  SP::Interaction inter;
  SP::InteractionsGraph indexSet0 = model()->nonSmoothDynamicalSystem()->topology()->indexSet0();
  for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
  {
    inter = indexSet0->bundle(*ui);
    assert(inter->lowerLevelForOutput() <= level);
    assert(inter->upperLevelForOutput() >= level);
    inter->computeOutput(time, indexSet0->properties(*ui), level);
  }
  DEBUG_PRINTF("Simulation::updateOutput(unsigned int level) ends for level = %i\n", level);

}
Exemplo n.º 16
0
void Simulation::updateInput(unsigned int level)
{
  // To compute input(level) (ie with lambda[level]) for all Interactions.
  //  assert(level>=0);
  //  double time = nextTime();
  double time = model()->currentTime();
  // Set dynamical systems non-smooth part to zero.
  reset(level);

  // We compute input using lambda(level).
  InteractionsGraph::VIterator ui, uiend;
  SP::Interaction inter;
  SP::InteractionsGraph indexSet0 = model()->nonSmoothDynamicalSystem()->topology()->indexSet0();
  for (std11::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
  {
    inter = indexSet0->bundle(*ui);
    assert(inter->lowerLevelForInput() <= level);
    assert(inter->upperLevelForInput() >= level);
    inter->computeInput(time, indexSet0->properties(*ui), level);
  }
}
Exemplo n.º 17
0
void Simulation::initializeInteraction(double time, SP::Interaction inter)
{
  // determine which (lower and upper) levels are required for this Interaction
  // in this Simulation.
  computeLevelsForInputAndOutput(inter);

  // Get the interaction properties from the topology for initialization.
  SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0();
  InteractionsGraph::VDescriptor ui = indexSet0->descriptor(inter);

  // This calls computeOutput() and initializes qMemory and q_k.
  inter->initialize(time, indexSet0->properties(ui));
}
Exemplo n.º 18
0
  void visit(const SchatzmanPaoliOSI&)
  {
    unsigned int lowerLevelForOutput = LEVELMAX;
    unsigned int upperLevelForOutput = 0;
    unsigned int lowerLevelForInput = LEVELMAX;
    unsigned int upperLevelForInput = 0;

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

    if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS || dsType == Type::NewtonEulerDS)
    {

      if (Type::value(*_parent) == Type::TimeStepping)
      {
        lowerLevelForOutput = 0;
        upperLevelForOutput = 0;
        lowerLevelForInput = 0;
        upperLevelForInput = 0;
      }
      else
        RuntimeException::selfThrow("Simulation::SetupLevels::visit - unknown simulation type: " + Type::name(*_parent));
    }
    else RuntimeException::selfThrow("Simulation::SetupLevels::visit - not yet implemented for Dynamical system type :" + dsType);

    _parent->_levelMinForInput = std::min<int>(lowerLevelForInput, _parent->_levelMinForInput);
    _parent->_levelMaxForInput = std::max<int>(upperLevelForInput, _parent->_levelMaxForInput);
    _parent->_levelMinForOutput = std::min<int>(lowerLevelForOutput, _parent->_levelMinForInput);
    _parent->_levelMaxForOutput = std::max<int>(upperLevelForOutput, _parent->_levelMaxForInput);
    _parent->_numberOfIndexSets = std::max<int>(_parent->_levelMaxForOutput + 1, _parent->_numberOfIndexSets);

    _interaction->setLowerLevelForOutput(lowerLevelForOutput);
    _interaction->setUpperLevelForOutput(upperLevelForOutput);

    _interaction->setLowerLevelForInput(lowerLevelForInput);
    _interaction->setUpperLevelForInput(upperLevelForInput);

    _interaction->setSteps(2);
  };
Exemplo n.º 19
0
void SchatzmanPaoliOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp)
{

  DEBUG_BEGIN("SchatzmanPaoliOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp)\n");
  /** \warning: ensures that it can also work with two different osi for two different ds ?
   */

  SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel());
  SP::Interaction inter = indexSet->bundle(vertex_inter);
  SP::OneStepNSProblems  allOSNS  = _simulation->oneStepNSProblems();
  VectorOfBlockVectors& inter_work_block = *indexSet->properties(vertex_inter).workBlockVectors;


  // Get relation and non smooth law types
  RELATION::TYPES relationType = inter->relation()->getType();
  RELATION::SUBTYPES relationSubType = inter->relation()->getSubType();
  unsigned int sizeY = inter->nonSmoothLaw()->size();

  unsigned int relativePosition = 0;



  Index coord(8);
  coord[0] = relativePosition;
  coord[1] = relativePosition + sizeY;
  coord[2] = 0;
  coord[4] = 0;
  coord[6] = 0;
  coord[7] = sizeY;
  SP::SiconosMatrix  C;
  SP::SiconosMatrix  D;
  SP::SiconosMatrix  F;
  SP::BlockVector deltax;
  SiconosVector& osnsp_rhs = *(*indexSet->properties(vertex_inter).workVectors)[SchatzmanPaoliOSI::OSNSP_RHS];

  SP::SiconosVector e;
  SP::BlockVector Xfree =  inter_work_block[SchatzmanPaoliOSI::xfree];;
  assert(Xfree);


  SP::Interaction mainInteraction = inter;
  assert(mainInteraction);
  assert(mainInteraction->relation());
  DEBUG_EXPR(inter->display(););
Exemplo n.º 20
0
void BulletSpaceFilter::buildInteractions(double time)
{

    if (! _dynamicCollisionsObjectsInserted)
    {
        DynamicalSystemsGraph& dsg = *(_model->nonSmoothDynamicalSystem()->dynamicalSystems());
        DynamicalSystemsGraph::VIterator dsi, dsiend;
        std11::tie(dsi, dsiend) = dsg.vertices();
        for (; dsi != dsiend; ++dsi)
        {
            CollisionObjects& collisionObjects = *ask<ForCollisionObjects>(*dsg.bundle(*dsi));

            for (CollisionObjects::iterator ico = collisionObjects.begin();
                    ico != collisionObjects.end(); ++ico)
            {
                _collisionWorld->addCollisionObject(const_cast<btCollisionObject*>((*ico).first));
                DEBUG_PRINTF("add dynamic collision object %p\n", const_cast<btCollisionObject*>((*ico).first));
            }
        }

        _dynamicCollisionsObjectsInserted = true;
    }

    if (! _staticCollisionsObjectsInserted)
    {
        for(StaticObjects::iterator
                ic = _staticObjects->begin(); ic != _staticObjects->end(); ++ic)
        {
            _collisionWorld->addCollisionObject(const_cast<btCollisionObject*>((*ic).first));
            DEBUG_PRINTF("add static collision object %p\n", const_cast<btCollisionObject*>((*ic).first));
        }

        _staticCollisionsObjectsInserted = true;
    }

    DEBUG_PRINT("-----start build interaction\n");

    // 1. perform bullet collision detection
    gOrphanedInteractions.clear();
    _collisionWorld->performDiscreteCollisionDetection();

    // 2. collect old contact points from Siconos graph
    std::map<btManifoldPoint*, bool> contactPoints;

    std::map<Interaction*, bool> activeInteractions;

    SP::InteractionsGraph indexSet0 = model()->nonSmoothDynamicalSystem()->topology()->indexSet(0);
    InteractionsGraph::VIterator ui0, ui0end, v0next;
    std11::tie(ui0, ui0end) = indexSet0->vertices();
    for (v0next = ui0 ;
            ui0 != ui0end; ui0 = v0next)
    {
        ++v0next;  // trick to iterate on a dynamic bgl graph
        SP::Interaction inter0 = indexSet0->bundle(*ui0);

        if (gOrphanedInteractions.find(&*inter0) != gOrphanedInteractions.end())
        {
            model()->nonSmoothDynamicalSystem()->removeInteraction(inter0);
        }

        else
        {
            SP::btManifoldPoint cp = ask<ForContactPoint>(*(inter0->relation()));
            if(cp)
            {
                DEBUG_PRINTF("Contact point in interaction : %p\n", &*cp);

                contactPoints[&*cp] = false;
            }
        }
    }
    unsigned int numManifolds =
        _collisionWorld->getDispatcher()->getNumManifolds();

    DEBUG_PRINTF("number of manifolds : %d\n", numManifolds);

    for (unsigned int i = 0; i < numManifolds; ++i)
    {
        btPersistentManifold* contactManifold =
            _collisionWorld->getDispatcher()->getManifoldByIndexInternal(i);

        DEBUG_PRINTF("processing manifold %d : %p\n", i, contactManifold);

        const btCollisionObject* obA = contactManifold->getBody0();
        const btCollisionObject* obB = contactManifold->getBody1();


        DEBUG_PRINTF("object A : %p, object B: %p\n", obA, obB);

        //    contactManifold->refreshContactPoints(obA->getWorldTransform(),obB->getWorldTransform());

        unsigned int numContacts = contactManifold->getNumContacts();

        if ( (obA->getUserPointer() && obA->getUserPointer() != obB->getUserPointer()) ||
                (obB->getUserPointer() && obA->getUserPointer() != obB->getUserPointer()) )
        {

            for (unsigned int z = 0; z < numContacts; ++z)
            {

                SP::btManifoldPoint cpoint(createSPtrbtManifoldPoint(contactManifold->getContactPoint(z)));
                DEBUG_PRINTF("manifold %d, contact %d, &contact %p, lifetime %d\n", i, z, &*cpoint, cpoint->getLifeTime());


                // should no be mixed with something else that use UserPointer!
                SP::BulletDS dsa;
                SP::BulletDS dsb;
                unsigned long int gid1, gid2;

                if(obA->getUserPointer())
                {
                    dsa = static_cast<BulletDS*>(obA->getUserPointer())->shared_ptr();

                    assert(dsa->collisionObjects()->find(contactManifold->getBody0()) !=
                           dsa->collisionObjects()->end());
                    gid1 = boost::get<2>((*((*dsa->collisionObjects()).find(obA))).second);
                }
                else
                {
                    gid1 = (*_staticObjects->find(obA)).second.second;
                }

                SP::NonSmoothLaw nslaw;
                if (obB->getUserPointer())
                {
                    dsb = static_cast<BulletDS*>(obB->getUserPointer())->shared_ptr();

                    assert(dsb->collisionObjects()->find(obB) != dsb->collisionObjects()->end());

                    gid2 = boost::get<2>((*((*dsb->collisionObjects()).find(obB))).second);
                }

                else
                {
                    gid2 = (*_staticObjects->find(obB)).second.second;
                }


                DEBUG_PRINTF("collision between group %ld and %ld\n", gid1, gid2);

                nslaw = (*_nslaws)(gid1, gid2);

                if (!nslaw)
                {
                    RuntimeException::selfThrow(
                        (boost::format("Cannot find nslaw for collision between group %1% and %2%") %
                         gid1 % gid2).str());
                }

                assert(nslaw);

                std::map<btManifoldPoint*, bool>::iterator itc;
                itc = contactPoints.find(&*cpoint);

                DEBUG_EXPR(if (itc == contactPoints.end())
            {
                DEBUG_PRINT("contact point not found\n");
                    for(std::map<btManifoldPoint*, bool>::iterator itd=contactPoints.begin();
                            itd != contactPoints.end(); ++itd)
                    {
                        DEBUG_PRINTF("-->%p != %p\n", &*cpoint, &*(*itd).first);
                    }
                });


                if (itc == contactPoints.end() || !cpoint->m_userPersistentData)
                {
                    /* new interaction */

                    SP::Interaction inter;
                    if (nslaw->size() == 3)
                    {
                        SP::BulletR rel(new BulletR(cpoint, createSPtrbtPersistentManifold(*contactManifold)));
                        inter.reset(new Interaction(3, nslaw, rel, 4 * i + z));
                    }
                    else
                    {
                        if (nslaw->size() == 1)
                        {
                            SP::BulletFrom1DLocalFrameR rel(new BulletFrom1DLocalFrameR(cpoint));
                            inter.reset(new Interaction(1, nslaw, rel, 4 * i + z));
                        }
                    }

                    if (obB->getUserPointer())
                    {
                        SP::BulletDS dsb(static_cast<BulletDS*>(obB->getUserPointer())->shared_ptr());

                        if (dsa != dsb)
                        {
                            DEBUG_PRINTF("LINK obA:%p obB:%p inter:%p\n", obA, obB, &*inter);
                            assert(inter);

                            cpoint->m_userPersistentData = &*inter;
                            link(inter, dsa, dsb);
                        }
                        /* else collision shapes belong to the same object do nothing */
                    }
                    else
                    {
                        DEBUG_PRINTF("LINK obA:%p inter :%p\n", obA, &*inter);
                        assert(inter);

                        cpoint->m_userPersistentData = &*inter;
                        link(inter, dsa);
                    }
                }

                if (cpoint->m_userPersistentData)
                {
                    activeInteractions[static_cast<Interaction *>(cpoint->m_userPersistentData)] = true;
                    DEBUG_PRINTF("Interaction %p = true\n", static_cast<Interaction *>(cpoint->m_userPersistentData));
                    DEBUG_PRINTF("cpoint %p  = true\n", &*cpoint);
                }
                else
                {
                    assert(false);
                    DEBUG_PRINT("cpoint->m_userPersistentData is empty\n");
                }

                contactPoints[&*cpoint] = true;
                DEBUG_PRINTF("cpoint %p  = true\n", &*cpoint);
            }
        }
    }
Exemplo n.º 21
0
 SetupLevels(SP::Simulation s, SP::Interaction inter,
             SP::DynamicalSystem ds) :
   _parent(s), _interaction(inter), _ds(ds)
 {
   _nonSmoothLaw = inter->nonSmoothLaw();
 };
Exemplo n.º 22
0
void EulerMoreauOSI::computeW(double t, DynamicalSystem& ds, DynamicalSystemsGraph::VDescriptor& dsgVD)
{
  // 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.

  unsigned int dsN = ds.number();
  assert((WMap.find(dsN) != WMap.end()) &&
         "EulerMoreauOSI::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);

  SiconosMatrix& W = *WMap[dsN];

  // 1 - First order non linear systems
  if (dsType == Type::FirstOrderNonLinearDS)
  {
    // W =  M - h*_theta* [jacobian_x f(t,x,z)]
    FirstOrderNonLinearDS& d = static_cast<FirstOrderNonLinearDS&> (ds);

    // Copy M or I if M is Null into W
    if (d.M())
      W = *d.M();
    else
      W.eye();

    d.computeJacobianfx(t);
    // Add -h*_theta*jacobianfx to W
    scal(-h * _theta, *d.jacobianfx(), W, false);
  }
  // 2 - First order linear systems
  else if (dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS)
  {
    FirstOrderLinearDS& d = static_cast<FirstOrderLinearDS&> (ds);
    if (dsType == Type::FirstOrderLinearDS)
      d.computeA(t);

    if (d.M())
      W = *d.M();
    else
      W.eye();

    scal(-h * _theta, *d.A(), W, false);
  }
  else RuntimeException::selfThrow("EulerMoreauOSI::computeW - not yet implemented for Dynamical system type :" + dsType);

//  if (_useGamma)
  {
    Topology& topo = *simulationLink->model()->nonSmoothDynamicalSystem()->topology();
    DynamicalSystemsGraph& DSG0 = *topo.dSG(0);
    InteractionsGraph& indexSet = *topo.indexSet(0);
    DynamicalSystemsGraph::OEIterator oei, oeiend;
    InteractionsGraph::VDescriptor ivd;
    SP::SiconosMatrix K;
    SP::Interaction inter;
    for (std11::tie(oei, oeiend) = DSG0.out_edges(dsgVD); oei != oeiend; ++oei)
    {
      inter = DSG0.bundle(*oei);
      ivd = indexSet.descriptor(inter);
      FirstOrderR& rel = static_cast<FirstOrderR&>(*inter->relation());
      K = rel.K();
      if (!K) K = (*indexSet.properties(ivd).workMatrices)[FirstOrderR::mat_K];
      if (K)
      {
        scal(-h * _gamma, *K, W, false);
      }
    }
  }
  // Remark: W is not LU-factorized here.
  // Function PLUForwardBackward will do that if required.
}
Exemplo n.º 23
0
void EventDriven::updateIndexSet(unsigned int i)
{
  assert(_nsds);
  assert(_nsds->topology());
  SP::Topology topo = _nsds->topology();

  assert(i < topo->indexSetsSize() &&
         "EventDriven::updateIndexSet(i), indexSets[i] does not exist.");
  // IndexSets[0] must not be updated in simulation, since it belongs to Topology.
  assert(i > 0  &&
         "EventDriven::updateIndexSet(i=0), indexSets[0] cannot be updated.");

  // For all Interactions in indexSet[i-1], compute y[i-1] and
  // update the indexSet[i].
  SP::InteractionsGraph indexSet1 = topo->indexSet(1);
  SP::InteractionsGraph indexSet2 = topo->indexSet(2);
  assert(_indexSet0);
  assert(indexSet1);
  assert(indexSet2);

  // DEBUG_PRINTF("update indexSets start : indexSet0 size : %ld\n", indexSet0->size());
  // DEBUG_PRINTF("update IndexSets start : indexSet1 size : %ld\n", indexSet1->size());
  // DEBUG_PRINTF("update IndexSets start : indexSet2 size : %ld\n", indexSet2->size());

  InteractionsGraph::VIterator uibegin, uipend, uip;
  std11::tie(uibegin, uipend) = _indexSet0->vertices();
  // loop over all vertices of the indexSet[i-1]
  for (uip = uibegin; uip != uipend; ++uip)
  {
    SP::Interaction inter = _indexSet0->bundle(*uip);
    if (i == 1) // IndexSet[1]
    {
      // if indexSet[1]=>getYRef(0): output y
      // if indexSet[2]=>getYRef(1): output ydot
      double y = inter->getYRef(0); // output to define the IndexSets at this Interaction
      if (y < -_TOL_ED) // y[0] < 0
      {
        inter->display();
        cout << "y = " << y << " < -_TOL_ED =  "   << -_TOL_ED  <<endl;
        RuntimeException::selfThrow("EventDriven::updateIndexSet, output of level 0 must be positive!!! ");
      }
      // 1 - If the Interaction is not yet in the set
      if (!indexSet1->is_vertex(inter)) // Interaction is not yet in the indexSet[i]
      {
        if (fabs(y) <= _TOL_ED)
        {
          // vertex and edges insertions
          indexSet1->copy_vertex(inter, *_indexSet0);
        }
      }
      else // if the Interaction was already in the set
      {
        if (fabs(y) > _TOL_ED)
        {
          indexSet1->remove_vertex(inter); // remove the Interaction from IndexSet[1]
          inter->lambda(1)->zero(); // reset the lambda[1] to zero
        }
      }
    }
    else if (i == 2) // IndexSet[2]
    {
      if (indexSet1->is_vertex(inter)) // Interaction is in the indexSet[1]
      {
        double y = inter->getYRef(1); // output of level 1 at this Interaction
        if (!indexSet2->is_vertex(inter)) // Interaction is not yet in the indexSet[2]
        {
          if (fabs(y) <= _TOL_ED)
          {
            // vertex and edges insertions
            indexSet2->copy_vertex(inter, *_indexSet0);
          }
        }
        else // if the Interaction was already in the set
        {
          if (fabs(y) > _TOL_ED)
          {
            indexSet2->remove_vertex(inter); // remove the Interaction from IndexSet[1]
            inter->lambda(2)->zero(); // reset the lambda[i] to zero
          }
        }
      }
      else // Interaction is not in the indexSet[1]
      {
        if (indexSet2->is_vertex(inter)) // Interaction is in the indexSet[2]
        {
          indexSet2->remove_vertex(inter); // remove the Interaction from IndexSet[2]
          inter->lambda(2)->zero(); // reset the lambda[i] to zero
        }
      }
    }
    else
    {
      RuntimeException::selfThrow("EventDriven::updateIndexSet, IndexSet[i > 2] doesn't exist");
    }
  }

  // DEBUG_PRINTF("update indexSets end : indexSet0 size : %ld\n", indexSet0->size());
  // DEBUG_PRINTF("update IndexSets end : indexSet1 size : %ld\n", indexSet1->size());
  // DEBUG_PRINTF("update IndexSets end : indexSet2 size : %ld\n", indexSet2->size());
}
Exemplo n.º 24
0
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++;
      }
    }

  }
}
Exemplo n.º 25
0
void LsodarOSI::computeFreeOutput(InteractionsGraph::VDescriptor& vertex_inter, OneStepNSProblem* osnsp)
{
  SP::OneStepNSProblems  allOSNS  = simulationLink->oneStepNSProblems();
  SP::InteractionsGraph indexSet = osnsp->simulation()->indexSet(osnsp->indexSetLevel());
  SP::Interaction inter = indexSet->bundle(vertex_inter);

  VectorOfBlockVectors& DSlink = *indexSet->properties(vertex_inter).DSlink;
  // Get relation and non smooth law types
  RELATION::TYPES relationType = inter->relation()->getType();
  RELATION::SUBTYPES relationSubType = inter->relation()->getSubType();
  unsigned int sizeY = inter->nonSmoothLaw()->size();

  unsigned int relativePosition = 0;
  SP::Interaction mainInteraction = inter;
  Index coord(8);
  coord[0] = relativePosition;
  coord[1] = relativePosition + sizeY;
  coord[2] = 0;
  coord[4] = 0;
  coord[6] = 0;
  coord[7] = sizeY;
  SP::SiconosMatrix  C;
  //   SP::SiconosMatrix  D;
  //   SP::SiconosMatrix  F;
  SiconosVector& yForNSsolver = *inter->yForNSsolver();
  SP::BlockVector Xfree;


  // All of these values should be stored in the node corrseponding to the Interactionwhen a MoreauJeanOSI scheme is used.

  /* V.A. 10/10/2010
       * Following the type of OSNS  we need to retrieve the velocity or the acceleration
       * This tricks is not very nice but for the moment the OSNS do not known if
       * it is in accelaration of not
       */

  //SP::OneStepNSProblems  allOSNS  = _simulation->oneStepNSProblems();
  if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp)
  {
    if (relationType == Lagrangian)
    {
      Xfree = DSlink[LagrangianR::xfree];
    }
    // else if  (relationType == NewtonEuler)
    // {
    //   Xfree = inter->data(NewtonEulerR::free);
    // }
    assert(Xfree);
    //        std::cout << "Computeqblock Xfree (Gamma)========" << std::endl;
    //       Xfree->display();
  }
  else  if (((*allOSNS)[SICONOS_OSNSP_ED_IMPACT]).get() == osnsp)
  {
    Xfree = DSlink[LagrangianR::q1];
    //        std::cout << "Computeqblock Xfree (Velocity)========" << std::endl;
    //       Xfree->display();

  }
  else
    RuntimeException::selfThrow(" computeqBlock for Event Event-driven is wrong ");

  if (relationType == Lagrangian)
  {
    C = mainInteraction->relation()->C();
    if (C)
    {
      assert(Xfree);

      coord[3] = C->size(1);
      coord[5] = C->size(1);

      subprod(*C, *Xfree, yForNSsolver, coord, true);
    }

    SP::SiconosMatrix ID(new SimpleMatrix(sizeY, sizeY));
    ID->eye();

    Index xcoord(8);
    xcoord[0] = 0;
    xcoord[1] = sizeY;
    xcoord[2] = 0;
    xcoord[3] = sizeY;
    xcoord[4] = 0;
    xcoord[5] = sizeY;
    xcoord[6] = 0;
    xcoord[7] = sizeY;
    // For the relation of type LagrangianRheonomousR
    if (relationSubType == RheonomousR)
    {
      if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp)
      {
        RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not yet implemented for LCP at acceleration level with LagrangianRheonomousR");
      }
      else if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY]).get() == osnsp)
      {
        SiconosVector q = *DSlink[LagrangianR::q0];
        SiconosVector z = *DSlink[LagrangianR::z];

        std11::static_pointer_cast<LagrangianRheonomousR>(inter->relation())->computehDot(simulation()->getTkp1(), q, z);
        *DSlink[LagrangianR::z] = z;
        subprod(*ID, *(std11::static_pointer_cast<LagrangianRheonomousR>(inter->relation())->hDot()), yForNSsolver, xcoord, false); // y += hDot
      }
      else
        RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not implemented for SICONOS_OSNSP ");
    }
    // For the relation of type LagrangianScleronomousR
    if (relationSubType == ScleronomousR)
    {
      if (((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == osnsp)
      {
        std11::static_pointer_cast<LagrangianScleronomousR>(inter->relation())->computedotjacqhXqdot(simulation()->getTkp1(), *inter, DSlink);
        subprod(*ID, *(std11::static_pointer_cast<LagrangianScleronomousR>(inter->relation())->dotjacqhXqdot()), yForNSsolver, xcoord, false); // y += NonLinearPart
      }
    }
  }
  else
    RuntimeException::selfThrow("LsodarOSI::computeFreeOutput not yet implemented for Relation of type " + relationType);
  if (((*allOSNS)[SICONOS_OSNSP_ED_IMPACT]).get() == osnsp)
  {
    if (inter->relation()->getType() == Lagrangian || inter->relation()->getType() == NewtonEuler)
    {
      SP::SiconosVisitor nslEffectOnFreeOutput(new _NSLEffectOnFreeOutput(osnsp, inter));
      inter->nonSmoothLaw()->accept(*nslEffectOnFreeOutput);
    }
  }

}
Exemplo n.º 26
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(););
Exemplo n.º 27
0
std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor>
Topology::addInteractionInIndexSet0(SP::Interaction inter, SP::DynamicalSystem ds1, SP::DynamicalSystem ds2)
{
  // !! Private function !!
  //
  // Add inter and ds into IG/DSG

  // Compute number of constraints
  unsigned int nsLawSize = inter->nonSmoothLaw()->size();
  unsigned int m = inter->getSizeOfY() / nsLawSize;
  if (m > 1)
    RuntimeException::selfThrow("Topology::addInteractionInIndexSet0 - m > 1. Obsolete !");

  _numberOfConstraints += nsLawSize;

  SP::DynamicalSystem ds2_ = ds2;
  // _DSG is the hyper forest : (vertices : dynamical systems, edges :
  // Interactions)
  //
  // _IG is the hyper graph : (vertices : Interactions, edges :
  // dynamical systems)
  assert(_DSG[0]->edges_number() == _IG[0]->size());

  // _IG = L(_DSG),  L is the line graph transformation
  // vector of the Interaction
  DynamicalSystemsGraph::VDescriptor dsgv1, dsgv2;
  dsgv1 = _DSG[0]->add_vertex(ds1);

  SP::VectorOfVectors workVds1 = _DSG[0]->properties(dsgv1).workVectors;
  SP::VectorOfVectors workVds2;
  if (!workVds1)
  {
    workVds1.reset(new VectorOfVectors());
    _DSG[0]->properties(dsgv1).workMatrices.reset(new VectorOfMatrices());
    ds1->initWorkSpace(*workVds1, *_DSG[0]->properties(dsgv1).workMatrices);
  }
  if(ds2)
  {
    dsgv2 = _DSG[0]->add_vertex(ds2);
    workVds2 = _DSG[0]->properties(dsgv2).workVectors;
    if (!workVds2)
    {
      workVds2.reset(new VectorOfVectors());
      _DSG[0]->properties(dsgv2).workMatrices.reset(new VectorOfMatrices());
      ds2->initWorkSpace(*workVds2, *_DSG[0]->properties(dsgv2).workMatrices);
    }
  }
  else
  {
    dsgv2 = dsgv1;
    ds2_ = ds1;
    workVds2 = workVds1;
  }

  // this may be a multi edges graph
  assert(!_DSG[0]->is_edge(dsgv1, dsgv2, inter));
  assert(!_IG[0]->is_vertex(inter));
  InteractionsGraph::VDescriptor ig_new_ve;
  DynamicalSystemsGraph::EDescriptor new_ed;
  std11::tie(new_ed, ig_new_ve) = _DSG[0]->add_edge(dsgv1, dsgv2, inter, *_IG[0]);
  InteractionProperties& interProp = _IG[0]->properties(ig_new_ve);
  interProp.DSlink.reset(new VectorOfBlockVectors);
  interProp.workVectors.reset(new VectorOfVectors);
  interProp.workMatrices.reset(new VectorOfSMatrices);
  unsigned int nslawSize = inter->nonSmoothLaw()->size();
  interProp.block.reset(new SimpleMatrix(nslawSize, nslawSize));
  inter->setDSLinkAndWorkspace(interProp, *ds1, *workVds1, *ds2_, *workVds2);

  // add self branches in vertex properties
  // note : boost graph SEGFAULT on self branch removal
  // see https://svn.boost.org/trac/boost/ticket/4622
  _IG[0]->properties(ig_new_ve).source = ds1;
  _IG[0]->properties(ig_new_ve).source_pos = 0;
  if(!ds2)
  {
    _IG[0]->properties(ig_new_ve).target = ds1;
    _IG[0]->properties(ig_new_ve).target_pos = 0;
  }
  else
  {
    _IG[0]->properties(ig_new_ve).target = ds2;
    _IG[0]->properties(ig_new_ve).target_pos = ds1->getDim();
  }

  assert(_IG[0]->bundle(ig_new_ve) == inter);
  assert(_IG[0]->is_vertex(inter));
  assert(_DSG[0]->is_edge(dsgv1, dsgv2, inter));
  assert(_DSG[0]->edges_number() == _IG[0]->size());

  return std::pair<DynamicalSystemsGraph::EDescriptor, InteractionsGraph::VDescriptor>(new_ed, ig_new_ve);
}
Exemplo n.º 28
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;
}
Exemplo n.º 29
0
// Fill the SparseMat
void BlockCSRMatrix::fill(SP::InteractionsGraph indexSet)
{
  // ======> Aim: find inter1 and inter2 both in indexSets[level] and which
  // have common DynamicalSystems.  Then get the corresponding matrix
  // from map blocks.

  assert(indexSet);

  // Number of blocks in a row = number of active constraints.
  _nr = indexSet->size();

  // (re)allocate memory for ublas matrix
  _blockCSR->resize(_nr, _nr, false);

  _diagsize0->resize(_nr);
  _diagsize1->resize(_nr);

  // === Loop through "active" Interactions (ie present in
  // indexSets[level]) ===


  int sizeV = 0;

  InteractionsGraph::VIterator vi, viend;
  for (std11::tie(vi, viend) = indexSet->vertices();
       vi != viend; ++vi)
  {
    SP::Interaction inter = indexSet->bundle(*vi);

    assert(inter->nonSmoothLaw()->size() > 0);

    sizeV  += inter->nonSmoothLaw()->size();
    (*_diagsize0)[indexSet->index(*vi)] = sizeV;
    (*_diagsize1)[indexSet->index(*vi)] = sizeV;
    assert((*_diagsize0)[indexSet->index(*vi)] > 0);
    assert((*_diagsize1)[indexSet->index(*vi)] > 0);

    (*_blockCSR)(indexSet->index(*vi), indexSet->index(*vi)) =
      indexSet->properties(*vi).block->getArray();
  }

  InteractionsGraph::EIterator ei, eiend;
  for (std11::tie(ei, eiend) = indexSet->edges();
       ei != eiend; ++ei)
  {
    InteractionsGraph::VDescriptor vd1 = indexSet->source(*ei);
    InteractionsGraph::VDescriptor vd2 = indexSet->target(*ei);
    SP::Interaction inter1 = indexSet->bundle(vd1);
    SP::Interaction inter2 = indexSet->bundle(vd2);

    assert(indexSet->index(vd1) < _nr);
    assert(indexSet->index(vd2) < _nr);

    assert(indexSet->is_vertex(inter2));

    assert(vd2 == indexSet->descriptor(inter2));
    assert(indexSet->index(vd2) == indexSet->index(indexSet->descriptor(inter2)));


    unsigned int pos = indexSet->index(vd1);
    unsigned int col = indexSet->index(vd2);

    assert(pos != col);

    (*_blockCSR)(std::min(pos, col), std::max(pos, col)) =
      indexSet->properties(*ei).upper_block->getArray();

    (*_blockCSR)(std::max(pos, col), std::min(pos, col)) =
      indexSet->properties(*ei).lower_block->getArray();
  }
  DEBUG_EXPR(display(););
Exemplo n.º 30
0
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++)
    // {
    //   relationOfBeads[i].reset(new LagrangianLinearTIR(HOfBeads,bOfBeads));
    //   interOfBeads[i].reset(new Interaction(1, nslaw, relationOfBeads[i]));
    // }


    // --------------------------------------
    // ---      Model and simulation      ---
    // --------------------------------------
    SP::Model columnOfBeads(new Model(t0, T));
    // --  (1) OneStepIntegrators --
    SP::MoreauJeanOSI OSI(new MoreauJeanOSI(theta));

    // add the dynamical system in the non smooth dynamical system
    for (unsigned int i = 0; i < nBeads; i++)
    {
      columnOfBeads->nonSmoothDynamicalSystem()->insertDynamicalSystem(beads[i]);
    }

    // // link the interaction and the dynamical system
    // for (unsigned int i =0; i< nBeads-1; i++)
    // {
    //   columnOfBeads->nonSmoothDynamicalSystem()->link(interOfBeads[i],beads[i]);
    //   columnOfBeads->nonSmoothDynamicalSystem()->link(interOfBeads[i],beads[i+1]);
    // }


    // -- (2) Time discretisation --
    SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));

    // -- (3) one step non smooth problem
    SP::OneStepNSProblem osnspb(new LCP());

    // -- (4) Simulation setup with (1) (2) (3)
    SP::TimeStepping s(new TimeStepping(t, OSI, osnspb));

    columnOfBeads->setSimulation(s);

    // =========================== End of model definition ===========================

    // ================================= Computation =================================

    // --- Simulation initialization ---

    cout << "====> Initialisation ..." << endl << endl;
    columnOfBeads->initialize();

    int N = ceil((T - t0) / h); // Number of time steps

    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 1 + nBeads * 4;
    SimpleMatrix dataPlot(N + 1, outputSize);

    dataPlot(0, 0) = columnOfBeads->t0();

    for (unsigned int i = 0; i < nBeads; i++)
    {
      dataPlot(0, 1 + i * 2) = (beads[i]->q())->getValue(0);
      dataPlot(0, 2 + i * 2) = (beads[i]->velocity())->getValue(0);
      //      dataPlot(0,3+i*4) = (beads[i]->p(1))->getValue(0);
    }

    // for (unsigned int i =1; i< nBeads; i++)
    // {
    // dataPlot(0,4+i*4) = (interOfBeads[i-1]->lambda(1))->getValue(0);
    // }

    // --- Time loop ---
    cout << "====> Start computation ... " << endl << endl;
    // ==== Simulation loop - Writing without explicit event handling =====
    int k = 1;
    boost::progress_display show_progress(N);

    boost::timer time;
    time.restart();
    int ncontact = 0 ;
    bool isOSNSinitialized = false;
    while (s->hasNextEvent())
    {
      // Rough contact detection
      for (unsigned int i = 0; i < nBeads - 1; i++)
      {
        // Between first bead and plane
        if (abs(((beads[i])->q())->getValue(0) - R) < alert)
        {
          if (!inter)
          {
            ncontact++;
            // std::cout << "Number of contact = " << ncontact << std::endl;

            inter.reset(new Interaction(1, nslaw, relation));
            columnOfBeads->nonSmoothDynamicalSystem()->link(inter, beads[0]);
            s->initializeInteraction(s->nextTime(), inter);

            if (!isOSNSinitialized)
            {
              s->initOSNS();
              isOSNSinitialized = true;
            }

            assert(inter->y(0)->getValue(0) >= 0);
          }
        }

        // Between two beads
        if (abs(((beads[i + 1])->q())->getValue(0) - ((beads[i])->q())->getValue(0) - 2 * R) < alert)
        {
          //std::cout << "Alert distance for declaring contact = ";
          //std::cout << abs(((beads[i])->q())->getValue(0)-((beads[i+1])->q())->getValue(0))   <<std::endl;
          if (!interOfBeads[i].get())
          {
            ncontact++;
            // std::cout << "Number of contact = " << ncontact << std::endl;

            relationOfBeads[i].reset(new LagrangianLinearTIR(HOfBeads, bOfBeads));
            interOfBeads[i].reset(new Interaction(1, nslaw, relationOfBeads[i]));

            columnOfBeads->nonSmoothDynamicalSystem()->link(interOfBeads[i], beads[i], beads[i+1]);
            s->initializeInteraction(s->nextTime(), interOfBeads[i]);

            if (!isOSNSinitialized)
            {
              s->initOSNS();
              isOSNSinitialized = true;
            }

            assert(interOfBeads[i]->y(0)->getValue(0) >= 0);
          }
        }
      }

      s->computeOneStep();

      // --- Get values to be plotted ---
      dataPlot(k, 0) =  s->nextTime();
      for (unsigned int i = 0; i < nBeads; i++)
      {
        dataPlot(k, 1 + i * 2) = (beads[i]->q())->getValue(0);
        dataPlot(k, 2 + i * 2) = (beads[i]->velocity())->getValue(0);
      }
      // for (unsigned int i =1; i< nBeads; i++)
      // {
      //   dataPlot(k,4+i*4) = (interOfBeads[i-1]->lambda(1))->getValue(0);
      // }
      // for (unsigned int i =1; i< nBeads; i++)
      // {
      //   std::cout <<  (interOfBeads[i-1]->y(0))->getValue(0) << std::endl ;
      // }

      s->nextStep();
      ++show_progress;
      k++;
    }
    cout << endl << "End of computation - Number of iterations done: " << k - 1 << endl;
    cout << "Computation Time " << time.elapsed()  << endl;

    // --- Output files ---
    cout << "====> Output file writing ..." << endl;
    dataPlot.resize(k, outputSize);
    ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");
    // Comparison with a reference file
    SimpleMatrix dataPlotRef(dataPlot);
    dataPlotRef.zero();

    ioMatrix::read("result.ref", "ascii", dataPlotRef);

    cout << "====> Comparison with reference file ..." << endl;
    std::cout << "Error w.r.t. reference file : " << (dataPlot - dataPlotRef).normInf() << std::endl;
    if ((dataPlot - dataPlotRef).normInf() > 1e-12)
    {
      std::cout << "Warning. The result is rather different from the reference file." << std::endl;
      return 1;
    }

  }

  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught in ColumnOfBeadsTS.cpp" << endl;
  }



}