Loi_horaire.cpp

/****************************************************************************
* Copyright (c) 2026, CEA
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*****************************************************************************/
 
#include <Schema_Temps_base.h>
#include <Probleme_base.h>
#include <Loi_horaire.h>
#include <TRUST_Ref.h>
#include <SFichier.h>
#include <sys/stat.h>
#include <EChaine.h>
#include <Param.h>
 
Implemente_instanciable(Loi_horaire,"Loi_horaire",Objet_U);
// XD loi_horaire objet_u loi_horaire BRACE to define the movement with a time-dependant law for the solid interface.
 
Sortie& Loi_horaire::printOn(Sortie& os) const { return os; }
 
Entree& Loi_horaire::readOn(Entree& is)
{
  EChaine x(dimension==3?"3 0. 0. 0.":"2 0. 0.");
  x >> position_;
  EChaine v(dimension==3?"3 0. 0. 0.":"2 0. 0.");
  v >> vitesse_;
  EChaine r(dimension==3?"9 1. 0. 0. 0. 1. 0. 0. 0. 1.":"4 1. 0. 0. 1.");
  r >> rotation_;
  EChaine drdt(dimension==3?"9 0. 0. 0. 0. 0. 0. 0. 0. 0.":"4 0. 0. 0. 0.");
  drdt >> derivee_rotation_;
 
  // Reading of dataset
  Param param(que_suis_je());
  param.ajouter("position",&position_);    // XD attr position listchaine position OPT Vecteur position (default: zero
  // XD_CONT vector of length `dimension`)
  param.ajouter("vitesse",&vitesse_);      // XD attr vitesse listchaine vitesse OPT Vecteur vitesse (default: zero
  // XD_CONT vector of length `dimension`)
  param.ajouter("rotation",&rotation_);    // XD attr rotation listchaine rotation OPT Matrice de passage
  param.ajouter("derivee_rotation",&derivee_rotation_);          // XD attr derivee_rotation listchaine derivee_rotation OPT Derivee matrice de passage
  param.ajouter("verification_derivee",&verification_derivee_);  // XD attr verification_derivee entier verification_derivee OPT not_set
  param.ajouter("impr",&impr_);            // XD attr impr entier impr OPT Whether to print output
  param.lire_avec_accolades_depuis(is);
 
  // Verification of what was read: (why does le_nom() return nothing???
  if (position_.valeurs().size_array()!=dimension)
    {
      Cerr << "The position vector of the schedule law " << le_nom() << " must have " << dimension << " components." << finl;
      Process::exit();
    }
  if (vitesse_.valeurs().size_array()!=dimension)
    {
      Cerr << "The velocity vector of the schedule law " << le_nom() << " must have " << dimension << " components." << finl;
      Process::exit();
    }
  if (rotation_.valeurs().size_array()!=dimension*dimension)
    {
      Cerr << "The matrix rotation of the schedule law " << le_nom() << " must be read" << finl;
      Cerr << "as a vector of " << dimension*dimension << " components." << finl;
      Process::exit();
    }
  if (derivee_rotation_.valeurs().size_array()!=dimension*dimension)
    {
      Cerr << "The matrix derivee_rotation of the schedule law " << le_nom() << " must be read" << finl;
      Cerr << "as a vector of " << dimension*dimension << " components." << finl;
      Process::exit();
    }
  return is;
}
 
inline void check(const DoubleTab& mat)
{
  // Champ_Fonc_t DoubleTab(1, nb_comp)
  if (mat.dimension(0)!=1)
    {
      Cerr << "Problem in Loi_horaire::check" << finl;
      Cerr << "Format of the array of values of Champ_Fonc_t" << finl;
      Cerr << "Contact TRUST support." << finl;
      Process::exit();
    }
}
 
// Matrix*Vector product stored in arrays
inline void multiplie(const DoubleTab& matrice, ArrOfDouble& vecteur)
{
  check(matrice);
  ArrOfDouble x(vecteur);
  vecteur=0;
  int dimension = vecteur.size_array();
  for (int i=0; i<dimension; i++)
    for (int j=0; j<dimension; j++)
      vecteur[i] += matrice(0,i*dimension+j) * x[j];
}
 
