<commit_before>/* Siconos-Kernel, Copyright INRIA 2005-2010.
 * Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 * Siconos is a free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * Siconos is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Siconos; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Contact: <NAME>, <EMAIL>
 */

#include "TimeStepping.hpp"

#include "TimeSteppingProjectOnConstraints.hpp"
#include "NewtonEulerDS.hpp"
#include "NewtonEulerRImpact.hpp"
using namespace std;

#define TSPROJ_DEBUG
TimeSteppingProjectOnConstraints::TimeSteppingProjectOnConstraints(SP::TimeDiscretisation td,
    SP::OneStepIntegrator osi,
    SP::OneStepNSProblem osnspb_velo,
    SP::OneStepNSProblem osnspb_pos)
  : TimeStepping(td, osi, osnspb_velo)
{
  (*_allNSProblems).resize(SICONOS_NB_OSNSP_TSP);
  insertNonSmoothProblem(osnspb_pos, SICONOS_OSNSP_TS_POS);
  _constraintTol = 10e-4;
}

// --- Destructor ---
TimeSteppingProjectOnConstraints::~TimeSteppingProjectOnConstraints()
{
}

void TimeSteppingProjectOnConstraints::initOSNS()
{
  TimeStepping::initOSNS();
  (*_allNSProblems)[SICONOS_OSNSP_TS_POS]->setLevelMin(1);
  (*_allNSProblems)[SICONOS_OSNSP_TS_POS]->setLevelMax(1);
}
void TimeSteppingProjectOnConstraints::newtonSolve(double criterion, unsigned int maxStep)
{
#ifdef TSPROJ_DEBUG
  cout << "TimeStepping::newtonSolve begin :\n";
#endif
  TimeStepping::newtonSolve(criterion, maxStep);
#ifdef TSPROJ_DEBUG
  cout << "TimeStepping::newtonSolve end : nbit=" << getNewtonNbSteps() << "\n";
#endif

  int info = 0;
#ifdef TSPROJ_DEBUG
  cout << "TimeSteppingProjectOnConstraints::newtonSolve begin projection:\n";
#endif
  SP::DynamicalSystemsGraph dsGraph = model()->nonSmoothDynamicalSystem()->dynamicalSystems();
  for (DynamicalSystemsGraph::VIterator vi = dsGraph->begin(); vi != dsGraph->end(); ++vi)
  {
    SP::DynamicalSystem ds = dsGraph->bundle(*vi);
    Type::Siconos dsType = Type::value(*ds);
    if (dsType != Type::NewtonEulerDS)
      RuntimeException::selfThrow("TS:: - ds is not from NewtonEulerDS.");
    SP::NewtonEulerDS neds = boost::static_pointer_cast<NewtonEulerDS>(ds);
    *(neds->deltaq()) = *(neds->q());
  }
  bool runningNewton = true;
  int cmp = 0;
  SP::InteractionsSet allInteractions = model()->nonSmoothDynamicalSystem()->interactions();
  for (InteractionsIterator it = allInteractions->begin(); it != allInteractions->end(); it++)
  {
    double criteria = (*it)->relation()->interaction()->y(0)->getValue(0);
    if (Type::value(*((*it)->nonSmoothLaw())) ==  Type::NewtonImpactFrictionNSL ||
        Type::value(*((*it)->nonSmoothLaw())) == Type::NewtonImpactNSL)
    {
      SP::NewtonEulerRImpact ri = boost::static_pointer_cast<NewtonEulerRImpact> ((*it)->relation());
      if (criteria < -1e-7)
      {
        ri->_isOnContact = true;
      }
      else
      {
        ri->_isOnContact = false;
      }
    }
    if (criteria < -_constraintTol)
      runningNewton = true;
  }


  while (runningNewton && cmp < 100)
  {
    cmp++;
    printf("TimeSteppingProjectOnConstraints Newton step = %d\n", cmp);

    runningNewton = false;
    for (InteractionsIterator it = allInteractions->begin(); it != allInteractions->end(); it++)
    {
      (*it)->relation()->computeJach(getTkp1());
      (*it)->relation()->computeh(getTkp1());
      double criteria = (*it)->relation()->interaction()->y(0)->getValue(0);

      if (criteria < -1e-4)
        runningNewton = true;
    }
    info = 0;
    if (runningNewton)
    {
      info = computeOneStepNSProblem(SICONOS_OSNSP_TS_POS);
      for (DynamicalSystemsGraph::VIterator vi = dsGraph->begin(); vi != dsGraph->end(); ++vi)
      {
        SP::DynamicalSystem ds = dsGraph->bundle(*vi);
        Type::Siconos dsType = Type::value(*ds);
        if (dsType != Type::NewtonEulerDS)
          RuntimeException::selfThrow("TS:: - ds is not from NewtonEulerDS.");
        SP::NewtonEulerDS neds = boost::static_pointer_cast<NewtonEulerDS>(ds);

        neds->normalizeq();
      }

      updateWorldFromDS();
    }
    //cout<<"||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||  Z:"<<endl;
    //(*_allNSProblems)[SICONOS_OSNSP_TS_POS]->display();
    //(boost::static_pointer_cast<LinearOSNS>((*_allNSProblems)[SICONOS_OSNSP_TS_POS]))->z()->display();
    if (info)
      cout << "TimeSteppingProjectOnConstraints project on constraints failed." << endl ;
    //cout<<"during projection before normalizing of q:\n";
    //for (InteractionsIterator it = allInteractions->begin(); it != allInteractions->end(); it++)
    //{
    //  (*it)->relation()->computeh(getTkp1());
    //}
  }

  for (DynamicalSystemsGraph::VIterator vi = dsGraph->begin(); vi != dsGraph->end(); ++vi)
  {
    SP::DynamicalSystem ds = dsGraph->bundle(*vi);
    Type::Siconos dsType = Type::value(*ds);
    if (dsType != Type::NewtonEulerDS)
      RuntimeException::selfThrow("TS:: - ds is not from NewtonEulerDS.");
    SP::NewtonEulerDS neds = boost::static_pointer_cast<NewtonEulerDS>(ds);
    neds->normalizeq();
    // SP::SiconosVector dotq = neds->dotq();
    // SP::SiconosVector q = neds->q();
    // SP::SiconosVector qold = neds->qMemory()->getSiconosVector(0);
    // double h = timeStep();
    // dotq->setValue(3,(q->getValue(3)-qold->getValue(3))/h);
    // dotq->setValue(4,(q->getValue(4)-qold->getValue(4))/h);
    // dotq->setValue(5,(q->getValue(5)-qold->getValue(5))/h);
    // dotq->setValue(6,(q->getValue(6)-qold->getValue(6))/h);
    neds->updateT();
    //continue;
    /*compute the new velocity seeing the work of fext*/
    *(neds->deltaq()) -= *(neds->q());
    double  n2q = neds->deltaq()->norm2();
    double n2 = neds->fExt()->norm2();
    if (n2 > 1e-7 && n2q > 1e-14)
    {
      //if (n2q < 1e-14)
      //  continue;

      SP::SimpleVector FextNorm(new SimpleVector(3));
      FextNorm->setValue(0, neds->fExt()->getValue(0));
      FextNorm->setValue(1, neds->fExt()->getValue(1));
      FextNorm->setValue(2, neds->fExt()->getValue(2));<commit_msg>case without fext<commit_after>/* Siconos-Kernel, Copyright INRIA 2005-2010.
 * Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 * Siconos is a free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * Siconos is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Siconos; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Contact: <NAME>, <EMAIL>
 */

#include "TimeStepping.hpp"

#include "TimeSteppingProjectOnConstraints.hpp"
#include "NewtonEulerDS.hpp"
#include "NewtonEulerRImpact.hpp"
using namespace std;

#define TSPROJ_DEBUG
TimeSteppingProjectOnConstraints::TimeSteppingProjectOnConstraints(SP::TimeDiscretisation td,
    SP::OneStepIntegrator osi,
    SP::OneStepNSProblem osnspb_velo,
    SP::OneStepNSProblem osnspb_pos)
  : TimeStepping(td, osi, osnspb_velo)
{
  (*_allNSProblems).resize(SICONOS_NB_OSNSP_TSP);
  insertNonSmoothProblem(osnspb_pos, SICONOS_OSNSP_TS_POS);
  _constraintTol = 10e-4;
}

// --- Destructor ---
TimeSteppingProjectOnConstraints::~TimeSteppingProjectOnConstraints()
{
}

void TimeSteppingProjectOnConstraints::initOSNS()
{
  TimeStepping::initOSNS();
  (*_allNSProblems)[SICONOS_OSNSP_TS_POS]->setLevelMin(1);
  (*_allNSProblems)[SICONOS_OSNSP_TS_POS]->setLevelMax(1);
}
void TimeSteppingProjectOnConstraints::newtonSolve(double criterion, unsigned int maxStep)
{
#ifdef TSPROJ_DEBUG
  cout << "TimeStepping::newtonSolve begin :\n";
#endif
  TimeStepping::newtonSolve(criterion, maxStep);
#ifdef TSPROJ_DEBUG
  cout << "TimeStepping::newtonSolve end : nbit=" << getNewtonNbSteps() << "\n";
#endif

  int info = 0;
#ifdef TSPROJ_DEBUG
  cout << "TimeSteppingProjectOnConstraints::newtonSolve begin projection:\n";
#endif
  SP::DynamicalSystemsGraph dsGraph = model()->nonSmoothDynamicalSystem()->dynamicalSystems();
  for (DynamicalSystemsGraph::VIterator vi = dsGraph->begin(); vi != dsGraph->end(); ++vi)
  {
    SP::DynamicalSystem ds = dsGraph->bundle(*vi);
    Type::Siconos dsType = Type::value(*ds);
    if (dsType != Type::NewtonEulerDS)
      RuntimeException::selfThrow("TS:: - ds is not from NewtonEulerDS.");
    SP::NewtonEulerDS neds = boost::static_pointer_cast<NewtonEulerDS>(ds);
    *(neds->deltaq()) = *(neds->q());
  }
  bool runningNewton = true;
  int cmp = 0;
  SP::InteractionsSet allInteractions = model()->nonSmoothDynamicalSystem()->interactions();
  for (InteractionsIterator it = allInteractions->begin(); it != allInteractions->end(); it++)
  {
    double criteria = (*it)->relation()->interaction()->y(0)->getValue(0);
    if (Type::value(*((*it)->nonSmoothLaw())) ==  Type::NewtonImpactFrictionNSL ||
        Type::value(*((*it)->nonSmoothLaw())) == Type::NewtonImpactNSL)
    {
      SP::NewtonEulerRImpact ri = boost::static_pointer_cast<NewtonEulerRImpact> ((*it)->relation());
      if (criteria < -1e-7)
      {
        ri->_isOnContact = true;
      }
      else
      {
        ri->_isOnContact = false;
      }
    }
    if (criteria < -_constraintTol)
      runningNewton = true;
  }


  while (runningNewton && cmp < 100)
  {
    cmp++;
    printf("TimeSteppingProjectOnConstraints Newton step = %d\n", cmp);

    runningNewton = false;
    for (InteractionsIterator it = allInteractions->begin(); it != allInteractions->end(); it++)
    {
      (*it)->relation()->computeJach(getTkp1());
      (*it)->relation()->computeh(getTkp1());
      double criteria = (*it)->relation()->interaction()->y(0)->getValue(0);

      if (criteria < -1e-4)
        runningNewton = true;
    }
    info = 0;
    if (runningNewton)
    {
      info = computeOneStepNSProblem(SICONOS_OSNSP_TS_POS);
      for (DynamicalSystemsGraph::VIterator vi = dsGraph->begin(); vi != dsGraph->end(); ++vi)
      {
        SP::DynamicalSystem ds = dsGraph->bundle(*vi);
        Type::Siconos dsType = Type::value(*ds);
        if (dsType != Type::NewtonEulerDS)
          RuntimeException::selfThrow("TS:: - ds is not from NewtonEulerDS.");
        SP::NewtonEulerDS neds = boost::static_pointer_cast<NewtonEulerDS>(ds);

        neds->normalizeq();
      }

      updateWorldFromDS();
    }
    //cout<<"||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||  Z:"<<endl;
    //(*_allNSProblems)[SICONOS_OSNSP_TS_POS]->display();
    //(boost::static_pointer_cast<LinearOSNS>((*_allNSProblems)[SICONOS_OSNSP_TS_POS]))->z()->display();
    if (info)
      cout << "TimeSteppingProjectOnConstraints project on constraints failed." << endl ;
    //cout<<"during projection before normalizing of q:\n";
    //for (InteractionsIterator it = allInteractions->begin(); it != allInteractions->end(); it++)
    //{
    //  (*it)->relation()->computeh(getTkp1());
    //}
  }

  for (DynamicalSystemsGraph::VIterator vi = dsGraph->begin(); vi != dsGraph->end(); ++vi)
  {
    SP::DynamicalSystem ds = dsGraph->bundle(*vi);
    Type::Siconos dsType = Type::value(*ds);
    if (dsType != Type::NewtonEulerDS)
      RuntimeException::selfThrow("TS:: - ds is not from NewtonEulerDS.");
    SP::NewtonEulerDS neds = boost::static_pointer_cast<NewtonEulerDS>(ds);
    neds->normalizeq();
    // SP::SiconosVector dotq = neds->dotq();
    // SP::SiconosVector q = neds->q();
    // SP::SiconosVector qold = neds->qMemory()->getSiconosVector(0);
    // double h = timeStep();
    // dotq->setValue(3,(q->getValue(3)-qold->getValue(3))/h);
    // dotq->setValue(4,(q->getValue(4)-qold->getValue(4))/h);
    // dotq->setValue(5,(q->getValue(5)-qold->getValue(5))/h);
    // dotq->setValue(6,(q->getValue(6)-qold->getValue(6))/h);
    neds->updateT();
    //continue;
    /*compute the new velocity seeing the work of fext*/
    *(neds->deltaq()) -= *(neds->q());
    double  n2q = neds->deltaq()->norm2();
    double n2 = 0.0;
    if (neds->fExt())
      n2 = neds->fExt()->norm2();
    if (n2 > 1e-7 && n2q > 1e-14)
    {
      //if (n2q < 1e-14)
      //  continue;

      SP::SimpleVector FextNorm(new SimpleVector(3));
      FextNorm->setValue(0, neds->fExt()->getValue(0));
      FextNorm->setValue(1, neds->fExt()->getValue(1));
      FextNorm->setValue(2, neds->fExt()->getValue(2));
{'fraction_non_alphanumeric': 0.16821363359707853, 'fraction_numerical': 0.011944613511868533, 'mean_word_length': 4.048003072196621, 'pattern_counts': {'":': 0, '<': 67, '<?xml version=': 0, '>': 180, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1889070', 'n_tokens_mistral': 4554, 'n_tokens_neox': 4372, 'n_words': 891}