//==========================================================================================================
//                                             Main function
//==========================================================================================================
int main(int argc, char* argv[])
{
  //---------------------------- calculate the computation time --------------------------------------------------
  boost::timer time;
  time.restart();
  try
  {
    //===========================================================================================================
    //                  I: Declare the dynamical systems
    //===========================================================================================================
    //1. Set the mass matrix
    SP::SiconosMatrix Mass(new SimpleMatrix(Nfreedom, Nfreedom));
    double InertiaBlock;
    InertiaBlock = (MassBlock / 12.0) * ((HeightBlock*HeightBlock) + (LengthBlock*LengthBlock)); // moment of inertia
    (*Mass)(0, 0) = MassBlock;
    (*Mass)(1, 1) = MassBlock;
    (*Mass)(2, 2) = InertiaBlock;
    //2. Set the initial position of the block in function of the initial position of the contact point A (left-hand contact)
    SP::SiconosVector PosIniBlock(new SiconosVector(Nfreedom));
    (*PosIniBlock)(0) = PosXiniPointA + 0.5 * LengthBlock * cos(AngleThetaIni) - 0.5 * HeightBlock * sin(AngleThetaIni);
    (*PosIniBlock)(1) = PosYiniPointA + 0.5 * LengthBlock * sin(AngleThetaIni) + 0.5 * HeightBlock * cos(AngleThetaIni);
    (*PosIniBlock)(2) = AngleThetaIni;
    cout.precision(15);
    cout << "x0: " << (*PosIniBlock)(0) << endl;
    cout << "y0: " << (*PosIniBlock)(1) << endl;
    cout << "theta0: " << (*PosIniBlock)(2) << endl;
    cout.precision(15);
    cout << "PI: " << PI << endl;
    // (*PosIniBlock)(0) = 0.5;
    // (*PosIniBlock)(1) = 0.5;
    // (*PosIniBlock)(2) = 0.0;

    //3. Set the initial velocity of the block in function of the initial relative velocity of the contact point A
    SP::SiconosVector VelIniBlock(new SiconosVector(Nfreedom));
    (*VelIniBlock)(0) = VelXiniPointA - (0.5 * LengthBlock * sin(AngleThetaIni) + 0.5 * HeightBlock * cos(AngleThetaIni)) * RotVelBlockIni;
    (*VelIniBlock)(1) = VelYiniPointA + (0.5 * LengthBlock * cos(AngleThetaIni) - 0.5 * HeightBlock * sin(AngleThetaIni)) * RotVelBlockIni;
    (*VelIniBlock)(2) = RotVelBlockIni;

    // (*VelIniBlock)(0) = 0.0;
    // (*VelIniBlock)(1) = 0.0;
    // (*VelIniBlock)(2) = 0.0;

    //4. Instantiate the object of "LagrangianTIDS"
    SP::LagrangianLinearTIDS RockingBlock(new LagrangianLinearTIDS(PosIniBlock, VelIniBlock, Mass));
    //5. Set the external force
    SP::SiconosVector ForceExtern(new SiconosVector(Nfreedom));
    (*ForceExtern)(1) = -MassBlock * GGearth;
    RockingBlock->setFExtPtr(ForceExtern);
    //
    //----------------------------- Display variables of the dynamical system---------------------------------------
    cout << "Initial position of the rocking block:" << endl;
    PosIniBlock->display();
    cout << "Initial velocity of the rocking block:" << endl;
    VelIniBlock->display();
    cout << "Mass matrix of the rocking block:" << endl;
    Mass->display();
    cout << "External force applied on the rocking block:"  << endl;
    ForceExtern->display();
    //==================================================================================================================
    //              II: Declare the relation et interaction between dynamical systems
    //==================================================================================================================
    //
    /*
    SP::SiconosMatrix H(new SimpleMatrix(1,Nfreedom));
    (*H)(0,1) = 1.0;
    SP::SiconosVector E(new SiconosVector(1));
    (*E)(0) = -0.5*HeightBlock;
    */
    // Impact law
    SP::NonSmoothLaw nslaw(new NewtonImpactNSL(e));
    // Interaction at contact point 1
    //SP::Relation relation1(new LagrangianLinearTIR(H, E));
    SP::Relation relation1(new LagrangianScleronomousR("RockingBlockPlugin:h1", "RockingBlockPlugin:G1", "RockingBlockPlugin:G1dot"));
    SP::Interaction inter1(new Interaction(1, nslaw, relation1));
    // Interaction at contact point 2
    //SP::Relation relation2(new LagrangianLinearTIR(H, E));
    SP::Relation relation2(new LagrangianScleronomousR("RockingBlockPlugin:h2", "RockingBlockPlugin:G2", "RockingBlockPlugin:G2dot"));
    SP::Interaction inter2(new Interaction(1, nslaw, relation2));
    // Interactions for the whole dynamical system
    //================================================================================================================
    //            III. Create the "model" object
    //================================================================================================================
    SP::Model RoBlockModel(new Model(TimeInitial, TimeFinal));
    RoBlockModel->nonSmoothDynamicalSystem()->insertDynamicalSystem(RockingBlock);
    RoBlockModel->nonSmoothDynamicalSystem()->link(inter1, RockingBlock);
    RoBlockModel->nonSmoothDynamicalSystem()->link(inter2, RockingBlock);
    //================================================================================================================
    //            IV. Create the simulation
    //================================================================================================================
    //1. Time discretization
    SP::TimeDiscretisation TimeDiscret(new TimeDiscretisation(TimeInitial, StepSize));
    //2. Integration solver for one step
    SP::OneStepIntegrator OSI(new NewMarkAlphaOSI(_rho, IsHandleVelConstraint));
    SP::NewMarkAlphaOSI _NewMarkAlpha = std11::static_pointer_cast<NewMarkAlphaOSI>(OSI);
    //3. Nonsmooth problem
    SP::OneStepNSProblem impact(new LCP());
    SP::OneStepNSProblem position(new LCP());
    SP::OneStepNSProblem acceleration(new LCP());

    position->numericsSolverOptions()->dparam[0] = 1e-12;
    impact->numericsSolverOptions()->dparam[0] = 1e-12;
    acceleration->numericsSolverOptions()->dparam[0] = 1e-12;
   

    //4. Simulation with (1), (2), (3)
    SP::Simulation EDscheme(new EventDriven(TimeDiscret));
    EDscheme->insertIntegrator(OSI);
    EDscheme->insertNonSmoothProblem(impact, SICONOS_OSNSP_ED_IMPACT);
    EDscheme->insertNonSmoothProblem(acceleration, SICONOS_OSNSP_ED_SMOOTH_ACC);
    EDscheme->insertNonSmoothProblem(position, SICONOS_OSNSP_ED_SMOOTH_POS);
    RoBlockModel->setSimulation(EDscheme); // initialize the model
    // bool check1 = EDscheme->hasOneStepNSProblem(impact);
    // bool check2 = EDscheme->hasOneStepNSProblem(acceleration);
    // cout << "Impact law included in the simulation: " << check1 << endl;
    // cout << "LCP at acceleration level included in the simulation: " << check2 << endl;
    //==================================================================================================================
    //                    V. Process the simulation
    //==================================================================================================================
    // -------------------------------- Simulation initialization ------------------------------------------------------
    cout << "====> Simulation initialisation ..." << endl << endl;
    RoBlockModel->initialize(); // initialize the model
    EDscheme->setPrintStat(true);
    SP::EventsManager eventsManager = EDscheme->eventsManager(); // ponters point to the "eventsManager" object
    SP::SiconosVector PosBlock = RockingBlock->q();              // pointer points to the position vector of the rocking block
    SP::SiconosVector VelBlock = RockingBlock->velocity();       // pointer points to the velocity of the rocking block
    SP::SiconosVector AcceBlock = RockingBlock->acceleration();       // pointer points to the velocity of the rocking block
    SP::SiconosVector GapCon1 = inter1->y(0);
    SP::SiconosVector GapCon2 = inter2->y(0);
    SP::SiconosVector VelCon1 = inter1->y(1);
    SP::SiconosVector VelCon2 = inter2->y(1);
    SP::SiconosVector LambdaCon1 = inter1->lambda(2);
    SP::SiconosVector LambdaCon2 = inter2->lambda(2);
    SP::InteractionsGraph indexSet0 = RoBlockModel->nonSmoothDynamicalSystem()->topology()->indexSet(0);
    SP::InteractionsGraph indexSet1 = RoBlockModel->nonSmoothDynamicalSystem()->topology()->indexSet(1);
    SP::InteractionsGraph indexSet2 = RoBlockModel->nonSmoothDynamicalSystem()->topology()->indexSet(2);
    cout << "Size of IndexSet0: " << indexSet0->size() << endl;
    cout << "Size of IndexSet1: " << indexSet1->size() << endl;
    cout << "Size of IndexSet2: " << indexSet2->size() << endl;

    InteractionsGraph::VIterator ui, uiend;
    //-------------------- Save the output during simulation ---------------------------------------------------------
    SimpleMatrix DataPlot(NpointSave, SizeOutput);
    //------------- At the initial time -----------------------------------------------------------------------------
    DataPlot(0, 0) = RoBlockModel->t0();
    DataPlot(0, 1) = (*PosBlock)(0); // Position X
    DataPlot(0, 2) = (*PosBlock)(1); // Position Y
    DataPlot(0, 3) = (*PosBlock)(2); // Angle theta
    DataPlot(0, 4) = (*VelBlock)(0); // Velocity Vx
    DataPlot(0, 5) = (*VelBlock)(1); // Velocity Vy
    DataPlot(0, 6) = (*VelBlock)(2); // Angular velocity
    DataPlot(0, 7) = (*GapCon1)(0);  // Gap at first contact
    DataPlot(0, 8) = (*GapCon2)(0);  // Gap at second contact
    DataPlot(0, 9) = (*VelCon1)(0);  // Relative velocity at first contact
    DataPlot(0, 10) = (*VelCon2)(0);  // Relative velocity at second contact
    DataPlot(0, 11) = (*LambdaCon1)(0); // Force at first contact
    DataPlot(0, 12) = (*LambdaCon2)(0); // Force at second contact
    //----------------------------------- Simulation starts ----------------------------------------------------------
    cout << "====> Start computation ... " << endl << endl;
    bool NSEvent = false;
    unsigned int NumberNSEvent = 0;
    double alpha_m, alpha_f, beta, gamma;
    unsigned int k = 1;
    boost::progress_display show_progress(NpointSave);
    while (EDscheme->hasNextEvent() && (k < NpointSave))
    {
      if (IsTreatFirstSteps)
      {
        if (k == 1) // first step
        {
          alpha_m = 0.0;
          alpha_f = 0.0;
          gamma = 1.0/2.0 + 1.0/10.0;
          beta = 1.0/4.0 * (gamma + 1.0/2.0) *(gamma + 1.0/2.0)  ;
          _NewMarkAlpha->setAlpha_m(alpha_m);
          _NewMarkAlpha->setAlpha_f(alpha_f);
          _NewMarkAlpha->setBeta(beta);
          _NewMarkAlpha->setGamma(gamma);
        }
        else if (k == 2)
        {
          alpha_m = 0.0;
          alpha_f = -1.0 / 3.0; // -1/3 <= alpha_f <= 0
          gamma = 1.0/2.0 - alpha_f;
          beta = 1.0/4.0 * (1.0 - alpha_f)*(1.0 - alpha_f);
          _NewMarkAlpha->setAlpha_m(alpha_m);
          _NewMarkAlpha->setAlpha_f(alpha_f);
          _NewMarkAlpha->setBeta(beta);
          _NewMarkAlpha->setGamma(gamma);
        }
        else
        {
          _NewMarkAlpha->setParametersFromRho_infty(_rho);
        }
      }
      EDscheme->advanceToEvent(); // lead the simulation run from one event to the next
      //---------- detect the statue of the current event ------------------------------------
      if (eventsManager->nextEvent()->getType() == 2) // the current event is non-smooth
      {
        NSEvent = true;
      };
      EDscheme->processEvents();  // process the current event
      //------------------- get data at the beginning of non-smooth events ---------------------------
      if (NSEvent)
      {
        DataPlot(k, 0) = EDscheme->startingTime(); // instant at non-smooth event
        DataPlot(k, 1) = (*(RockingBlock->qMemory()->getSiconosVector(1)))(0);      // Position X
        DataPlot(k, 2) = (*(RockingBlock->qMemory()->getSiconosVector(1)))(1);      // Position Y
        DataPlot(k, 3) = (*(RockingBlock->qMemory()->getSiconosVector(1)))(2);      // Angle theta
        DataPlot(k, 4) = (*(RockingBlock->velocityMemory()->getSiconosVector(1)))(0); // Velocity Vx
        DataPlot(k, 5) = (*(RockingBlock->velocityMemory()->getSiconosVector(1)))(1); // Velocity Vy
        DataPlot(k, 6) = (*(RockingBlock->velocityMemory()->getSiconosVector(1)))(2); // Angular velocity
        //EDscheme->update(1);
        k++;
        ++NumberNSEvent;
        ++show_progress;
        NSEvent = false;                        // The next event is maybe smooth
      };
      //-------------------- get data at smooth events or at the end of non-smooth events ---------------
      DataPlot(k, 0) = EDscheme->startingTime();
      DataPlot(k, 1) = (*PosBlock)(0); //Position X
      DataPlot(k, 2) = (*PosBlock)(1); //Position Y
      DataPlot(k, 3) = (*PosBlock)(2); // Position theta
      DataPlot(k, 4) = (*VelBlock)(0); // Velocity Vx
      DataPlot(k, 5) = (*VelBlock)(1); // Velocity Vy
      DataPlot(k, 6) = (*VelBlock)(2); // Velocity Vtheta
      DataPlot(k, 7) = (*GapCon1)(0);  // Gap at first contact
      DataPlot(k, 8) = (*GapCon2)(0);  // Gap at second contact
      DataPlot(k, 9) = (*VelCon1)(0);  // Relative velocity at first contact
      DataPlot(k, 10) = (*VelCon2)(0);  // Relative velocity at second contact
      DataPlot(k, 11) = (*LambdaCon1)(0); // Force at first contact
      DataPlot(k, 12) = (*LambdaCon2)(0); // Force at second contact
      // go to the next time step
      k++;
      ++show_progress;
      // // Display information
      // cout << "********At the end of integation step***************"<< (k - 1) << endl;
      // cout << "Information on Dynamical System" << endl;
      // cout << "Position: ";
      // PosBlock->display();
      // cout << "Velocity: ";
      // VelBlock->display();
      // cout << "Acceleration: ";
      // AcceBlock->display();
      // cout << "Information on contacts" << endl;
      // for(std11::tie(ui,uiend) = indexSet0->vertices(); ui!=uiend; ++ui)
      //   {
      //     SP::Interaction inter = indexSet0->bundle(*ui);
      //     cout << "Contact number: " << inter->number() << endl;
      //     cout << "Contact gap: ";
      //     inter->y(0)->display();
      //     cout << "Contact relative velocity: ";
      //     inter->y(1)->display();
      //     cout << "Contact Force: " << endl;
      //     inter->lambda(2)->display();
      //   }
    };
    //----------------------- At the end of the simulation --------------------------
    cout << "End of the simulation" << endl;
    cout << "Number of events processed during simulation: " << (k + 1) << endl;
    cout << "Number of non-smooth events: " << NumberNSEvent << endl;
    cout << "====> Output file writing ..." << endl << endl;
    ioMatrix::write("result.dat", "ascii", DataPlot, "noDim");
    // Comparison with a reference file
    std::cout << "Comparison with a reference file" << std::endl;
    SimpleMatrix dataPlotRef(DataPlot);
    dataPlotRef.zero();
    ioMatrix::read("resultED_NewMarkAlpha.ref", "ascii", dataPlotRef);
    double error = (DataPlot - dataPlotRef).normInf()/ dataPlotRef.normInf();
    std::cout << "Error = "<< error << std::endl;
    std::cout << "Error by column = "<< std::endl;

    SP::SiconosVector errCol(new SiconosVector(SizeOutput));
    (DataPlot - dataPlotRef).normInfByColumn(errCol);
    errCol->display();
    
    if (error > 1e-06)
    {
      std::cout << "Warning. The results is rather different from the reference file." << std::endl;
      std::cout << (DataPlot - dataPlotRef).normInf()/ dataPlotRef.normInf() << std::endl;
      
      return 1;
    }
  }
  //============================== Catch exceptions ===================================================================
  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught." << endl;
  }
  cout << "Computation Time: " << time.elapsed()  << endl;
}
int main(int argc, char* argv[])
{
  boost::timer time;
  time.restart();
  try
  {
    // ================= Creation of the model =======================

    // User-defined main parameters
    unsigned int nDofBall = 1;            // degrees of freedom of ball 1
    double Height = 0.05;         // Distance between impactor balls and monodisperse balls
    // Balls in the tapered chain
    unsigned int NumberBalls  = 10;            // Number
    double R_base_taper = 0.005;         // Base radii of the tapered chain
    double q_taper = 0.0;               // Tapering factor
    // Material properties of balls
    double mass_density = 7780;           // mass density
    double Res_BallBall = 1.0;          // Restitution coefficient at contacts ball-ball
    double Res_BallWall = 1.0;          // Restitution coefficient at contacts ball-wall
    double PowCompLaw = 1.5;              // Power of the compliance law: 1.0 for linear contact and 3/2 for the Hertzian contact
    string TypeContactLaw = "BiStiffness"; // Type of compliance contact law
    // Parameters for the global simulation
    double t0 = 0;                         // initial computation time
    double T = 0.5;                        // final computation time
    double h = 0.001;                      // time step
    unsigned int Npointsave = 500;        // Number of data points to be saved
    //---------------------------------------
    // ---- Configuration of chaines
    //--------------------------------------
    //************* Balls ******************
    double NumberContacts = NumberBalls ; // Number of contacts
    //(1) Radius of balls
    SP::SiconosVector RadiusBalls(new SiconosVector(NumberBalls));
    for (unsigned int k = 0; k < NumberBalls; ++k)
    {
      (*RadiusBalls)(k) = (pow(double(1.0 - q_taper), int(k + 1))) * R_base_taper;
    }
    // (2) Mass of balls
    SP::SiconosVector MassBalls(new SiconosVector(NumberBalls));
    for (unsigned int id = 0; id < NumberBalls; ++id)
    {
      (*MassBalls)(id) = (4.0 / 3.0) * PI * pow((*RadiusBalls)(id), 3) * mass_density;
    }
    // (3) Initial position of balls
    // For the impactor balls
    SP::SiconosVector InitPosBalls(new SiconosVector(NumberBalls));
    (*InitPosBalls)(0) = Height + (*RadiusBalls)(0);
    for (unsigned int j = 1; j < NumberBalls; ++j)
    {
      (*InitPosBalls)(j) = (*InitPosBalls)(j - 1) + (*RadiusBalls)(j - 1) + (*RadiusBalls)(j);
    }
    // (4) Initial velocity of balls
    SP::SiconosVector InitVelBalls(new SiconosVector(NumberBalls));
    for (unsigned int i = 0; i < NumberBalls; ++i)
    {
      (*InitVelBalls)(i) = 0.0;
    }
    //****************** Contacts ******************
    // (1) Restitution coefficient at contacts
    SP::SiconosVector ResCofContacts(new SiconosVector(NumberContacts));
    SP::SiconosVector ElasCofContacts(new SiconosVector(NumberContacts));
    for (unsigned int id = 0; id < NumberContacts; ++id)
    {
      if (id == 0) // contact ball-wall
      {
        (*ResCofContacts)(id) = Res_BallWall;
      }
      else  // contact ball-ball
      {
        (*ResCofContacts)(id) = Res_BallBall;
      }
    }

    // // Display and save the configuration of the chain simulated
    // cout << "Configuation of ball chains" << endl;
    // cout.precision(15);
    // cout << "Radius of balls: " << endl;
    // RadiusBalls->display();
    // cout << "Mass of balls: " << endl;
    // MassBalls->display();
    // cout << "Initial position of balls: " << endl;
    // InitPosBalls->display();
    // cout << "Initial velocity of balls: " << endl;
    // InitVelBalls->display();
    // cout << "Restitution coefficient at contacts:" << endl;
    // ResCofContacts->display();
    // -------------------------
    // --- Dynamical systems ---
    // -------------------------
    cout << "====> Model loading ..." << endl << endl;
    std::vector<SP::DynamicalSystem> VecOfallDS;
    SP::SiconosMatrix MassBall;
    SP::SiconosVector q0Ball;
    SP::SiconosVector v0Ball;
    SP::LagrangianLinearTIDS ball;
    SP::SiconosVector FextBall;
    double _Rball, _massBall, _Pos0Ball, _Vel0Ball ;
    // -------------
    // --- Model ---
    // -------------
    SP::Model BallChain(new Model(t0, T));
    // ----------------
    // --- Simulation ---
    // ----------------
    // -- (1) OneStepIntegrators --
    SP::OneStepIntegrator OSI(new LsodarOSI());

    for (unsigned int i = 0; i < NumberBalls; ++i)
    {
      _Rball = (*RadiusBalls)(i); // radius of the ball
      _massBall = (*MassBalls)(i); // mass of the ball
      _Pos0Ball = (*InitPosBalls)(i); // initial position of the ball
      _Vel0Ball = (*InitVelBalls)(i); // initial velocity of the ball
      // Declaration of the DS in Siconos
      MassBall =  SP::SiconosMatrix(new SimpleMatrix(nDofBall, nDofBall));
      (*MassBall)(0, 0) = _massBall;
      // -- Initial positions and velocities --
      q0Ball = SP::SiconosVector(new SiconosVector(nDofBall));
      v0Ball = SP::SiconosVector(new SiconosVector(nDofBall));
      (*q0Ball)(0) = _Pos0Ball;
      (*v0Ball)(0) = _Vel0Ball;
      // -- The dynamical system --
      ball = SP::LagrangianLinearTIDS(new LagrangianLinearTIDS(q0Ball, v0Ball, MassBall));
      // -- Set external forces (weight1) --
      FextBall = SP::SiconosVector(new SiconosVector(nDofBall));
      (*FextBall)(0) = -_massBall * g;
      ball->setFExtPtr(FextBall);
      //
      VecOfallDS.push_back(ball);
      BallChain->nonSmoothDynamicalSystem()->insertDynamicalSystem(ball);

    }
    // --------------------
    // --- Interactions ---
    // --------------------
    SP::SimpleMatrix H;
    SP::SiconosVector E;
    SP::NonSmoothLaw  nslaw;
    SP::Relation relation;
    SP::Interaction interaction;
    double ResCoef, Stiff, ElasPow;
    for (unsigned int j = 0; j < NumberContacts; ++j)
    {
      ResCoef = (*ResCofContacts)(j) ;
      if (j == 0) // for contact wall-ball
      {
        H.reset(new SimpleMatrix(1, nDofBall));
        (*H)(0, 0) = 1.0;
        E = SP::SiconosVector(new SiconosVector(1));
        (*E)(0) = -(*RadiusBalls)(j);
        
      }
      else // For ball-ball contact
      {
        H.reset(new SimpleMatrix(1, (nDofBall + nDofBall)));
        (*H)(0, 0) = -1.0;
        (*H)(0, 1) = 1.0;
        E = SP::SiconosVector(new SiconosVector(1));
        (*E)(0) = -1.0 * ((*RadiusBalls)(j - 1) + (*RadiusBalls)(j));
      }
      //
      nslaw = SP::NonSmoothLaw(new NewtonImpactNSL(ResCoef));
      relation = SP::Relation(new LagrangianLinearTIR(H, E));
      interaction = SP::Interaction(new Interaction(1, nslaw, relation));
      if (j == 0) // for contact wall-ball
        BallChain->nonSmoothDynamicalSystem()->link(interaction, VecOfallDS[j]);
      else // For ball-ball contact
        BallChain->nonSmoothDynamicalSystem()->link(interaction, VecOfallDS[j-1],VecOfallDS[j]);
    }

    // -- (2) Time discretisation --
    SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));
    // -- (3) Non smooth problem --
    SP::OneStepNSProblem impact(new LCP());
    SP::OneStepNSProblem acceleration(new LCP());
    // -- (4) Simulation setup with (1) (2) (3)
    SP::EventDriven s(new EventDriven(t));
    s->insertIntegrator(OSI);
    s->insertNonSmoothProblem(impact, SICONOS_OSNSP_ED_IMPACT);
    s->insertNonSmoothProblem(acceleration, SICONOS_OSNSP_ED_SMOOTH_ACC);
    BallChain->setSimulation(s);

    // =========================== End of model definition ===========================
    //----------------------------------- Initialization-------------------------------
    s->setPrintStat(true);
    BallChain->initialize();
    SP::DynamicalSystemsGraph DSG0 = BallChain->nonSmoothDynamicalSystem()->topology()->dSG(0);
    SP::InteractionsGraph IndexSet0 = BallChain->nonSmoothDynamicalSystem()->topology()->indexSet(0);
    SP::InteractionsGraph IndexSet1 = BallChain->nonSmoothDynamicalSystem()->topology()->indexSet(1);
    SP::InteractionsGraph IndexSet2 = BallChain->nonSmoothDynamicalSystem()->topology()->indexSet(2);
    // // Display topology of the system
    // cout << "Number of vectices of IndexSet0: " << IndexSet0->size() << endl;
    // cout << "Number of vectices of IndexSet1: " << IndexSet1->size() << endl;
    // cout << "Number of vectices of IndexSet2: " << IndexSet2->size() << endl;
    // cout << "Number of vectices of DSG0: " << DSG0->size() << endl;
    //
    SP::EventsManager eventsManager = s->eventsManager();
    boost::progress_display show_progress(Npointsave);
    // ================================= Computation =================================
    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 2 * NumberBalls + 1;
    SimpleMatrix dataPlot(Npointsave, outputSize);

    // --- Time loop ---
    cout << "====> Start computation ... " << endl << endl;
    // ==== Simulation loop - Writing without explicit event handling =====
    bool nonSmooth = false;
    unsigned int NumberOfEvents = 0;
    unsigned int NumberOfNSEvents = 0;
    unsigned int k = 0;
    DynamicalSystemsGraph::VIterator ui, uiend;
    //====================================================================
    while ((k < Npointsave) & (s->hasNextEvent()))
    {
      dataPlot(k, 0) =  s->startingTime();
      // Save state of the balls
      unsigned int col_pos = 1;
      unsigned int col_vel = NumberBalls + 1;
      for (boost::tie(ui, uiend) = DSG0->vertices(); ui != uiend; ++ui)
      {
        SP::DynamicalSystem ds = DSG0->bundle(*ui);
        SP::LagrangianDS lag_ds = std11::dynamic_pointer_cast<LagrangianDS>(ds);
        SP::SiconosVector q = lag_ds->q();
        SP::SiconosVector v = lag_ds->velocity();
        dataPlot(k, col_pos) = (*q)(0);
        dataPlot(k, col_vel) = (*v)(0);
        col_pos++;
        col_vel++;
      }
      ++k;
      s->advanceToEvent(); // run simulation from one event to the next
      if (eventsManager->nextEvent()->getType() == 2)
      {
        nonSmooth = true;
      };
      //
      s->processEvents();  // process events
      if (nonSmooth)
      {
        dataPlot(k, 0) = s->startingTime();
        // Save state of the balls
        unsigned int col_pos = 1;
        unsigned int col_vel = NumberBalls + 1;
        for (boost::tie(ui, uiend) = DSG0->vertices(); ui != uiend; ++ui)
        {
          SP::DynamicalSystem ds = DSG0->bundle(*ui);
          SP::LagrangianDS lag_ds = std11::dynamic_pointer_cast<LagrangianDS>(ds);
          SP::SiconosVector q = lag_ds->qMemory()->getSiconosVector(1);
          SP::SiconosVector v = lag_ds->velocityMemory()->getSiconosVector(1);
          dataPlot(k, col_pos) = (*q)(0);
          dataPlot(k, col_vel) = (*v)(0);
          col_pos++;
          col_vel++;
        }
        nonSmooth = false;
        ++NumberOfNSEvents;
        ++NumberOfEvents;
        ++show_progress;
        ++k;
      }
      // --- Get values to be plotted ---
      ++NumberOfEvents;
      ++show_progress;
    }

    cout << "Computation Time " << time.elapsed()  << endl;
    // --- Output files ---
    cout << "====> Output file writing ..." << endl;
    ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");
    // Comparison with a reference file
    SimpleMatrix dataPlotRef(dataPlot);
    dataPlotRef.zero();
    ioMatrix::read("resultED_NewtonModel.ref", "ascii", dataPlotRef);

    if ((dataPlot - dataPlotRef).normInf() > 1e-12)
    {
      std::cout << "Warning. The results is rather different from the reference file." << std::endl;
      return 1;
    }
  }
  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught." << endl;
  }
  cout << "Computation Time: " << time.elapsed()  << endl;
}
Esempio n. 3
0
int main(int argc, char* argv[])
{
    try
    {

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

        // User-defined main parameters
        unsigned int nDof = 3;           // degrees of freedom for robot arm
        double t0 = 0;                   // initial computation time
        double T = 1.9;                   // final computation time
        double h = 0.005;                // time step
        double e = 0.9;                  // restit. coef. for impact on the ground.
        double e2 = 0.0;                 // restit. coef for angular stops impacts.

        // -> mind to set the initial conditions below.

        // -------------------------
        // --- Dynamical systems ---
        // -------------------------

        // unsigned int i;

        // --- DS: robot arm ---

        // The dof are angles between ground and arm and between differents parts of the arm. (See Robot.fig for more details)
        //


        // Initial position (angles in radian)
        SiconosVector q0(nDof), v0(nDof);
        q0(0) = 0.05;
        q0(1) = 0.05;

        SP::LagrangianDS arm(new LagrangianDS(q0, v0));

        // external plug-in
        arm->setComputeMassFunction("RobotPlugin", "mass");
        arm->setComputeFGyrFunction("RobotPlugin", "FGyr");
        arm->setComputeJacobianFGyrFunction(1, "RobotPlugin", "jacobianVFGyr");
        arm->setComputeJacobianFGyrFunction(0, "RobotPlugin", "jacobianFGyrq");

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

        // Two interactions:
        //  - one with Lagrangian non linear relation to define contact with ground
        //  - the other to define angles limitations (articular stops), with lagrangian linear relation
        //  Both with newton impact ns laws.

        // -- relations --

        // => arm-floor relation
        SP::NonSmoothLaw nslaw(new NewtonImpactNSL(e));
        string G = "RobotPlugin:G2";
        SP::Relation relation(new LagrangianScleronomousR("RobotPlugin:h2", G));
        SP::Interaction inter(new Interaction(2, nslaw, relation));

        // => angular stops

        //     SimpleMatrix H(6,3);
        //     SiconosVector b(6);
        //     H.zero();
        //     H(0,0) =-1;
        //     H(1,0) =1;
        //     H(2,1) =-1;
        //     H(3,1) =1;
        //     H(4,2) =-1;
        //     H(5,2) =1;

        //     b(0) = 1.7;
        //     b(1) = 1.7;
        //     b(2) = 0.3;
        //     b(3) = 0.3;
        //     b(4) = 3.14;
        //     b(5) = 3.14;
        double lim0 = 1.6;
        double lim1 = 3.1;  // -lim_i <= q[i] <= lim_i
        SimpleMatrix H(4, 3);
        SiconosVector b(4);
        H.zero();

        H(0, 0) = -1;
        H(1, 0) = 1;
        H(2, 1) = -1;
        H(3, 1) = 1;

        b(0) = lim0;
        b(1) = lim0;
        b(2) = lim1;
        b(3) = lim1;

        SP::NonSmoothLaw nslaw2(new NewtonImpactNSL(e2));
        SP::Relation relation2(new LagrangianLinearTIR(H, b));
        SP::Interaction inter2(new Interaction(4, nslaw2, relation2));
        // -------------
        // --- Model ---
        // -------------

        SP::Model Robot(new Model(t0, T));
        Robot->nonSmoothDynamicalSystem()->insertDynamicalSystem(arm);

        Robot->nonSmoothDynamicalSystem()->link(inter1, arm);
        Robot->nonSmoothDynamicalSystem()->link(inter2, arm);
        // ----------------
        // --- Simulation ---
        // ----------------

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

        SP::EventDriven s(new EventDriven(t));

        // -- OneStepIntegrators --
        SP::LsodarOSI OSI(new LsodarOSI(arm));
        s->insertIntegrator(OSI);
        // -- OneStepNsProblem --
        IntParameters iparam(5);
        iparam[0] = 20001; // Max number of iteration
        DoubleParameters dparam(5);
        dparam[0] =  0.005; // Tolerance
        string solverName = "PGS" ;
        SP::NonSmoothSolver mySolver(new NonSmoothSolver(solverName, iparam, dparam));
        SP::OneStepNSProblem impact(new LCP(mySolver));
        SP::OneStepNSProblem acceleration(new LCP(mySolver));
        s->insertNonSmoothProblem(impact, SICONOS_OSNSP_ED_IMPACT);
        s->insertNonSmoothProblem(acceleration, SICONOS_OSNSP_ED_ACCELERATION);
        Robot->setSimulation(s);
        cout << "=== End of model loading === " << endl;

        // =========================== End of model definition ===========================  dataPlot(k,7) = (*inter->y(0))(0);


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

        // --- Simulation initialization ---
        Robot->initialize();
        cout << "End of model initialisation" << endl;

        int k = 0;
        int N = 10630;

        // --- Get the values to be plotted ---
        // -> saved in a matrix dataPlot
        unsigned int outputSize = 11;
        SimpleMatrix dataPlot(N + 1, outputSize);
        // For the initial time step:
        // time

        SP::SiconosVector q = arm->q();
        SP::SiconosVector vel = arm->velocity();
        SP::SiconosVector y = inter->y(0);
        SP::SiconosVector yDot = inter->y(1);
        // When a non-smooth event occurs, pre-impact values are saved in memory vectors at pos. 1:
        SP::SiconosVector qMem = arm->getQMemoryPtr()->getSiconosVector(1);
        SP::SiconosVector velMem = arm->getVelocityMemoryPtr()->getSiconosVector(1);
        SP::SiconosVector yMem = inter->getYOldPtr(0);
        SP::SiconosVector yDotMem = inter->getYOldPtr(1);

        dataPlot(k, 0) =  Robot->t0();
        dataPlot(k, 1) = (*q)(0);
        dataPlot(k, 2) = (*vel)(0);
        dataPlot(k, 3) = (*q)(1);
        dataPlot(k, 4) = (*vel)(1);
        dataPlot(k, 5) = (*q)(2);
        dataPlot(k, 6) = (*vel)(2);
        dataPlot(k, 7) = (*y)(0);
        dataPlot(k, 8) = (*y)(1);
        dataPlot(k, 9) = (*yDot)(0);
        dataPlot(k, 10) = (*yDot)(1);

        // --- Time loop ---
        cout << "Start computation ... " << endl;
        boost::timer boostTimer;
        boostTimer.restart();

        unsigned int numberOfEvent = 0 ;
        SP::EventsManager eventsManager = s->eventsManager();
        bool nonSmooth = false;
        while (s->hasNextEvent())
        {
            // get current time step
            k++;
            s->advanceToEvent();
            if (eventsManager->nextEvent()->getType() == 2)
                nonSmooth = true;

            s->processEvents();
            // If the treated event is non smooth, we get the pre-impact state.
            if (nonSmooth)
            {
                dataPlot(k, 0) =  s->startingTime();
                dataPlot(k, 1) = (*qMem)(0);
                dataPlot(k, 2) = (*velMem)(0);
                dataPlot(k, 3) = (*qMem)(1);
                dataPlot(k, 4) = (*velMem)(1);
                dataPlot(k, 5) = (*qMem)(2);
                dataPlot(k, 6) = (*velMem)(2);
                dataPlot(k, 7) = (*yMem)(0);
                dataPlot(k, 8) = (*yMem)(1);
                dataPlot(k, 9) = (*yDotMem)(0);
                dataPlot(k, 10) = (*yDotMem)(1);


                k++;
            }
            dataPlot(k, 0) =  s->startingTime();
            dataPlot(k, 1) = (*q)(0);
            dataPlot(k, 2) = (*vel)(0);
            dataPlot(k, 3) = (*q)(1);
            dataPlot(k, 4) = (*vel)(1);
            dataPlot(k, 5) = (*q)(2);
            dataPlot(k, 6) = (*vel)(2);
            dataPlot(k, 7) = (*y)(0);
            dataPlot(k, 8) = (*y)(1);
            dataPlot(k, 9) = (*yDot)(0);
            dataPlot(k, 10) = (*yDot)(1);
            numberOfEvent++;
            //  cout << k << endl;
            //  if (k==N) break;
        }

        cout << "===== End of Event Driven simulation. " << numberOfEvent << " events have been processed. ==== " << endl << endl;
        cout << "Computation Time: " << boostTimer.elapsed()  << endl;

        cout << endl << "Output writing ..." << endl;
        // --- Output files ---
        ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");
    }

    catch (SiconosException e)
    {
        cout << e.report() << endl;
    }
    catch (...)
    {
        cout << "Exception caught in \'sample/MultiBeadsColumn\'" << endl;
    }
}
Esempio n. 4
0
//==========================================================================================================
//                                             Main function
//==========================================================================================================
int main(int argc, char* argv[])
{
  //---------------------------- calculate the computation time --------------------------------------------------
  boost::timer time;
  time.restart();
  try
  {
    //===========================================================================================================
    //                  I: Declare the dynamical systems
    //===========================================================================================================
    //1. Set the mass matrix
    SP::SiconosMatrix Mass(new SimpleMatrix(Nfreedom, Nfreedom));
    double InertiaBlock;
    InertiaBlock = (MassBlock / 12.0) * (pow(HeightBlock, 2) + pow(LengthBlock, 2)); // moment of inertia
    (*Mass)(0, 0) = MassBlock;
    (*Mass)(1, 1) = MassBlock;
    (*Mass)(2, 2) = InertiaBlock;
    //2. Set the initial position of the block in function of the initial position of the contact point A (left-hand contact)
    SP::SiconosVector PosIniBlock(new SiconosVector(Nfreedom));
    /*
    (*PosIniBlock)(0) = PosXiniPointA + 0.5*LengthBlock*cos(AngleThetaIni) - 0.5*HeightBlock*sin(AngleThetaIni);
    (*PosIniBlock)(1) = PosYiniPointA + 0.5*LengthBlock*sin(AngleThetaIni) + 0.5*HeightBlock*cos(AngleThetaIni);
    (*PosIniBlock)(2) = AngleThetaIni;
    */
    (*PosIniBlock)(0) = 0.5;
    (*PosIniBlock)(1) = 0.5;
    (*PosIniBlock)(2) = 0.0;

    //3. Set the initial velocity of the block in function of the initial relative velocity of the contact point A
    SP::SiconosVector VelIniBlock(new SiconosVector(Nfreedom));
    /*
    (*VelIniBlock)(0) = VelXiniPointA - (0.5*LengthBlock*sin(AngleThetaIni) + 0.5*HeightBlock*cos(AngleThetaIni))*RotVelBlockIni;
    (*VelIniBlock)(1) = VelYiniPointA + (0.5*LengthBlock*cos(AngleThetaIni) - 0.5*HeightBlock*sin(AngleThetaIni))*RotVelBlockIni;
    (*VelIniBlock)(2) = RotVelBlockIni;
    */
    (*VelIniBlock)(0) = 0.0;
    (*VelIniBlock)(1) = 0.0;
    (*VelIniBlock)(2) = 0.0;

    //4. Instantiate the object of "LagrangianTIDS"
    SP::LagrangianLinearTIDS RockingBlock(new LagrangianLinearTIDS(PosIniBlock, VelIniBlock, Mass));
    //5. Set the external force
    SP::SiconosVector ForceExtern(new SiconosVector(Nfreedom));
    (*ForceExtern)(1) = -MassBlock * GGearth;
    RockingBlock->setFExtPtr(ForceExtern);
    //
    //----------------------------- Display variables of the dynamical system---------------------------------------
    cout << "Initial position of the rocking block:" << endl;
    PosIniBlock->display();
    cout << "Initial velocity of the rocking block:" << endl;
    VelIniBlock->display();
    cout << "Mass matrix of the rocking block:" << endl;
    Mass->display();
    cout << "External force applied on the rocking block:"  << endl;
    ForceExtern->display();
    //==================================================================================================================
    //              II: Declare the relation et interaction between dynamical systems
    //==================================================================================================================
    //
    // Impact law
    SP::NonSmoothLaw nslaw(new NewtonImpactNSL(e));
    // Interaction at contact point 1
    //SP::Relation relation1(new LagrangianLinearTIR(H, E));
    SP::Relation relation1(new LagrangianScleronomousR("RockingBlockPlugin:h1", "RockingBlockPlugin:G1", "RockingBlockPlugin:G1dot"));
    SP::Interaction inter1(new Interaction(1, nslaw, relation1));
    // Interaction at contact point 2
    //SP::Relation relation2(new LagrangianLinearTIR(H, E));
    SP::Relation relation2(new LagrangianScleronomousR("RockingBlockPlugin:h2", "RockingBlockPlugin:G2", "RockingBlockPlugin:G2dot"));
    SP::Interaction inter2(new Interaction(1, nslaw, relation2));
    // Interactions for the whole dynamical system
    //================================================================================================================
    //            III. Create the "model" object
    //================================================================================================================
    SP::Model RoBlockModel(new Model(TimeInitial, TimeFinal));
    RoBlockModel->nonSmoothDynamicalSystem()->insertDynamicalSystem(RockingBlock);
    RoBlockModel->nonSmoothDynamicalSystem()->link(inter1, RockingBlock);
    RoBlockModel->nonSmoothDynamicalSystem()->link(inter2, RockingBlock);
    
    //================================================================================================================
    //            IV. Create the simulation
    //================================================================================================================
    //1. Time discretization
    SP::TimeDiscretisation TimeDiscret(new TimeDiscretisation(TimeInitial, StepSize));
    //2. Integration solver for one step
    SP::OneStepIntegrator OSI(new LsodarOSI());
    //3. Nonsmooth problem
    SP::OneStepNSProblem impact(new LCP());
    SP::OneStepNSProblem acceleration(new LCP());
    //4. Simulation with (1), (2), (3)
    SP::Simulation EDscheme(new EventDriven(TimeDiscret));
    EDscheme->insertIntegrator(OSI);
    EDscheme->insertNonSmoothProblem(impact, SICONOS_OSNSP_ED_IMPACT);
    EDscheme->insertNonSmoothProblem(acceleration, SICONOS_OSNSP_ED_SMOOTH_ACC);
    RoBlockModel->setSimulation(EDscheme); // initialize the model

    // bool check1 = EDscheme->hasOneStepNSProblem(impact);
    // bool check2 = EDscheme->hasOneStepNSProblem(acceleration);
    // cout << "Impact law included in the simulation: " << check1 << endl;
    // cout << "LCP at acceleration level included in the simulation: " << check2 << endl;
    //==================================================================================================================
    //                    V. Process the simulation
    //==================================================================================================================
    // -------------------------------- Simulation initialization ------------------------------------------------------
    cout << "====> Simulation initialisation ..." << endl << endl;
    RoBlockModel->initialize(); // initialize the model
    EDscheme->setPrintStat(true);
    SP::EventsManager eventsManager = EDscheme->eventsManager(); // ponters point to the "eventsManager" object
    SP::SiconosVector PosBlock = RockingBlock->q();              // pointer points to the position vector of the rocking block
    SP::SiconosVector VelBlock = RockingBlock->velocity();       // pointer points to the velocity of the rocking block
    //-------------------- Save the output during simulation ---------------------------------------------------------
    SimpleMatrix DataPlot(NpointSave, SizeOutput);
    //------------- At the initial time -----------------------------------------------------------------------------
    DataPlot(0, 0) = RoBlockModel->t0();
    DataPlot(0, 1) = (*PosBlock)(0); // Position X
    DataPlot(0, 2) = (*PosBlock)(1); // Position Y
    DataPlot(0, 3) = (*PosBlock)(2); // Angle theta
    DataPlot(0, 4) = (*VelBlock)(0); // Velocity Vx
    DataPlot(0, 5) = (*VelBlock)(1); // Velocity Vy
    DataPlot(0, 6) = (*VelBlock)(2); // Angular velocity
    //----------------------------------- Simulation starts ----------------------------------------------------------
    cout << "====> Start computation ... " << endl << endl;
    bool NSEvent = false;
    unsigned int NumberNSEvent = 0;
    unsigned int k = 1;
    boost::progress_display show_progress(NpointSave);
    while (EDscheme->hasNextEvent() && (k < NpointSave))
    {
      EDscheme->advanceToEvent(); // lead the simulation run from one event to the next
      //---------- detect the statue of the current event ------------------------------------
      if (eventsManager->nextEvent()->getType() == 2) // the current event is non-smooth
      {
        NSEvent = true;
      };
      EDscheme->processEvents();  // process the current event
      //------------------- get data at the beginning of non-smooth events ---------------------------
      if (NSEvent)
      {
        DataPlot(k, 0) = EDscheme->startingTime(); // instant at non-smooth event
        DataPlot(k, 1) = (*(RockingBlock->qMemory()->getSiconosVector(1)))(0);      // Position X
        DataPlot(k, 2) = (*(RockingBlock->qMemory()->getSiconosVector(1)))(1);      // Position Y
        DataPlot(k, 3) = (*(RockingBlock->qMemory()->getSiconosVector(1)))(2);      // Angle theta
        DataPlot(k, 4) = (*(RockingBlock->velocityMemory()->getSiconosVector(1)))(0); // Velocity Vx
        DataPlot(k, 5) = (*(RockingBlock->velocityMemory()->getSiconosVector(1)))(1); // Velocity Vy
        DataPlot(k, 6) = (*(RockingBlock->velocityMemory()->getSiconosVector(1)))(2); // Angular velocity
        //EDscheme->update(1);
        k++;
        ++NumberNSEvent;
        ++show_progress;
        NSEvent = false;                        // The next event is maybe smooth
      };
      //-------------------- get data at smooth events or at the end of non-smooth events ---------------
      DataPlot(k, 0) = EDscheme->startingTime();
      DataPlot(k, 1) = (*PosBlock)(0); //Position X
      DataPlot(k, 2) = (*PosBlock)(1); //Position Y
      DataPlot(k, 3) = (*PosBlock)(2); // Position theta
      DataPlot(k, 4) = (*VelBlock)(0); // Velocity Vx
      DataPlot(k, 5) = (*VelBlock)(1); // Velocity Vy
      DataPlot(k, 6) = (*VelBlock)(2); // Velocity Vtheta
      // go to the next time step
      k++;
      ++show_progress;
    };
    //----------------------- At the end of the simulation --------------------------
    cout << "End of the simulation" << endl;
    cout << "Number of events processed during simulation: " << (k + 1) << endl;
    cout << "Number of non-smooth events: " << NumberNSEvent << endl;
    cout << "====> Output file writing ..." << endl << endl;
    ioMatrix::write("result.dat", "ascii", DataPlot, "noDim");
  }
  //============================== Catch exceptions ===================================================================
  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught." << endl;
  }
  cout << "Computation Time: " << time.elapsed()  << endl;
}
int main(int argc, char* argv[])
{
  boost::timer time;
  time.restart();
  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 = 10.0;                   // final computation time
    double h = 0.01;                // time step
    double position_init = 1.0;      // initial position for lowest bead.
    double velocity_init = 10.0;      // initial velocity for lowest bead.
    double Heightbox = 1.5;
    double R = 0.1; // Ball radius
    double m = 1; // Ball mass
    double g = 10.0; // Gravity

    // -------------------------
    // --- Dynamical systems ---
    // -------------------------
    cout << "====> Model loading ..." << endl << endl;
    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 --
    SP::SiconosVector q0(new SiconosVector(nDof));
    SP::SiconosVector v0(new SiconosVector(nDof));
    (*q0)(0) = position_init;
    (*v0)(0) = velocity_init;
    // -- The dynamical system --
    SP::LagrangianLinearTIDS ball(new LagrangianLinearTIDS(q0, v0, Mass));
    // -- Set external forces (weight) --
    SP::SiconosVector weight(new SiconosVector(nDof));
    (*weight)(0) = -m * g;
    ball->setFExtPtr(weight);
    //
    // --------------------
    // --- Interactions ---
    // --------------------

    // -- nslaw --
    double e = 0.8; // Warning this example does not work with e=0.0

    // Interaction ball-floor-ceiling
    //
    SP::SimpleMatrix H1(new SimpleMatrix(1, nDof));
    (*H1)(0, 0) = 1.0;
    SP::SiconosVector E1(new SiconosVector(1));
    (*E1)(0) = 0.0;//-1.0*R;
    //
    SP::SimpleMatrix H2(new SimpleMatrix(1, nDof));
    (*H2)(0, 0) = -1.0;
    SP::SiconosVector E2(new SiconosVector(1));
    (*E2)(0) = Heightbox ;//- R;
    // impact law
    SP::NonSmoothLaw  nslaw(new NewtonImpactNSL(e));
    // Interaction at contact 1 (ball-floor)
    SP::Relation relation1(new LagrangianLinearTIR(H1, E1));
    SP::Interaction inter1(new Interaction(1, nslaw, relation1));
    // Interaction at contact 2 (ball-ceiling)
    SP::Relation relation2(new LagrangianLinearTIR(H2, E2));
    SP::Interaction inter2(new Interaction(1, nslaw, relation2));
    // --------------------------------
    // --- NonSmoothDynamicalSystem ---
    // --------------------------------

    // -------------
    // --- Model ---
    // -------------

    SP::Model bouncingBall(new Model(t0, T));
    // add the dynamical system in the non smooth dynamical system
    bouncingBall->nonSmoothDynamicalSystem()->insertDynamicalSystem(ball);

    // link the interaction and the dynamical system
    bouncingBall->nonSmoothDynamicalSystem()->link(inter1, ball);
    bouncingBall->nonSmoothDynamicalSystem()->link(inter2, ball);

    // ----------------
    // --- Simulation ---
    // ----------------

    // -- (1) OneStepIntegrators --
    SP::OneStepIntegrator OSI(new LsodarOSI());

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

    // -- (3) Non smooth problem --
    SP::OneStepNSProblem impact(new LCP());
    SP::OneStepNSProblem acceleration(new LCP());

    // -- (4) Simulation setup with (1) (2) (3)
    SP::EventDriven s(new EventDriven(t));
    s->insertIntegrator(OSI);
    s->insertNonSmoothProblem(impact, SICONOS_OSNSP_ED_IMPACT);
    s->insertNonSmoothProblem(acceleration, SICONOS_OSNSP_ED_SMOOTH_ACC);
    cout << "SICONOS_OSNSP_ED_IMPACT: " << SICONOS_OSNSP_ED_IMPACT << endl;
    cout << "SICONOS_OSNSP_ED_ACCELERATION :" << SICONOS_OSNSP_ED_SMOOTH_ACC << endl;
    bouncingBall->setSimulation(s);
    // =========================== End of model definition ===========================

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

    // --- Simulation initialization ---
    cout << "====> Simulation initialisation ..." << endl << endl;
    s->setPrintStat(true);
    bouncingBall->initialize();
    OSI->display();
    int N = 1850; // Number of saved points: depends on the number of events ...
    int ll = 0;

    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 9;
    SimpleMatrix dataPlot(N, outputSize);
    SP::SiconosVector q = ball->q();        // ball position
    SP::SiconosVector v = ball->velocity(); // ball velocity
    SP::SiconosVector gamma = ball->acceleration(); // ball velocity
    SP::SiconosVector f = ball->p(2);       // resultant force deduced from the LCP at acceleration level
    SP::SiconosVector p = ball->p(1);       // resultant force deduced from the LCP at velocity level


    SP::SiconosVector y1 = inter1->y(0);
    SP::SiconosVector y2 = inter2->y(0);
    //   SiconosVector * y = bouncingBall->nonSmoothDynamicalSystem()->interaction(0)->y(0);

    SP::EventsManager eventsManager = s->eventsManager();
    OSI->display();
    // For the initial time step:
    // time

    dataPlot(0, 0) = bouncingBall->t0();
    dataPlot(0, 1) = (*q)(0);
    dataPlot(0, 2) = (*v)(0);
    dataPlot(0, 3) = (*p)(0);
    dataPlot(0, 4) = 0;
    dataPlot(0, 5) = (*y1)(0);
    dataPlot(0, 6) = (*y2)(0);
    dataPlot(0, 7) = (*gamma)(0);
    dataPlot(0, 8) = (*f)(0);

    // --- Time loop ---
    cout << "====> Start computation ... " << endl << endl;
    bool nonSmooth = false;
    unsigned int numberOfEvent = 0 ;
    double k = 1;
    boost::progress_display show_progress(N);
    s->setPrintStat(true);
    //    s->setTolerance(1e-10);
    while (s->hasNextEvent())
    {
      s->advanceToEvent(); // run simulation from one event to the next
      if (eventsManager->nextEvent()->getType() == 2)
        nonSmooth = true;

      s->processEvents();  // process events
      // If the treated event is non smooth, the pre-impact state has been solved in memory vectors during process.
      if (nonSmooth) // if the event is nonsmooth
      {
        dataPlot(k,0) = s->startingTime(); // get the time at nonsmooth event
        dataPlot(k,1) = (*ball->qMemory()->getSiconosVector(1))(0);
        dataPlot(k,2) = (*ball->velocityMemory()->getSiconosVector(1))(0);
        k++;
        nonSmooth = false;
        ++show_progress;
        dataPlot(k,4) = 1;
        ++ll;
        //         cout << "========================================" << endl;
        //         cout << "Nonsmooth event" << endl;
      }
      dataPlot(k, 0) = s->startingTime();
      dataPlot(k, 1) = (*q)(0);
      dataPlot(k, 2) = (*v)(0);
      dataPlot(k, 3) = (*p)(0);
      dataPlot(k, 5) = (*y1)(0);
      dataPlot(k, 6) = (*y2)(0);
      dataPlot(k, 4) = 0;
      dataPlot(k, 7) = (*gamma)(0);
      dataPlot(k, 8) = (*f)(0);




      cout << "========================================" << endl;
      cout << " time: " << s->startingTime() << endl;
      cout << "ball position: " << (*q)(0) << endl;
      cout << "ball velocity: " << (*v)(0) << endl;
      cout << "gap at contact 1: " << (*y1)(0) << endl;
      cout << "gap at contact 2: " << (*y2)(0) << endl;
      //
      k++;
      ++numberOfEvent;
      ++show_progress;
    }

    // --- Output files ---
    cout << endl;
    cout << "===== End of Event Driven simulation. " << numberOfEvent << " events have been processed. ==== " << endl << endl;
    cout << "Number of nonsmooth events = " << ll << endl;
    cout << "====> Output file writing ..." << endl << endl;
    dataPlot.resize(k, outputSize);
    ioMatrix::write("BouncingBallTwoContactsED.dat", "ascii", dataPlot, "noDim");

    // Comparison with a reference file
    SimpleMatrix dataPlotRef(dataPlot);
    dataPlotRef.zero();
    ioMatrix::read("BouncingBallTwoContactsED.cpp", "ascii", dataPlotRef);

    std:: cout << " Error ="<< (dataPlot - dataPlotRef).normInf() << std::endl;

    if ((dataPlot - dataPlotRef).normInf() > 1e-12)
    {
      std::cout << "Warning. The results is rather different from the reference file." << std::endl;
      return 1;
    }

  }

  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught." << endl;
  }
  cout << "Computation Time: " << time.elapsed()  << endl;
}
int main(int argc, char* argv[])
{
  //---------------------------- calculate the computation time --------------------------------------------------
  boost::timer time;
  time.restart();
  try
  {
    // ================= Creation of the model =======================



    // -> mind to set the initial conditions below.

    // -------------------------
    // --- Dynamical systems ---
    // -------------------------

    // --- DS: Simple Pendulum ---

    // Initial position (angles in radian)
    SP::SiconosVector q0(new SiconosVector(nDof));
    SP::SiconosVector v0(new SiconosVector(nDof));
    (*q0).zero();
    (*v0).zero();
    (*q0)(0) = L * sin(InitAngle);
    (*q0)(1) = L * cos(InitAngle);

    SP::SimpleMatrix Mass(new SimpleMatrix(nDof, nDof));
    (*Mass)(0, 0) = m;
    (*Mass)(1, 1) = m;
    SP::LagrangianDS simplependulum(new LagrangianLinearTIDS(q0, v0, Mass));


    SP::SiconosVector ForceExtern(new SiconosVector(nDof));
    (*ForceExtern)(0) = 0.0;
    (*ForceExtern)(1) = m * gravity;
    simplependulum->setFExtPtr(ForceExtern);

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


    SP::NonSmoothLaw nslaw(new NewtonImpactNSL(e));
    SP::Relation relation(new LagrangianScleronomousR("SimplePendulumBilateralConstraintPlugin:h0", "SimplePendulumBilateralConstraintPlugin:G0", "SimplePendulumBilateralConstraintPlugin:G0dot"));
    SP::Interaction inter(new Interaction(1, nslaw, relation));

    // -------------
    // --- Model ---
    // -------------

    SP::Model Pendulum(new Model(t0, T));
    Pendulum->nonSmoothDynamicalSystem()->insertDynamicalSystem(simplependulum);
    Pendulum->nonSmoothDynamicalSystem()->link(inter, simplependulum);

    // ----------------
    // --- Simulation ---
    // ----------------

    //1. Time discretization
    SP::TimeDiscretisation TimeDiscret(new TimeDiscretisation(t0, h));
    //2. Integration solver for one step
    SP::OneStepIntegrator OSI(new LsodarOSI());
    //3. Nonsmooth problem
    SP::OneStepNSProblem impact(new LCP());
    SP::OneStepNSProblem acceleration(new LCP());
    //4. Simulation with (1), (2), (3)
    SP::Simulation EDscheme(new EventDriven(TimeDiscret));
    EDscheme->insertIntegrator(OSI);
    EDscheme->insertNonSmoothProblem(impact, SICONOS_OSNSP_ED_IMPACT);
    EDscheme->insertNonSmoothProblem(acceleration, SICONOS_OSNSP_ED_SMOOTH_ACC);
    Pendulum->setSimulation(EDscheme); // initialize the model

    // =========================== End of model definition ===========================
    // --- Simulation Initialization ---
    cout << "====> Simulation initialisation ..." << endl << endl;
    EDscheme->setPrintStat(true);
    Pendulum->initialize(); // initialize the model
    cout << "End of simulation initialisation" << endl;

    // SP::LsodarOSI lsodar = std11::static_pointer_cast<LsodarOSI>(OSI);
    // lsodar->setMinMaxStepSizes(9.5e-4,1.0e-3);
    // lsodar->setTol(1,1.0e-3,1.0e-6);
    // lsodar->setMaxOrder(2, 2);


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

    SP::EventsManager eventsManager = EDscheme->eventsManager(); // ponters point to the "eventsManager" object
    SP::SiconosVector _q = simplependulum->q();              // pointer points to the position vector of the rocking block
    SP::SiconosVector _qdot = simplependulum->velocity();       // pointer points to the velocity of the rocking block
    SP::SiconosVector _qddot = simplependulum->acceleration();       // pointer points to the velocity of the rocking block
    SP::SiconosVector _g = inter->y(0);
    SP::SiconosVector _gdot = inter->y(1);
    SP::SiconosVector _lambda = inter->lambda(2);
    SP::InteractionsGraph indexSet0 = Pendulum->nonSmoothDynamicalSystem()->topology()->indexSet(0);
    SP::InteractionsGraph indexSet1 = Pendulum->nonSmoothDynamicalSystem()->topology()->indexSet(1);
    SP::InteractionsGraph indexSet2 = Pendulum->nonSmoothDynamicalSystem()->topology()->indexSet(2);
    cout << "Size of IndexSet0: " << indexSet0->size() << endl;
    cout << "Size of IndexSet1: " << indexSet1->size() << endl;
    cout << "Size of IndexSet2: " << indexSet2->size() << endl;
    //-------------------- Save the output during simulation ---------------------------------------------------------
    SimpleMatrix DataPlot(N, 10);
    //------------- At the initial time -----------------------------------------------------------------------------
    DataPlot(0, 0) = Pendulum->t0();
    DataPlot(0, 1) = (*_q)(0); // Position X
    DataPlot(0, 2) = (*_q)(1); // Position Y
    DataPlot(0, 3) = (*_qdot)(0); // Velocity Vx
    DataPlot(0, 4) = (*_qdot)(1); // Velocity Vy
    DataPlot(0, 5) = (*_qddot)(0); // Acceleration ax
    DataPlot(0, 6) = (*_qddot)(1); // Acceleration ay
    DataPlot(0, 7) = (*_g)(0);     // Contraint in position
    DataPlot(0, 8) = (*_gdot)(0);  // Constraint in velocity
    DataPlot(0, 9) = (*_lambda)(0); // Reaction force

    //----------------------------------- Simulation starts ----------------------------------------------------------
    cout << "====> Start computation ... " << endl << endl;
    bool NSEvent = false;
    unsigned int NumberNSEvent = 0;
    unsigned int k = 1;
    boost::progress_display show_progress(N);
    while ((EDscheme->hasNextEvent()) && (k < N))
    {
      EDscheme->advanceToEvent(); // lead the simulation run from one event to the next
      //---------- detect the statue of the current event ------------------------------------
      if (eventsManager->nextEvent()->getType() == 2) // the current event is non-smooth
      {
        NSEvent = true;
      };
      EDscheme->processEvents();  // process the current event
      //------------------- get data at the beginning of non-smooth events ---------------------------
      if (NSEvent)
      {
        DataPlot(k, 0) = EDscheme->startingTime(); // instant at non-smooth event
        SP::SiconosVector _qMemory = simplependulum->qMemory()->getSiconosVector(1);
        SP::SiconosVector _qdotMemory = simplependulum->velocityMemory()->getSiconosVector(1);
        DataPlot(k, 1) = (*_qMemory)(0);
        DataPlot(k, 2) = (*_qMemory)(1);
        DataPlot(k, 3) = (*_qdotMemory)(0);
        DataPlot(k, 4) = (*_qdotMemory)(1);
        k++;
        ++NumberNSEvent;
        ++show_progress;
        NSEvent = false;                        // The next event is maybe smooth
      };
      //-------------------- get data at smooth events or at the end of non-smooth events ---------------
      DataPlot(k, 0) = EDscheme->startingTime();
      DataPlot(k, 1) = (*_q)(0); // Position X
      DataPlot(k, 2) = (*_q)(1); // Position Y
      DataPlot(k, 3) = (*_qdot)(0); // Velocity Vx
      DataPlot(k, 4) = (*_qdot)(1); // Velocity Vy
      DataPlot(k, 5) = (*_qddot)(0); // Acceleration ax
      DataPlot(k, 6) = (*_qddot)(1); // Acceleration ay
      DataPlot(k, 7) = (*_g)(0);     // Contraint in position
      DataPlot(k, 8) = (*_gdot)(0);  // Constraint in velocity
      DataPlot(k, 9) = (*_lambda)(0); // Reaction force
      // go to the next time step
      k++;
      ++show_progress;
    }
    //----------------------- At the end of the simulation --------------------------
    cout << " " << endl;
 
    cout << "End of the simulation" << endl;
    cout << "Number of events processed during simulation: " << (k + 1) << endl;
    cout << "Number of non-smooth events: " << NumberNSEvent << endl;
    cout << "====> Output file writing ..." << endl << endl;
    ioMatrix::write("result.dat", "ascii", DataPlot, "noDim");
    // Comparison with a reference file
    SimpleMatrix dataPlotRef(DataPlot);
    dataPlotRef.zero();
    ioMatrix::read("result_LsodarOSI.ref", "ascii", dataPlotRef);
    std::cout << (DataPlot - dataPlotRef).normInf() << std::endl;
    if ((DataPlot - dataPlotRef).normInf() > 1e-12)
    {
      std::cout << "Warning. The results is rather different from the reference file." << std::endl;
      return 1;
    }
  }

  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught" << endl;
  }
  cout << "Computation Time: " << time.elapsed()  << endl;
}
Esempio n. 7
0
int main(int argc, char* argv[])
{
  boost::timer time;
  time.restart();
  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 = 8.5;                   // final computation time
    double h = 0.005;                // time step
    double position_init = 1.0;      // initial position for lowest bead.
    double velocity_init = 0.0;      // initial velocity for lowest bead.
    double R = 0.1; // Ball radius
    double m = 1; // Ball mass
    double g = 9.81; // Gravity

    // -------------------------
    // --- Dynamical systems ---
    // -------------------------

    cout << "====> Model loading ..." << endl << endl;
    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 --
    SP::SiconosVector q0(new SiconosVector(nDof));
    SP::SiconosVector v0(new SiconosVector(nDof));
    (*q0)(0) = position_init;
    (*v0)(0) = velocity_init;
    // -- The dynamical system --
    SP::LagrangianLinearTIDS ball(new LagrangianLinearTIDS(q0, v0, Mass));
    // -- Set external forces (weight) --
    SP::SiconosVector weight(new SiconosVector(nDof));
    (*weight)(0) = -m * g;
    ball->setFExtPtr(weight);

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

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

    // Interaction ball-floor
    //
    SP::SimpleMatrix H(new SimpleMatrix(1, nDof));
    (*H)(0, 0) = 1.0;
    SP::NonSmoothLaw  nslaw0(new NewtonImpactNSL(e));
    SP::Relation relation0(new LagrangianLinearTIR(H));

    SP::Interaction inter(new Interaction(1, nslaw0, relation0));

    // --------------------------------
    // --- NonSmoothDynamicalSystem ---
    // --------------------------------

    // -------------
    // --- Model ---
    // -------------

    SP::Model bouncingBall(new Model(t0, T));

    // add the dynamical system in the non smooth dynamical system
    bouncingBall->nonSmoothDynamicalSystem()->insertDynamicalSystem(ball);

    // link the interaction and the dynamical system
    bouncingBall->nonSmoothDynamicalSystem()->link(inter, ball);

    // ----------------
    // --- Simulation ---
    // ----------------

    // -- (1) OneStepIntegrators --
    SP::OneStepIntegrator OSI(new LsodarOSI(ball));

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

    // -- (3) Non smooth problem --
    SP::OneStepNSProblem impact(new LCP());
    SP::OneStepNSProblem acceleration(new LCP());

    // -- (4) Simulation setup with (1) (2) (3)
    SP::EventDriven s(new EventDriven(t));
    s->insertIntegrator(OSI);
    s->insertNonSmoothProblem(impact, SICONOS_OSNSP_ED_IMPACT);
    s->insertNonSmoothProblem(acceleration, SICONOS_OSNSP_ED_SMOOTH_ACC);

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

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

    // --- Simulation initialization ---
    cout << "====> Simulation initialisation ..." << endl << endl;
    s->setPrintStat(true);
    bouncingBall->initialize(s);

    int N = 1854; // Number of saved points: depends on the number of events ...

    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 7;
    SimpleMatrix dataPlot(N + 1, outputSize);
    SP::SiconosVector q = ball->q();
    SP::SiconosVector v = ball->velocity();
    SP::SiconosVector p = ball->p(1);
    SP::SiconosVector f = ball->p(2);
    //   SiconosVector * y = bouncingBall->nonSmoothDynamicalSystem()->interaction(0)->y(0);

    SP::EventsManager eventsManager = s->eventsManager();

    // For the initial time step:
    // time

    dataPlot(0, 0) = bouncingBall->t0();
    dataPlot(0, 1) = (*q)(0);
    dataPlot(0, 2) = (*v)(0);
    dataPlot(0, 3) = (*p)(0);
    dataPlot(0, 4) = (*f)(0);

    // --- Time loop ---
    cout << "====> Start computation ... " << endl << endl;
    bool nonSmooth = false;
    unsigned int numberOfEvent = 0 ;
    int k = 0;
    int kns = 0;
    boost::progress_display show_progress(N);
    while (s->hasNextEvent() && k < N)
    {
      s->advanceToEvent();
      if (eventsManager->nextEvent()->getType() == 2)
        nonSmooth = true;

      s->processEvents();
      // If the treated event is non smooth, the pre-impact state has been solved in memory vectors during process.
      if (nonSmooth)
      {
        dataPlot(k, 0) = s->startingTime();
        dataPlot(k, 1) = (*ball->qMemory()->getSiconosVector(1))(0);
        dataPlot(k, 2) = (*ball->velocityMemory()->getSiconosVector(1))(0);
        dataPlot(k, 3) = (*p)(0);
        dataPlot(k, 4) = (*f)(0);
        k++;
        kns++;
        nonSmooth = false;
        ++show_progress;
      }
      dataPlot(k, 0) = s->startingTime();
      dataPlot(k, 1) = (*q)(0);
      dataPlot(k, 2) = (*v)(0);
      dataPlot(k, 3) = (*p)(0);
      dataPlot(k, 4) = (*f)(0);
      dataPlot(k, 5) = (*inter->lambda(1))(0);
      dataPlot(k, 6) = (*inter->lambda(2))(0);


      ++k;
      ++numberOfEvent;
      ++show_progress;
    }

    // --- Output files ---
    cout << endl;
    cout << "===== End of Event Driven simulation. " << endl;
    cout << numberOfEvent << " events have been processed. ==== " << endl;

    cout << numberOfEvent- kns << " events are of time--discretization type  ==== " << endl;
    cout << kns << " events are  of nonsmooth type  ==== " << endl << endl;

    cout << "====> Output file writing ..." << endl << endl;
    dataPlot.resize(k, outputSize);
    ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");

    // Comparison with a reference file
    SimpleMatrix dataPlotRef(dataPlot);
    dataPlotRef.zero();
    ioMatrix::read("BouncingBallED.ref", "ascii", dataPlotRef);
    
    if ((dataPlot - dataPlotRef).normInf() > 1e-12)
    {
      std::cout << "Warning. The results is rather different from the reference file." << std::endl;
      std::cout << "error = " << (dataPlot - dataPlotRef).normInf() << std::endl;
      return 1;
    }

  }

  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught." << endl;
  }
  cout << "Computation Time: " << time.elapsed()  << endl;
}