// Product Inverse(Matrix)*Vector in arrays
inline void inverse(const DoubleTab& mat, ArrOfDouble& x)
{
  check(mat);
  int dimension = x.size_array();
  if (dimension==2)
    {
      const double a=mat(0,0);
      const double b=mat(0,1);
      const double c=mat(0,2);
      const double d=mat(0,3);
      double det=a*d-b*c;
      if (det==0)
        {
          Cerr << "Matrix of order 2 not invertible in Loi_horaire::inverse" << finl;
          Cerr << mat << finl;
          Process::exit();
        }
      double y0=(d*x[0]-b*x[1])/det;
      double y1=(-c*x[0]+a*x[1])/det;
      x[0]=y0;
      x[1]=y1;
    }
  else if (dimension==3)
    {
      const double a=mat(0,0);
      const double b=mat(0,1);
      const double c=mat(0,2);
      const double d=mat(0,3);
      const double e=mat(0,4);
      const double f=mat(0,5);
      const double g=mat(0,6);
      const double h=mat(0,7);
      const double i=mat(0,8);
 
      double det=a*(e*i-f*h)
                 -b*(d*i-f*g)
                 +c*(d*h-e*g);
      if (det==0)
        {
          Cerr << "Matrix of order 3 not invertible in Loi_horaire::inverse" << finl;
          Cerr << mat << finl;
          Process::exit();
        }
      double y0=((e*i-f*h)*x[0]
                 +(c*h-b*i)*x[1]
                 +(b*f-c*e)*x[2])/det;
      double y1=((f*g-d*i)*x[0]
                 +(a*i-c*g)*x[1]
                 +(c*d-a*f)*x[2])/det;
      double y2=((d*h-e*g)*x[0]
                 +(b*g-a*h)*x[1]
                 +(a*e-b*d)*x[2])/det;
      x[0]=y0;
      x[1]=y1;
      x[2]=y2;
    }
  else
    {
      Cerr << "Case not provided in Loi_horaire::inverse()" << finl;
      Process::exit();
    }
}
 
/* Returns an array containing the position xM(t) of a point M at time t
   as a function of time t0 and its position xM(t0) at time t0.
   To do this, we use the following relations:
   xM(t)=xG(t)+R(t)(xM(0)-xG(0))
   xM(t0)=xG(t0)+R(t0)(xM(0)-xG(0))
   We have:
   xM(0)-xG(0)=R-1(t0)(xM(t0)-xG(t0))
   Therefore:
   xM(t) = xG(t)+R(t)R-1(t0)(xM(t0)-xG(t0))
*/
ArrOfDouble Loi_horaire::position(const double t, const double t0, const ArrOfDouble& xMt0)
{
  ArrOfDouble xMt(xMt0);                // xM(t0)
  position_.me_calculer(t0);                // xG(t0)
  xMt -= position_.valeurs();                // xM(t0)-xG(t0)
  rotation_.me_calculer(t0);                // R(t0)
  inverse(rotation_.valeurs(),xMt);        // R-1(t0)(xM(t0)-xG(t0))
  rotation_.me_calculer(t);                // R(t)
  multiplie(rotation_.valeurs(),xMt);        // R(t)R-1(t0)(xM(t0)-xG(t0))
  position_.me_calculer(t);                // xG(t)
  xMt += position_.valeurs();                // xM(t) = xG(t)+R(t)R-1(t0)(xM(t0)-xG(t0))
  return xMt;
}
 
/* Returns an array containing the velocity vM(t) of a point M at time t
   as a function of its position xM(t) at time t. To know this velocity of point M,
   we use the following relations:
   xM(t)=xG(t)+R(t)(xM(0)-xG(0))
   vM(t)=vG(t)+R'(t)(xM(0)-xG(0))
   We have:
   xM(0)-xG(0)=R-1(t)(xM(t)-xG(t))
   Therefore:
   vM(t) = vG(t)+R'(t)R-1(t)(xM(t)-xG(t))
*/
ArrOfDouble Loi_horaire::vitesse(const double t, const ArrOfDouble& xMt)
{
  ArrOfDouble vMt(xMt);                        // xM(t)
  position_.me_calculer(t);                        // xG(t)
  vMt -= position_.valeurs();                        // xM(t)-xG(t)
  rotation_.me_calculer(t);                        // R(t)
  inverse(rotation_.valeurs(),vMt);                // R-1(t)(xM(t)-xG(t))
  derivee_rotation_.me_calculer(t);                // R'(t)
  multiplie(derivee_rotation_.valeurs(),vMt);        // R'(t)R-1(t)(xM(t)-xG(t))
  vitesse_.me_calculer(t);                        // vG(t)
  vMt += vitesse_.valeurs();                        // vM(t) = vG(t)+R'(t)R-1(t)(xM(t)-xG(t))
  return vMt;
}
 
