text: /* Siconos is a program dedicated to modeling, simulation and control
   * of non smooth dynamical systems.
   *
   * Copyright 2018 INRIA.
   *
   * Licensed under the Apache License, Version 2.0 (the "License");
   * you may not use this file except in compliance with the License.
   * You may obtain a copy of the License at
   *
   * http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
  */
  
  /*!\file BouncingBallTS.cpp
    \brief \ref EMBouncingBall - C++ input file, Time-Stepping version -
    V. Acary, F. Perignon.
  
    A Ball bouncing on the ground.
    Direct description of the model.
    Simulation with a Time-Stepping scheme.
  */
  
  #include "SiconosKernel.hpp"
  
  using namespace std;
  
  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 = 10;                  // 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 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;
  
      SP::SiconosMatrix Mass(new SimpleMatrix(nDof, nDof));
      (*Mass)(0, 0) = m;
      (*Mass)(1, 1) = m;
      (*Mass)(2, 2) = 2. / 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 nslaw(new NewtonImpactNSL(e));
      SP::Relation relation(new LagrangianLinearTIR(H));
  
      SP::Interaction inter(new Interaction(nslaw, relation));
  
      // -------------
      // --- Model ---
      // -------------
      SP::NonSmoothDynamicalSystem bouncingBall(new NonSmoothDynamicalSystem(t0, T));
  
      // add the dynamical system in the non smooth dynamical system
      bouncingBall->insertDynamicalSystem(ball);
  
      // link the interaction and the dynamical system
      bouncingBall->link(inter, ball);
  
  
      // ------------------
      // --- Simulation ---
      // ------------------
  
      // -- (1) OneStepIntegrators --
      SP::MoreauJeanDirectProjectionOSI OSI(new MoreauJeanDirectProjectionOSI(theta));
  
      // -- (2) Time discretisation --
      SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));
  
      // -- (3) one step non smooth problem
      SP::OneStepNSProblem osnspb(new LCP());
      SP::OneStepNSProblem osnspb_pos(new MLCPProjectOnConstraints(SICONOS_MLCP_ENUM));
  
      // -- (4) Simulation setup with (1) (2) (3)
      unsigned int levelForProjection = 1; //(default =1)
      SP::TimeSteppingDirectProjection s(new TimeSteppingDirectProjection(bouncingBall, t, OSI, osnspb, osnspb_pos, levelForProjection));
      s->setProjectionMaxIteration(20);
      s->setConstraintTolUnilateral(1e-08);
      s->setConstraintTol(1e-04);
  
      // =========================== End of model definition ===========================
  
      // ================================= Computation =================================
  
      int N = ceil((T - t0) / h); // Number of time steps
  
      // --- 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 p1 = ball->p(1);
      SP::SiconosVector lambda1; // = inter->lambda(1);
      SP::SiconosVector lambda0 = inter->lambda(0);
      SP::SiconosVector p0; // = ball->p(0);
  
      dataPlot(0, 0) = bouncingBall->t0();
      dataPlot(0, 1) = (*q)(0);
      dataPlot(0, 2) = (*v)(0);
      dataPlot(0, 3) = (*p1)(0);
      dataPlot(0, 4) = 0.0;
      dataPlot(0, 5) = 0.0;
      dataPlot(0, 6) = (*lambda0)(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();
  
      while (s->hasNextEvent())
      {
        s->computeOneStep();
        //std ::cout << "time step k = " << k << std::endl;
        // --- Get values to be plotted ---
        lambda1 = inter->lambda(1);
        p0  = ball->p(0);
        dataPlot(k, 0) =  s->nextTime();
        dataPlot(k, 1) = (*q)(0);
        dataPlot(k, 2) = (*v)(0);
        dataPlot(k, 3) = (*p1)(0);
        dataPlot(k, 4) = (*lambda1)(0);
        dataPlot(k, 5) = (*p0)(0);
        dataPlot(k, 6) = (*lambda0)(0);
  
        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
      double error=0.0, eps=1e-12;
  
      if (levelForProjection == 1)
      {
        if ((error=ioMatrix::compareRefFile(
               dataPlot, "BouncingBallTS-MoreauJeanDirectProjectionOSI.ref",
               eps)) >= 0.0
            && error > eps)
          return 1;
      }
      else
      {
        if ((error=ioMatrix::compareRefFile(
              dataPlot, "BouncingBallTS-MoreauJeanDirectProjectionOSI-level0.ref",
              eps)) >= 0.0
            && error > eps)
          return 1;
      }
  
    }
  
    catch (SiconosException e)
    {
      cout << e.report() << endl;
    }
    catch (...)
    {
      cout << "Exception caught in BouncingBallTS.cpp" << endl;
    }
  
  
  
  }
  