src/INTERP_KERNEL/InterpKernelRootsMultiDim.hxx

   1 // Copyright (C) 2022-2024  CEA, EDF
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Lesser General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2.1 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Lesser General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU Lesser General Public
  14 // License along with this library; if not, write to the Free Software
  15 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  16 //
  17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
  18 //
  19
  20 // Implementation coming from Numerical Recipes in C of 1994 (version 2.04) p 384
  21 // Root finding and non linear sets of equation using line searches and backtracking
  22
  23 #include "InterpKernelDenseMatrix.hxx"
  24 #include "InterpKernelLUDecomp.hxx"
  25 #include "InterpKernelQRDecomp.hxx"
  26 #include "InterpKernelException.hxx"
  27 #include "MCIdType.hxx"
  28
  29 #include <vector>
  30 #include <limits>
  31 #include <cmath>
  32
  33 template<class T>
  34 inline T sqr(const T a) { return a*a; }
  35
  36 namespace INTERP_KERNEL
  37 {
  38   template <class T>
  39   void LineSearches(const std::vector<double>& xold, const double fold, const std::vector<double>& g, std::vector<double> &p,
  40   std::vector<double> &x, double &f, const double stpmax, bool &check, T &func)
  41   {
  42     const double ALF=1.0e-4, TOLX=std::numeric_limits<double>::epsilon();
  43     double a,alam,alam2=0.0,alamin,b,disc,f2=0.0;
  44     double rhs1,rhs2,slope=0.0,sum=0.0,temp,test,tmplam;
  45     mcIdType i,n=xold.size();
  46     check=false;
  47     for (i=0;i<n;i++) sum += p[i]*p[i];
  48     sum=std::sqrt(sum);
  49     if (sum > stpmax)
  50       for (i=0;i<n;i++)
  51         p[i] *= stpmax/sum;
  52     for (i=0;i<n;i++)
  53       slope += g[i]*p[i];
  54     if (slope >= 0.0) THROW_IK_EXCEPTION("Roundoff problem in LineSearches.");
  55     test=0.0;
  56     for (i=0;i<n;i++)
  57     {
  58       temp=std::abs(p[i])/std::fmax(std::abs(xold[i]),1.0);
  59       if (temp > test) test=temp;
  60     }
  61     alamin=TOLX/test;
  62     alam=1.0;
  63     for (;;)
  64     {
  65       for (i=0;i<n;i++) x[i]=xold[i]+alam*p[i];
  66       f=func(x);
  67       if (alam < alamin)
  68       {
  69         for (i=0;i<n;i++) x[i]=xold[i];
  70         check=true;
  71         return;
  72       } else if (f <= fold+ALF*alam*slope) return;
  73       else
  74       {
  75         if (alam == 1.0)
  76           tmplam = -slope/(2.0*(f-fold-slope));
  77         else
  78         {
  79           rhs1=f-fold-alam*slope;
  80           rhs2=f2-fold-alam2*slope;
  81           a=(rhs1/(alam*alam)-rhs2/(alam2*alam2))/(alam-alam2);
  82           b=(-alam2*rhs1/(alam*alam)+alam*rhs2/(alam2*alam2))/(alam-alam2);
  83           if (a == 0.0) tmplam = -slope/(2.0*b);
  84           else {
  85               disc=b*b-3.0*a*slope;
  86               if (disc < 0.0) tmplam=0.5*alam;
  87               else if (b <= 0.0) tmplam=(-b+std::sqrt(disc))/(3.0*a);
  88               else tmplam=-slope/(b+std::sqrt(disc));
  89           }
  90           if (tmplam>0.5*alam)
  91               tmplam=0.5*alam;
  92         }
  93       }
  94       alam2=alam;
  95       f2 = f;
  96       alam=std::fmax(tmplam,0.1*alam);
  97     }
  98   }
  99   template <class T>
 100   class JacobianCalculator
 101   {
 102   private:
 103     const double EPS;
 104     T &func;
 105   public:
 106     JacobianCalculator(T &funcc) : EPS(1.0e-8),func(funcc) {}
 107     INTERP_KERNEL::DenseMatrix operator() (const std::vector<double>& x, const std::vector<double>& fvec)
 108     {
 109       mcIdType n=x.size();
 110       INTERP_KERNEL::DenseMatrix df(n,n);
 111       std::vector<double> xh=x;
 112       for (mcIdType j=0;j<n;j++)
 113       {
 114         double temp=xh[j];
 115         double h=EPS*std::abs(temp);
 116         if (h == 0.0) h=EPS;
 117         xh[j]=temp+h;
 118         h=xh[j]-temp;
 119         std::vector<double> f=func(xh);
 120         xh[j]=temp;
 121         for (mcIdType i=0;i<n;i++)
 122           df[i][j]=(f[i]-fvec[i])/h;
 123       }
 124       return df;
 125     }
 126   };
 127
 128   template <class T>
 129   class FMin
 130   {
 131   private:
 132     std::vector<double> fvec;
 133     T &func;
 134     mcIdType n;
 135   public:
 136     FMin(T &funcc) : func(funcc){}
 137     double operator() (const std::vector<double>& x)
 138     {
 139       n=x.size();
 140       double sum=0;
 141       fvec=func(x);
 142       for (mcIdType i=0;i<n;i++) sum += sqr(fvec[i]);
 143       return 0.5*sum;
 144     }
 145     std::vector<double>& getVector() { return fvec; }
 146   };
 147
 148   /*!
 149    * check is false on normal return.
 150    * check is true if the routine has converged to a local minimum.
 151    */
 152   template <class T>
 153   void SolveWithNewtonWithJacobian(std::vector<double> &x, bool &check, T &vecfunc,
 154     std::function<void(const std::vector<double>&, const std::vector<double>&, INTERP_KERNEL::DenseMatrix&)> jacobian)
 155   {
 156     const mcIdType MAXITS=200;
 157     const double TOLF=1.0e-8,TOLMIN=1.0e-12,STPMX=100.0;
 158     const double TOLX=std::numeric_limits<double>::epsilon();
 159     mcIdType i,j,its,n=x.size();
 160     double den,f,fold,stpmax,sum,temp,test;
 161     std::vector<double> g(n),p(n),xold(n);
 162     INTERP_KERNEL::DenseMatrix fjac(n,n);
 163     FMin<T> fmin(vecfunc);
 164     std::vector<double> &fvec=fmin.getVector();
 165     f=fmin(x);
 166     test=0.0;
 167     for (i=0;i<n;i++)
 168       if (std::abs(fvec[i]) > test) test=std::abs(fvec[i]);
 169     if (test < 0.01*TOLF)
 170     {
 171       check=false;
 172       return;
 173     }
 174     sum=0.0;
 175     for (i=0;i<n;i++) sum += sqr(x[i]);
 176     stpmax=STPMX*std::fmax(std::sqrt(sum),double(n));
 177     for (its=0;its<MAXITS;its++)
 178     {
 179       jacobian(x,fvec,fjac);//fjac=fdjac(x,fvec);
 180       for (i=0;i<n;i++)
 181       {
 182         sum=0.0;
 183         for (j=0;j<n;j++) sum += fjac[j][i]*fvec[j];
 184         g[i]=sum;
 185       }
 186       for (i=0;i<n;i++) xold[i]=x[i];
 187       fold=f;
 188       for (i=0;i<n;i++) p[i] = -fvec[i];
 189       INTERP_KERNEL::LUDecomp alu(fjac);
 190       alu.solve(p,p);
 191       LineSearches(xold,fold,g,p,x,f,stpmax,check,fmin);
 192       test=0.0;
 193       for (i=0;i<n;i++)
 194           if (std::abs(fvec[i]) > test) test=std::abs(fvec[i]);
 195       if (test < TOLF)
 196       {
 197         check=false;
 198         return;
 199       }
 200       if (check)
 201       {
 202         test=0.0;
 203         den=std::fmax(f,0.5*double(n));
 204         for (i=0;i<n;i++) {
 205             temp=std::abs(g[i])*std::fmax(std::abs(x[i]),1.0)/den;
 206             if (temp > test) test=temp;
 207         }
 208         check=(test < TOLMIN);
 209         if( check )
 210           return;
 211         else
 212           continue;
 213       }
 214       test=0.0;
 215       for (i=0;i<n;i++)
 216       {
 217         temp=(std::abs(x[i]-xold[i]))/std::fmax(std::abs(x[i]),1.0);
 218         if (temp > test) test=temp;
 219       }
 220       if (test < TOLX)
 221           return;
 222     }
 223     THROW_IK_EXCEPTION("MAXITS exceeded in SolveWithNewtonWithJacobian");
 224   }
 225
 226   /*!
 227    * check is false on normal return.
 228    * check is true if the routine has converged to a local minimum.
 229    */
 230   template <class T>
 231   void SolveWithNewton(std::vector<double> &x, bool &check, T &vecfunc)
 232   {
 233     JacobianCalculator<T> fdjac(vecfunc);
 234     auto myJacobian = [&fdjac,vecfunc](const std::vector<double>& x, const std::vector<double>& fvec, INTERP_KERNEL::DenseMatrix& fjac)
 235     {
 236       fjac = fdjac(x,fvec);
 237     };
 238     SolveWithNewtonWithJacobian(x,check,vecfunc,myJacobian);
 239   }
 240 }