// Verification of consistency of velocity derivatives and derivee_rotation
// with respect to position and rotation. We use a limited development of order 2
// |f(t+dt)-f(t)-dt*f'(t)|=|0.5*f''(t)*dt*dt+O(dt*dt*dt)|>|10*f''(t)*dt*dt|
// An error on expressions with zero second derivative will therefore not be detected...
void Loi_horaire::verifier_derivee(const double t)
{
  if (verification_derivee_)
    {
      double dt=0.001*t;
      DoubleVect err_t(position_.valeurs());
      position_.me_calculer(t+dt);                        // xG(t+dt)
      err_t=position_.valeurs();                        // xG(t+dt)
      position_.me_calculer(t);                                // xG(t)
      err_t-=position_.valeurs();                        // xG(t+dt)-xG(t)
      vitesse_.me_calculer(t);                                // vG(t)
      err_t.ajoute(-dt, vitesse_.valeurs());                        // xG(t+dt)-xG(t)-dt*vG(t)
      // Evaluation of a threshold with the second derivative xG'' i.e. vG'
      ArrOfDouble seuil_t(position_.valeurs());
      vitesse_.me_calculer(t+dt);                        // vG(t+dt)
      seuil_t=vitesse_.valeurs();
      vitesse_.me_calculer(t);                                // vG(t)
      seuil_t-=vitesse_.valeurs();
      seuil_t*=100*dt;                                        // ~vG'(t)*dt*dt
      int n=err_t.size_array();
      for (int i=0; i<n; i++)
        {
          double seuil=std::fabs(seuil_t[i]);
          if (!est_egal(seuil,0) && std::fabs(err_t(i))>seuil)
            {
              Cerr << "At time " << t << ", inconsistency in the schedule law " << le_nom() << finl;
              Cerr << "The expression of the component " << i << " of velocity does not appear" << finl;
              Cerr << "to be the time derivative of the position expression." << finl;
              Cerr << "Verify the expressions of this schedule law in your data set." << finl;
              Cerr << "If the expressions are correct after all, add the option 'verification_derivee 0'" << finl;
              Cerr << "in the law to not to make this verification." << finl;
              Process::exit();
            }
        }
      DoubleTab err_r(rotation_.valeurs());
      rotation_.me_calculer(t+dt);                        // R(t+dt)
      err_r=rotation_.valeurs();                        // R(t+dt)
      rotation_.me_calculer(t);                                // R(t)
      err_r-=rotation_.valeurs();                        // R(t+dt)-R(t)
      derivee_rotation_.me_calculer(t);                        // R'(t)
      err_r.ajoute(-dt, derivee_rotation_.valeurs());                // R(t+dt)-R(t)-dt*R'(t)
      // Evaluation of a threshold with the second derivative of R i.e. R''
      DoubleTab seuil_r(rotation_.valeurs());
      derivee_rotation_.me_calculer(t+dt);                // R'(t+dt)
      seuil_r=derivee_rotation_.valeurs();
      derivee_rotation_.me_calculer(t);                        // R'(t)
      seuil_r-=derivee_rotation_.valeurs();                // R'(t+dt)-R'(t)
      seuil_r*=100*dt;                                        // ~R''(t)*dt*dt
      int ni=err_r.dimension(0);
      int nj=err_r.dimension(1);
      for (int i=0; i<ni; i++)
        for (int j=0; j<nj; j++)
          {
            double seuil=std::fabs(seuil_r(i,j));
            if (!est_egal(seuil,0) && std::fabs(err_r(i,j))>seuil)
              {
                Cerr << "At time " << t << ", inconsistency in the schedule law " << le_nom() << finl;
                Cerr << "The expression of the component " << i*ni+j << " of derivee_rotation does not appear" << finl;
                Cerr << "to be the time derivative of the rotation expression." << finl;
                Cerr << "Verify the expressions of this schedule law in your data set" << finl;
                Cerr << "or add the option 'verification_derivee 0' in this law." << finl;
                Process::exit();
              }
          }
    }
}
 
void Loi_horaire::imprimer(const Schema_Temps_base& sch, const ArrOfDouble& coord_barycentre)
{
  if (Process::je_suis_maitre())
    {
      // File opening
      Nom nom_fichier(nom_du_cas());
      nom_fichier+="_loi_horaire_";
      nom_fichier+=le_nom();
      nom_fichier+=".out";
      SFichier fic; // * os=nullptr;
      struct stat f;
      // Create the file on first printing with header or if the file does not exist
      if (stat(nom_fichier,&f) || (sch.nb_impr()==1 && !sch.pb_base().reprise_effectuee()))
        {
          tester(sch);
          fic.ouvrir(nom_fichier);
          // Writing header
          fic << "# (xG,yG,...):Position du centre de gravite dont le mouvement est defini par la loi horaire " << le_nom() << finl;
          fic << "# (moy(xi),moy(yi),...):Position du barycentre des noeuds du maillage de l'interface." << finl;
          fic << "# Les trajectoires de ces 2 points doivent etre proches." << finl;
          fic << "# Pour visualiser dans gnuplot ces trajectoires, faire: " << finl;
          if (dimension==3)
            {
              fic << "# set param;splot '" << nom_fichier << "' using 2:3:4 with lines,'" << nom_fichier << "' using 5:6:7 with points" << finl;
              fic << "# Temps     xG(t)     yG(t)     zG(t)     moy(xi)   moy(yi)   moy(zi)" << finl;
            }
          else
            {
              fic << "# set param;plot '" << nom_fichier << "' using 2:3 with lines,'" << nom_fichier << "' using 4:5 with points" << finl;
              fic << "# Temps     xG(t)     yG(t)     moy(xi)   moy(yi)" << finl;
            }
        }
      else
        fic.ouvrir(nom_fichier,ios::app);
 
      fic.precision(sch.precision_impr());
      fic.setf(ios::scientific);
      // Writing t, xG(t), barycenter_coord
      double t=sch.temps_courant();
      fic << t;
      position_.me_calculer(t);
      for (int i=0; i<dimension; i++)
        fic << " " << position_.valeurs()(0,i);        // xG(t)
      for (int i=0; i<dimension; i++)
        fic << " " << coord_barycentre[i];                 // Barycenter of interface nodes
      fic << finl;
      // File closing
      fic.close();
    }
}
 
// Various tests of the class
void Loi_horaire::tester(const Schema_Temps_base& sch)
{
  // Matrix product and inversion test
  for (int dim=2; dim<=3; dim++)
    {
      DoubleTab mat(1,dim*dim);
      for (int i=0; i<dim; i++)
        for (int j=0; j<dim; j++)
          mat(0,i*dim+j)=(i>j?1+i+j:-2*j);
      ArrOfDouble vect(dim);
      for (int i=0; i<dim; i++)
        vect[i]=i;
      ArrOfDouble tmp(vect);
      multiplie(mat,vect);
      inverse(mat,vect);
      for (int i=0; i<dim; i++)
        if (!est_egal(tmp[i],vect[i]))
          {
            Cerr << "Loi_horaire::inverse is not validated for dim=" << dim << finl;
            Cerr << "A=" << mat << finl;
            Cerr << "x=" << tmp << finl;
            Cerr << "Inv(A)Ax=" << vect << finl;
            Cerr << "Contact TRUST support." << finl;
            Process::exit();
          }
    }
  // Verification between tinit and tmax
  double tinit=sch.temps_init();
  double tmax=sch.temps_max();
  // Test of derivatives:
  int n=100;
  for (int i=1; i<n; i++)
    {
      double temps=tinit+(tmax-tinit)*(i+1)/n;
      //Cerr << "Verification a t=" << temps << finl;
      verifier_derivee(temps);
    }
  // Test of position and velocity:
  double t=0.5*(tinit+tmax);
  double eps=1e-8;
  double dt=eps*t;
  ArrOfDouble pos(dimension);
  pos[0]=dimension*2;
  pos[1]=dimension*3;
  if (dimension==3) pos[2]=-dimension;
  // Verification that position(t,t,pos)=pos
  ArrOfDouble tmp(position(t,t,pos));
  for (int i=0; i<dimension; i++)
    if (!est_egal(tmp[i],pos[i]))
      {
        Cerr << "Loi_horaire::position is not validated for pos=position(t,t,pos):" << finl;
        Cerr << pos << finl;
        Cerr << tmp << finl;
        Cerr << "Contact TRUST support." << finl;
        Process::exit();
      }
  // Verification that velocity(t,pos)~(position(t+dt,t,pos)-pos)/dt;
  tmp=position(t+dt,t,pos);
  tmp-=pos;
  tmp/=dt;
  ArrOfDouble vit(vitesse(t,pos));
  ArrOfDouble relative_error(vit);
  relative_error-=tmp;
  relative_error/=(norme_array(tmp)!=0?norme_array(tmp):1);
  for (int i=0; i<dimension; i++)
    if (!est_egal(relative_error[i],0,0.001))
      {
        Cerr << "Loi_horaire::position is not validated for velocity(t,pos):" << finl;
        Cerr << "t=" << t << finl;
        Cerr << "dt=" << dt << finl;
        Cerr << "vit=" << vit << finl;
        Cerr << "tmp=" << tmp << finl;
        Cerr << "relative_error=" << relative_error << finl;
        Cerr << "Contact TRUST support." << finl;
        Process::exit();
      }
  Cerr << "Loi_horaire::tester() OK" << finl;
}