src/math/math_FunctionSetRoot.cxx

   1 // Copyright (c) 1997-1999 Matra Datavision
   2 // Copyright (c) 1999-2012 OPEN CASCADE SAS
   3 //
   4 // The content of this file is subject to the Open CASCADE Technology Public
   5 // License Version 6.5 (the "License"). You may not use the content of this file
   6 // except in compliance with the License. Please obtain a copy of the License
   7 // at http://www.opencascade.org and read it completely before using this file.
   8 //
   9 // The Initial Developer of the Original Code is Open CASCADE S.A.S., having its
  10 // main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France.
  11 //
  12 // The Original Code and all software distributed under the License is
  13 // distributed on an "AS IS" basis, without warranty of any kind, and the
  14 // Initial Developer hereby disclaims all such warranties, including without
  15 // limitation, any warranties of merchantability, fitness for a particular
  16 // purpose or non-infringement. Please see the License for the specific terms
  17 // and conditions governing the rights and limitations under the License.
  18
  19 // pmn 15/05/97 pas de Gauss avec des pivot trop petit. SVD fait mieux
  20 // l'affaire + limitation de la longeur du pas + qq comentaire issus d'EUCLID3
  21 // pmn 10/06/97 refonte totale du traitement des bords + ajustement des init
  22 // et des tolerances pour brent...
  23
  24 //#ifndef DEB
  25 #define No_Standard_RangeError
  26 #define No_Standard_OutOfRange
  27 #define No_Standard_DimensionError
  28 //#endif
  29
  30 //math_FunctionSetRoot.cxx
  31
  32
  33 #include <math_FunctionSetRoot.ixx>
  34 #include <Standard_DimensionError.hxx>
  35 #include <math_Gauss.hxx>
  36 #include <math_SVD.hxx>
  37 #include <math_GaussLeastSquare.hxx>
  38 #include <math_IntegerVector.hxx>
  39 #include <math_Function.hxx>
  40 #include <math_BrentMinimum.hxx>
  41 #include <math_FunctionSetWithDerivatives.hxx>
  42 #include <Precision.hxx>
  43
  44
  45 //===========================================================================
  46 // - A partir d une solution de depart, recherche d une direction.( Newton la
  47 // plupart du temps, gradient si Newton echoue.
  48 // - Recadrage au niveau des bornes avec recalcul de la direction si une
  49 // inconnue a une valeur imposee.
  50 // -Si On ne sort pas des bornes
  51 //   Tant que (On ne progresse pas assez ou on ne change pas de direction)
  52 //    . Si (Progression encore possible)
  53 //        Si (On ne sort pas des bornes)
  54 //          On essaye de progresser dans cette meme direction.
  55 //        Sinon
  56 //          On diminue le pas d'avancement ou on change de direction.
  57 //      Sinon
  58 //        Si on depasse le minimum
  59 //          On fait une interpolation parabolique.
  60 // - Si on a progresse sur F
  61 //     On fait les tests d'arret
  62 //   Sinon
  63 //     On change de direction
  64 //============================================================================
  65
  66 #define FSR_DEBUG(arg)
  67 // Uncomment the following code to have debug output to cout
  68 /* * /
  69 static Standard_Boolean mydebug = Standard_False;
  70 #undef FSR_DEBUG
  71 #define FSR_DEBUG(arg) {if (mydebug) { cout << arg << endl; }}
  72 /* */
  73
  74 class MyDirFunction : public math_Function
  75 {
  76
  77      math_Vector *P0;
  78      math_Vector *Dir;
  79      math_Vector *P;
  80      math_Vector *FV;
  81      math_FunctionSetWithDerivatives *F;
  82
  83 public :
  84
  85      MyDirFunction(math_Vector& V1,
  86                    math_Vector& V2,
  87                    math_Vector& V3,
  88                    math_Vector& V4,
  89                    math_FunctionSetWithDerivatives& f) ;
  90
  91      void Initialize(const math_Vector& p0, const math_Vector& dir) const;
  92 //For hp :
  93      Standard_Boolean Value(const math_Vector& Sol, math_Vector& FF,
  94                             math_Matrix& DF, math_Vector& GH,
  95                             Standard_Real& F2, Standard_Real& Gnr1);
  96 //     Standard_Boolean MyDirFunction::Value(const math_Vector& Sol, math_Vector& FF,
  97 //                                         math_Matrix& DF, math_Vector& GH,
  98 //                                         Standard_Real& F2, Standard_Real& Gnr1);
  99      Standard_Boolean Value(const Standard_Real x, Standard_Real& fval) ;
 100
 101 };
 102
 103 MyDirFunction::MyDirFunction(math_Vector& V1,
 104                              math_Vector& V2,
 105                              math_Vector& V3,
 106                              math_Vector& V4,
 107                              math_FunctionSetWithDerivatives& f) {
 108
 109   P0  = &V1;
 110   Dir = &V2;
 111   P   = &V3;
 112   FV =  &V4;
 113   F   = &f;
 114 }
 115
 116 void MyDirFunction::Initialize(const math_Vector& p0,
 117                                     const math_Vector& dir)  const
 118 {
 119   *P0 = p0;
 120   *Dir = dir;
 121 }
 122
 123
 124 Standard_Boolean MyDirFunction::Value(const Standard_Real x,
 125                                       Standard_Real& fval)
 126 {
 127   Standard_Real p;
 128   for(Standard_Integer i = P->Lower(); i <= P->Upper(); i++) {
 129     p = Dir->Value(i);
 130     P->Value(i) = p * x + P0->Value(i);
 131   }
 132   if( F->Value(*P, *FV) ) {
 133
 134     Standard_Real aVal = 0.;
 135
 136     for(Standard_Integer i = FV->Lower(); i <= FV->Upper(); i++) {
 137       aVal = FV->Value(i);
 138       if(aVal < 0.) {
 139         if(aVal <= -1.e+100) // Precision::HalfInfinite() later
 140 //       if(Precision::IsInfinite(Abs(FV->Value(i)))) {
 141 //      fval = Precision::Infinite();
 142         return Standard_False;
 143       }
 144       else if(aVal >= 1.e+100) // Precision::HalfInfinite() later
 145         return Standard_False;
 146     }
 147
 148     fval = 0.5 * (FV->Norm2());
 149     return Standard_True;
 150   }
 151   return Standard_False;
 152 }
 153
 154 Standard_Boolean  MyDirFunction::Value(const math_Vector& Sol,
 155                                        math_Vector& FF,
 156                                        math_Matrix& DF,
 157                                        math_Vector& GH,
 158                                        Standard_Real& F2,
 159                                        Standard_Real& Gnr1)
 160 {
 161   if( F->Values(Sol, FF, DF) ) {
 162
 163     Standard_Real aVal = 0.;
 164
 165     for(Standard_Integer i = FF.Lower(); i <= FF.Upper(); i++) {
 166 // modified by NIZHNY-MKK  Mon Oct  3 17:56:50 2005.BEGIN
 167       aVal = FF.Value(i);
 168       if(aVal < 0.) {
 169         if(aVal <= -1.e+100) // Precision::HalfInfinite() later
 170 //       if(Precision::IsInfinite(Abs(FF.Value(i)))) {
 171 //      F2 = Precision::Infinite();
 172 //      Gnr1 = Precision::Infinite();
 173           return Standard_False;
 174       }
 175       else if(aVal >= 1.e+100) // Precision::HalfInfinite() later
 176         return Standard_False;
 177 // modified by NIZHNY-MKK  Mon Oct  3 17:57:05 2005.END
 178     }
 179
 180
 181     F2 = 0.5 * (FF.Norm2());
 182     GH.TMultiply(DF, FF);
 183     Gnr1 = GH.Norm2();
 184     return Standard_True;
 185   }
 186   return Standard_False;
 187 }
 188
 189
 190 //--------------------------------------------------------------
 191 static Standard_Boolean MinimizeDirection(const math_Vector&   P0,
 192                                           const math_Vector&   P1,
 193                                           const math_Vector&   P2,
 194                                           const Standard_Real  F1,
 195                                           math_Vector&         Delta,
 196                                           const math_Vector&   Tol,
 197                                           MyDirFunction& F)
 198 // Purpose : minimisation a partir de 3 points
 199 //-------------------------------------------------------
 200 {
 201   // (1) Evaluation d'un tolerance parametrique 1D
 202   Standard_Real tol1d = 2.1 , invnorme, tsol;
 203   Standard_Real Eps = 1.e-16;
 204   Standard_Real ax, bx, cx;
 205
 206   for (Standard_Integer ii =1; ii<=Tol.Length(); ii++) {
 207     invnorme = Abs(Delta(ii));
 208     if  (invnorme>Eps) tol1d = Min(tol1d, Tol(ii) / invnorme);
 209   }
 210   if (tol1d > 1.9) return Standard_False; //Pas la peine de se fatiguer
 211   tol1d /= 3;
 212
 213 //JR/Hp :
 214   math_Vector PP0 = P0 ;
 215   math_Vector PP1 = P1 ;
 216   Delta = PP1 - PP0;
 217 //  Delta = P1 - P0;
 218   invnorme = Delta.Norm();
 219   if (invnorme <= Eps) return Standard_False;
 220   invnorme = ((Standard_Real) 1) / invnorme;
 221
 222   F.Initialize(P1, Delta);
 223
 224   // (2) On minimise
 225   FSR_DEBUG ("      minimisation dans la direction")
 226   ax = -1; bx = 0;
 227   cx = (P2-P1).Norm()*invnorme;
 228   if (cx < 1.e-2) return Standard_False;
 229   math_BrentMinimum Sol(F, ax, bx, cx, tol1d, 100, tol1d);
 230   if(Sol.IsDone()) {
 231     tsol = Sol.Location();
 232     if (Sol.Minimum() < F1) {
 233       Delta.Multiply(tsol);
 234       return Standard_True;
 235     }
 236   }
 237   return Standard_False;
 238 }
 239
 240 //----------------------------------------------------------------------
 241 static Standard_Boolean MinimizeDirection(const math_Vector&   P,
 242                                           math_Vector&   Dir,
 243                                           const Standard_Real& PValue,
 244                                           const Standard_Real& PDirValue,
 245                                           const math_Vector&   Gradient,
 246                                           const math_Vector&   DGradient,
 247                                           const math_Vector&   Tol,
 248                                           MyDirFunction& F)
 249 // Purpose: minimisation a partir de 2 points et une derives
 250 //----------------------------------------------------------------------
 251
 252 {
 253   // (0) Evaluation d'un tolerance parametrique 1D
 254   Standard_Boolean good = Standard_False;
 255   Standard_Real Eps = 1.e-20;
 256   Standard_Real tol1d = 1.1, Result = PValue, absdir;
 257
 258   for (Standard_Integer ii =1; ii<=Tol.Length(); ii++) {
 259     absdir = Abs(Dir(ii));
 260     if (absdir >Eps) tol1d = Min(tol1d, Tol(ii) / absdir);
 261   }
 262   if (tol1d > 0.9) return Standard_False;
 263
 264   // (1) On realise une premiere interpolation quadratique
 265   Standard_Real ax, bx, cx, df1, df2, Delta, tsol, fsol, tsolbis;
 266   FSR_DEBUG("     essai d interpolation")
 267
 268   df1 = Gradient*Dir;
 269   df2 = DGradient*Dir;
 270
 271   if (df1<-Eps && df2>Eps) { // cuvette
 272     tsol = - df1 / (df2 - df1);
 273   }
 274   else {
 275     cx = PValue;
 276     bx = df1;
 277     ax = PDirValue - (bx+cx);
 278
 279     if (Abs(ax) <= Eps) { // cas lineaire
 280       if ((Abs(bx) >= Eps)) tsol = - cx/bx;
 281       else                  tsol = 0;
 282     }
 283     else { // cas quadratique
 284       Delta = bx*bx - 4*ax*cx;
 285       if (Delta > 1.e-9) {
 286         // il y a des racines, on prend la plus proche de 0
 287         Delta = Sqrt(Delta);
 288         tsol = -(bx + Delta);
 289         tsolbis = (Delta - bx);
 290         if (Abs(tsolbis) < Abs(tsol)) tsol = tsolbis;
 291         tsol /= 2*ax;
 292       }
 293       else {
 294         // pas ou peu de racine : on "extremise"
 295         tsol = -(0.5*bx)/ax;
 296       }
 297     }
 298   }
 299
 300   if (Abs(tsol) >= 1) return Standard_False; //resultat sans interet
 301
 302   F.Initialize(P, Dir);
 303   F.Value(tsol, fsol);
 304
 305   if (fsol<PValue) {
 306     good = Standard_True;
 307     Result = fsol;
 308     FSR_DEBUG("t= "<<tsol<<" F = " << fsol << " OldF = "<<PValue)
 309   }
 310
 311   // (2) Si l'on a pas assez progresser on realise une recherche
 312   //     en bonne et due forme, a partir des inits precedents
 313   if ((fsol > 0.2*PValue) && (tol1d < 0.5)) {
 314
 315     if (tsol <0) {
 316       ax = tsol; bx = 0.0; cx = 1.0;
 317     }
 318     else {
 319       ax = 0.0; bx = tsol; cx = 1.0;
 320     }
 321     FSR_DEBUG(" minimisation dans la direction")
 322     math_BrentMinimum Sol(F, ax, bx, cx, tol1d, 100, tol1d);
 323     if(Sol.IsDone()) {
 324       if (Sol.Minimum() <= Result) {
 325         tsol = Sol.Location();
 326         good = Standard_True;
 327         FSR_DEBUG("t= "<<tsol<<" F ="<< Sol.Minimum() << " OldF = "<<Result)
 328       }
 329     }
 330   }
 331   if (good) {
 332     // mise a jour du Delta
 333     Dir.Multiply(tsol);
 334   }
 335   return good;
 336 }
 337
 338 //------------------------------------------------------
 339 static void SearchDirection(const math_Matrix& DF,
 340                             const math_Vector& GH,
 341                             const math_Vector& FF,
 342                             Standard_Boolean ChangeDirection,
 343                             const math_Vector& InvLengthMax,
 344                             math_Vector& Direction,
 345                             Standard_Real& Dy)
 346
 347 {
 348   Standard_Integer Ninc = DF.ColNumber(), Neq = DF.RowNumber();
 349   Standard_Real Eps = 1.e-32;
 350   if (!ChangeDirection) {
 351     if (Ninc == Neq) {
 352       for (Standard_Integer i = FF.Lower(); i <= FF.Upper(); i++) {
 353         Direction(i) = -FF(i);
 354       }
 355       math_Gauss Solut(DF, 1.e-9);
 356       if (Solut.IsDone()) Solut.Solve(Direction);
 357       else { // we have to "forget" singular directions.
 358         FSR_DEBUG(" Matrice singuliere : On prend SVD")
 359         math_SVD SolvebySVD(DF);
 360         if (SolvebySVD.IsDone()) SolvebySVD.Solve(-1*FF, Direction);
 361         else ChangeDirection = Standard_True;
 362       }
 363     }
 364     else if (Ninc > Neq) {
 365       math_SVD Solut(DF);
 366       if (Solut.IsDone()) Solut.Solve(-1*FF, Direction);
 367       else ChangeDirection = Standard_True;
 368     }
 369     else if (Ninc < Neq) {         // Calcul par GaussLeastSquare
 370       math_GaussLeastSquare Solut(DF);
 371       if (Solut.IsDone()) Solut.Solve(-1*FF, Direction);
 372       else ChangeDirection = Standard_True;
 373     }
 374   }
 375   // Il vaut mieux interdire des directions trops longue
 376   // Afin de blinder les cas trop mal conditionne
 377   // PMN 12/05/97 Traitement des singularite dans les conges
 378   // Sur des surfaces periodiques
 379
 380   Standard_Real ratio = Abs( Direction(Direction.Lower())
 381                             *InvLengthMax(Direction.Lower()) );
 382   Standard_Integer i;
 383   for (i = Direction.Lower()+1; i<=Direction.Upper(); i++) {
 384     ratio = Max(ratio,  Abs( Direction(i)*InvLengthMax(i)) );
 385   }
 386   if (ratio > 1) {
 387     Direction /= ratio;
 388   }
 389
 390   Dy = Direction*GH;
 391   if (Dy >= -Eps) { // newton "ne descend pas" on prend le gradient
 392     ChangeDirection = Standard_True;
 393   }
 394   if (ChangeDirection) { // On va faire un gradient !
 395     for (i = Direction.Lower(); i <= Direction.Upper(); i++) {
 396       Direction(i) = - GH(i);
 397     }
 398     Dy = - (GH.Norm2());
 399   }
 400 }
 401
 402
 403 //=====================================================================
 404 static void SearchDirection(const math_Matrix& DF,
 405                             const math_Vector& GH,
 406                             const math_Vector& FF,
 407                             const math_IntegerVector& Constraints,
 408 //                          const math_Vector& X, // Le point d'init
 409                             const math_Vector& , // Le point d'init
 410                             Standard_Boolean ChangeDirection,
 411                             const math_Vector& InvLengthMax,
 412                             math_Vector& Direction,
 413                             Standard_Real& Dy)
 414 //Purpose : Recherche une direction (et un pas si Newton Fonctionne) le long
 415 //          d'une frontiere
 416 //=====================================================================
 417 {
 418   Standard_Integer Ninc = DF.ColNumber(), Neq = DF.RowNumber();
 419   Standard_Integer i, j, k, Cons = 0;
 420
 421   // verification sur les bornes imposees:
 422
 423   for (i = 1; i <= Ninc; i++) {
 424     if (Constraints(i) != 0) Cons++;
 425     // sinon le systeme a resoudre ne change pas.
 426   }
 427
 428   if (Cons == 0) {
 429     SearchDirection(DF, GH, FF, ChangeDirection, InvLengthMax,
 430                     Direction, Dy);
 431   }
 432   else if (Cons == Ninc) { // il n'y a plus rien a faire...
 433     for(Standard_Integer i = Direction.Lower(); i <= Direction.Upper(); i++) {
 434         Direction(i) = 0;
 435       }
 436     Dy = 0;
 437   }
 438   else { //(1) Cas general : On definit un sous probleme
 439     math_Matrix DF2(1, Neq, 1, Ninc-Cons);
 440     math_Vector MyGH(1, Ninc-Cons);
 441     math_Vector MyDirection(1, Ninc-Cons);
 442     math_Vector MyInvLengthMax(1, Ninc);
 443
 444     for (k=1, i = 1; i <= Ninc; i++) {
 445       if (Constraints(i) == 0) {
 446           MyGH(k) = GH(i);
 447           MyInvLengthMax(k) = InvLengthMax(i);
 448           MyDirection(k) = Direction(i);
 449           for (j = 1; j <= Neq; j++) {
 450            DF2(j, k) = DF(j, i);
 451          }
 452           k++; //on passe a l'inconnue suivante
 453         }
 454     }
 455     //(2) On le resoud
 456     SearchDirection(DF2, MyGH, FF, ChangeDirection, MyInvLengthMax,
 457                     MyDirection, Dy);
 458
 459     // (3) On l'interprete...
 460     // Reconstruction de Direction:
 461     for (i = 1, k = 1; i <= Ninc; i++) {
 462       if (Constraints(i) == 0) {
 463         if (!ChangeDirection) {
 464           Direction(i) = MyDirection(k);
 465         }
 466         else Direction(i) = - GH(i);
 467         k++;
 468       }
 469       else {
 470         Direction(i) = 0.0;
 471       }
 472     }
 473   }
 474 }
 475
 476
 477
 478 //====================================================
 479 Standard_Boolean Bounds(const math_Vector&  InfBound,
 480                         const math_Vector&  SupBound,
 481                         const math_Vector&  Tol,
 482                         math_Vector&        Sol,
 483                         const math_Vector&  SolSave,
 484                         math_IntegerVector& Constraints,
 485                         math_Vector&        Delta,
 486                         Standard_Boolean&   theIsNewSol)
 487 //
 488 // Purpose: Trims an initial solution Sol to be within a domain defined by
 489 //   InfBound and SupBound. Delta will contain a distance between final Sol and
 490 //   SolSave.
 491 //   IsNewSol returns False, if final Sol fully coincides with SolSave, i.e.
 492 //   if SolSave already lied on a boundary and initial Sol was fully beyond it
 493 //======================================================
 494 {
 495   Standard_Boolean Out = Standard_False;
 496   Standard_Integer i, Ninc = Sol.Length();
 497   Standard_Real    monratio = 1;
 498
 499   theIsNewSol = Standard_True;
 500
 501   // Calcul du ratio de recadrage
 502   for (i = 1; i <= Ninc; i++) {
 503     Constraints(i) = 0;
 504     Delta(i) = Sol(i) - SolSave(i);
 505     if (InfBound(i) == SupBound(i)) {
 506       Constraints(i) = 1;
 507       Out = Standard_True; // Ok mais, cela devrait etre eviter
 508     }
 509     else if(Sol(i) < InfBound(i)) {
 510       Constraints(i) = 1;
 511       Out = Standard_True;
 512       // Delta(i) is negative
 513       if (-Delta(i) > Tol(i)) // Afin d'eviter des ratio nulles pour rien
 514         monratio = Min(monratio, (InfBound(i) - SolSave(i))/Delta(i) );
 515     }
 516     else if (Sol(i) > SupBound(i)) {
 517       Constraints(i) = 1;
 518       Out = Standard_True;
 519       // Delta(i) is positive
 520       if (Delta(i) > Tol(i))
 521         monratio = Min(monratio, (SupBound(i) - SolSave(i))/Delta(i) );
 522     }
 523   }
 524
 525   if (Out){ // Troncature et derniers recadrage pour blinder (pb numeriques)
 526     if (monratio == 0.0) {
 527       theIsNewSol = Standard_False;
 528       Sol = SolSave;
 529       Delta.Init (0.0);
 530     } else {
 531       Delta *= monratio;
 532       Sol = SolSave+Delta;
 533       for (i = 1; i <= Ninc; i++) {
 534         if(Sol(i) < InfBound(i))  {
 535           Sol(i) = InfBound(i);
 536           Delta(i) = Sol(i) - SolSave(i);
 537         }
 538         else if (Sol(i) > SupBound(i)) {
 539           Sol(i) = SupBound(i);
 540           Delta(i) = Sol(i) - SolSave(i);
 541         }
 542       }
 543     }
 544   }
 545   return Out;
 546 }
 547
 548
 549
 550
 551
 552 math_FunctionSetRoot::math_FunctionSetRoot(math_FunctionSetWithDerivatives& F,
 553                                            const math_Vector& Tolerance,
 554                                            const Standard_Integer NbIterations) :
 555                                            Delta(1, F.NbVariables()),
 556                                            Sol(1, F.NbVariables()),
 557                                            DF(1, F.NbEquations(),
 558                                               1, F.NbVariables()),
 559                                            Tol(1,F.NbVariables()),
 560
 561                                            InfBound(1, F.NbVariables()),
 562                                            SupBound(1, F.NbVariables()),
 563
 564                                            SolSave(1, F.NbVariables()),
 565                                            GH(1, F.NbVariables()),
 566                                            DH(1, F.NbVariables()),
 567                                            DHSave(1, F.NbVariables()),
 568                                            FF(1, F.NbEquations()),
 569                                            PreviousSolution(1, F.NbVariables()),
 570                                            Save(0, NbIterations),
 571                                            Constraints(1, F.NbVariables()),
 572                                            Temp1(1, F.NbVariables()),
 573                                            Temp2(1, F.NbVariables()),
 574                                            Temp3(1, F.NbVariables()),
 575                                            Temp4(1, F.NbEquations())
 576
 577 {
 578   for (Standard_Integer i = 1; i <= Tol.Length(); i++) {
 579     Tol(i) =Tolerance(i);
 580   }
 581   Itermax = NbIterations;
 582 }
 583
 584 math_FunctionSetRoot::math_FunctionSetRoot(math_FunctionSetWithDerivatives& F,
 585                                            const Standard_Integer NbIterations) :
 586                                            Delta(1, F.NbVariables()),
 587                                            Sol(1, F.NbVariables()),
 588                                            DF(1, F.NbEquations(),
 589                                               1, F.NbVariables()),
 590                                            Tol(1, F.NbVariables()),
 591
 592                                            InfBound(1, F.NbVariables()),
 593                                            SupBound(1, F.NbVariables()),
 594
 595                                            SolSave(1, F.NbVariables()),
 596                                            GH(1, F.NbVariables()),
 597                                            DH(1, F.NbVariables()),
 598                                            DHSave(1, F.NbVariables()),
 599                                            FF(1, F.NbEquations()),
 600                                            PreviousSolution(1, F.NbVariables()),
 601                                            Save(0, NbIterations),
 602                                            Constraints(1, F.NbVariables()),
 603                                            Temp1(1, F.NbVariables()),
 604                                            Temp2(1, F.NbVariables()),
 605                                            Temp3(1, F.NbVariables()),
 606                                            Temp4(1, F.NbEquations())
 607
 608 {
 609   Itermax = NbIterations;
 610 }
 611
 612
 613
 614 math_FunctionSetRoot::math_FunctionSetRoot(math_FunctionSetWithDerivatives& F,
 615                                            const math_Vector& StartingPoint,
 616                                            const math_Vector& Tolerance,
 617                                            const math_Vector& infBound,
 618                                            const math_Vector& supBound,
 619                                            const Standard_Integer NbIterations) :
 620                                            Delta(1, F.NbVariables()),
 621                                            Sol(1, F.NbVariables()),
 622                                            DF(1, F.NbEquations(),
 623                                               1, F.NbVariables()),
 624                                            Tol(1,F.NbVariables()),
 625
 626
 627                                            InfBound(1, F.NbVariables()),
 628                                            SupBound(1, F.NbVariables()),
 629
 630                                            SolSave(1, F.NbVariables()),
 631                                            GH(1, F.NbVariables()),
 632                                            DH(1, F.NbVariables()),
 633                                            DHSave(1, F.NbVariables()),
 634                                            FF(1, F.NbEquations()),
 635                                            PreviousSolution(1, F.NbVariables()),
 636                                            Save(0, NbIterations),
 637                                            Constraints(1, F.NbVariables()),
 638                                            Temp1(1, F.NbVariables()),
 639                                            Temp2(1, F.NbVariables()),
 640                                            Temp3(1, F.NbVariables()),
 641                                            Temp4(1, F.NbEquations())
 642
 643 {
 644
 645   for (Standard_Integer i = 1; i <= Tol.Length(); i++) {
 646     Tol(i) =Tolerance(i);
 647   }
 648   Itermax = NbIterations;
 649   Perform(F, StartingPoint, infBound, supBound);
 650 }
 651
 652 math_FunctionSetRoot::math_FunctionSetRoot(math_FunctionSetWithDerivatives& F,
 653                                            const math_Vector& StartingPoint,
 654                                            const math_Vector& Tolerance,
 655                                            const Standard_Integer NbIterations) :
 656                                            Delta(1, F.NbVariables()),
 657                                            Sol(1, F.NbVariables()),
 658                                            DF(1, F.NbEquations(),
 659                                               1, StartingPoint.Length()),
 660                                            Tol(1,F.NbVariables()),
 661
 662                                            InfBound(1, F.NbVariables()),
 663                                            SupBound(1, F.NbVariables()),
 664
 665                                            SolSave(1, F.NbVariables()),
 666                                            GH(1, F.NbVariables()),
 667                                            DH(1, F.NbVariables()),
 668                                            DHSave(1, F.NbVariables()),
 669                                            FF(1, F.NbEquations()),
 670                                            PreviousSolution(1, F.NbVariables()),
 671                                            Save(0, NbIterations),
 672                                            Constraints(1, F.NbVariables()),
 673                                            Temp1(1, F.NbVariables()),
 674                                            Temp2(1, F.NbVariables()),
 675                                            Temp3(1, F.NbVariables()),
 676                                            Temp4(1, F.NbEquations())
 677
 678  {
 679   for (Standard_Integer i = 1; i <= Tol.Length(); i++) {
 680     Tol(i) = Tolerance(i);
 681   }
 682   Itermax = NbIterations;
 683   InfBound.Init(RealFirst());
 684   SupBound.Init(RealLast());
 685   Perform(F, StartingPoint, InfBound, SupBound);
 686 }
 687
 688 void math_FunctionSetRoot::Delete()
 689 {}
 690
 691 void math_FunctionSetRoot::SetTolerance(const math_Vector& Tolerance)
 692 {
 693   for (Standard_Integer i = 1; i <= Tol.Length(); i++) {
 694     Tol(i) = Tolerance(i);
 695   }
 696 }
 697
 698 void math_FunctionSetRoot::Perform(math_FunctionSetWithDerivatives& F,
 699                                    const math_Vector& StartingPoint,
 700                                    const math_Vector& InfBound,
 701                                    const math_Vector& SupBound)
 702 {
 703   Standard_Integer Ninc = F.NbVariables(), Neq = F.NbEquations();
 704
 705   if ((Neq <= 0)                      ||
 706       (StartingPoint.Length()!= Ninc) ||
 707       (InfBound.Length() != Ninc)     ||
 708       (SupBound.Length() != Ninc))  { Standard_DimensionError:: Raise(); }
 709
 710   Standard_Integer i;
 711   Standard_Boolean ChangeDirection = Standard_False, Sort = Standard_False, isNewSol = Standard_False;
 712   Standard_Boolean Good, Verif;
 713   Standard_Boolean Stop;
 714   const Standard_Real EpsSqrt = 1.e-16, Eps = 1.e-32, Eps2 = 1.e-64, Progres = 0.005;
 715   Standard_Real F2, PreviousMinimum, Dy, OldF;
 716   Standard_Real Ambda, Ambda2, Gnr1, Oldgr;
 717   math_Vector InvLengthMax(1, Ninc); // Pour bloquer les pas a 1/4 du domaine
 718   math_IntegerVector Constraints(1, Ninc); // Pour savoir sur quels bord on se trouve
 719   for (i = 1; i <= Ninc ; i++) {
 720 // modified by NIZHNY-MKK  Mon Oct  3 18:03:50 2005
 721 //      InvLengthMax(i) = 1. / Max(Abs(SupBound(i) - InfBound(i))/4, 1.e-9);
 722     InvLengthMax(i) = 1. / Max((SupBound(i) - InfBound(i))/4, 1.e-9);
 723    }
 724
 725   MyDirFunction F_Dir(Temp1, Temp2, Temp3, Temp4, F);
 726   Standard_Integer  DescenteIter;
 727
 728   Done = Standard_False;
 729   Sol = StartingPoint;
 730   Kount = 0;
 731
 732   // Verification de la validite des inconnues par rapport aux bornes.
 733   // Recentrage sur les bornes si pas valide.
 734   for ( i = 1; i <= Ninc; i++) {
 735     if (Sol(i) <= InfBound(i)) Sol(i) = InfBound(i);
 736     else if (Sol(i) > SupBound(i)) Sol(i) = SupBound(i);
 737   }
 738
 739   // Calcul de la premiere valeur de F et de son gradient
 740   if(!F_Dir.Value(Sol, FF, DF, GH, F2, Gnr1)) {
 741     Done = Standard_False;
 742     State = F.GetStateNumber();
 743     return;
 744   }
 745   Ambda2 = Gnr1;
 746   // Le rang 0 de Save ne doit servir q'au test accelarteur en fin de boucle
 747   // s'il on est dejas sur la solution, il faut leurer ce test pour eviter
 748   // de faire une seconde iteration...
 749   Save(0) = Max (F2, EpsSqrt);
 750
 751   FSR_DEBUG("=== Mode Debug de Function Set Root" << endl)
 752   FSR_DEBUG("    F2 Initial = " << F2)
 753
 754   if ((F2 <= Eps) || (Gnr1 <= Eps2)) {
 755     Done = Standard_True;
 756     State = F.GetStateNumber();
 757     return;
 758   }
 759
 760   for (Kount = 1; Kount <= Itermax; Kount++) {
 761     PreviousMinimum = F2;
 762     Oldgr = Gnr1;
 763     PreviousSolution = Sol;
 764     SolSave = Sol;
 765
 766     SearchDirection(DF, GH, FF, ChangeDirection, InvLengthMax, DH, Dy);
 767     if (Abs(Dy) <= Eps) {
 768       Done = Standard_True;
 769       ////modified by jgv, 31.08.2011////
 770       F.Value(Sol, FF); //update F before GetStateNumber
 771       ///////////////////////////////////
 772       State = F.GetStateNumber();
 773       return;
 774     }
 775     if (ChangeDirection) {
 776       Ambda = Ambda2 / Sqrt(Abs(Dy));
 777       if (Ambda > 1.0) Ambda = 1.0;
 778     }
 779     else {
 780       Ambda = 1.0;
 781       Ambda2 = 0.5*Ambda/DH.Norm();
 782     }
 783
 784     for( i = Sol.Lower(); i <= Sol.Upper(); i++) {
 785       Sol(i) = Sol(i) + Ambda * DH(i);
 786     }
 787     Sort = Bounds(InfBound, SupBound, Tol, Sol, SolSave,
 788                   Constraints, Delta, isNewSol);
 789
 790
 791     DHSave = GH;
 792     if (isNewSol) {
 793 //      F_Dir.Value(Sol, FF, DF, GH, F2, Gnr1);
 794       if(!F_Dir.Value(Sol, FF, DF, GH, F2, Gnr1)) {
 795         Done = Standard_False;
 796         State = F.GetStateNumber();
 797         return;
 798       }
 799     }
 800
 801     FSR_DEBUG("Kount         = " << Kount)
 802     FSR_DEBUG("Le premier F2 = " << F2)
 803     FSR_DEBUG("Direction     = " << ChangeDirection)
 804
 805     if ((F2 <= Eps) || (Gnr1 <= Eps2)) {
 806       Done = Standard_True;
 807       ////modified by jgv, 31.08.2011////
 808       F.Value(Sol, FF); //update F before GetStateNumber
 809       ///////////////////////////////////
 810       State = F.GetStateNumber();
 811       return;
 812     }
 813
 814     if (Sort || (F2/PreviousMinimum > Progres)) {
 815       Dy = GH*DH;
 816       OldF = PreviousMinimum;
 817       Stop = Standard_False;
 818       Good = Standard_False;
 819       DescenteIter = 0;
 820       Standard_Boolean Sortbis;
 821
 822       // -------------------------------------------------
 823       // Traitement standard sans traitement des bords
 824       // -------------------------------------------------
 825       if (!Sort) { // si l'on est pas sortie on essaye de progresser en avant
 826         while((F2/PreviousMinimum > Progres) && !Stop) {
 827           if (F2 < OldF && (Dy < 0.0)) {
 828             // On essaye de progresser dans cette direction.
 829             FSR_DEBUG(" iteration de descente = " << DescenteIter)
 830             DescenteIter++;
 831             SolSave = Sol;
 832             OldF = F2;
 833             for( i = Sol.Lower(); i <= Sol.Upper(); i++) {
 834               Sol(i) = Sol(i) + Ambda * DH(i);
 835             }
 836             Stop = Bounds(InfBound, SupBound, Tol, Sol, SolSave,
 837                           Constraints, Delta, isNewSol);
 838             FSR_DEBUG(" Augmentation de lambda")
 839             Ambda *= 1.7;
 840           }
 841           else {
 842             if ((F2 >= OldF)||(F2 >= PreviousMinimum)) {
 843               Good = Standard_False;
 844               if (DescenteIter == 0) {
 845                 // C'est le premier pas qui flanche, on fait une interpolation.
 846                 // et on minimise si necessaire.
 847                 DescenteIter++;
 848                 Good = MinimizeDirection(SolSave, Delta, OldF, F2, DHSave, GH,
 849                                          Tol, F_Dir);
 850               }
 851               else if (ChangeDirection || (DescenteIter>1)
 852                        || (OldF>PreviousMinimum) ) {
 853                 // La progression a ete utile, on minimise...
 854                 DescenteIter++;
 855                 Good = MinimizeDirection(PreviousSolution, SolSave, Sol,
 856                                          OldF, Delta, Tol, F_Dir);
 857               }
 858               if (!Good) {
 859                 Sol = SolSave;
 860                 F2 = OldF;
 861               }
 862               else {
 863                 Sol = SolSave+Delta;
 864                 Sort = Bounds(InfBound, SupBound, Tol, Sol, SolSave,
 865                   Constraints, Delta, isNewSol);
 866               }
 867               Sort = Standard_False; // On a rejete le point sur la frontiere
 868             }
 869             Stop = Standard_True; // et on sort dans tous les cas...
 870           }
 871           DHSave = GH;
 872           if (isNewSol) {
 873 //            F_Dir.Value(Sol, FF, DF, GH, F2, Gnr1);
 874             if(!F_Dir.Value(Sol, FF, DF, GH, F2, Gnr1)) {
 875               Done = Standard_False;
 876               State = F.GetStateNumber();
 877               return;
 878             }
 879           }
 880           Dy = GH*DH;
 881           if (Abs(Dy) <= Eps) {
 882             Done = Standard_True;
 883             if (F2 > OldF)
 884               Sol = SolSave;
 885             ////modified by jgv, 31.08.2011////
 886             F.Value(Sol, FF); //update F before GetStateNumber
 887             ///////////////////////////////////
 888             State = F.GetStateNumber();
 889             return;
 890           }
 891           if (DescenteIter >= 10) {
 892             Stop = Standard_True;
 893           }
 894         }
 895         FSR_DEBUG("--- Sortie du Traitement Standard")
 896         FSR_DEBUG("    DescenteIter = "<<DescenteIter << " F2 = " << F2)
 897       }
 898       // ------------------------------------
 899       //  on passe au traitement des bords
 900       // ------------------------------------
 901       if (Sort) {
 902         Stop = (F2 > 1.001*OldF); // Pour ne pas progresser sur le bord
 903         Sortbis = Sort;
 904         DescenteIter = 0;
 905         while (Sortbis && ((F2<OldF)|| (DescenteIter == 0))
 906                && (!Stop)) {
 907           DescenteIter++;
 908           // On essaye de progresser sur le bord
 909           SolSave = Sol;
 910           OldF = F2;
 911           SearchDirection(DF, GH, FF,  Constraints, Sol,
 912                           ChangeDirection, InvLengthMax, DH, Dy);
 913           FSR_DEBUG(" Conditional Direction = " << ChangeDirection)
 914           if (Dy<-Eps) { //Pour eviter des calculs inutiles et des /0...
 915             if (ChangeDirection) {
 916
 917 //            Ambda = Ambda2 / Sqrt(Abs(Dy));
 918               Ambda = Ambda2 / Sqrt(-Dy);
 919               if (Ambda > 1.0) Ambda = 1.0;
 920             }
 921             else {
 922               Ambda = 1.0;
 923               Ambda2 = 0.5*Ambda/DH.Norm();
 924             }
 925
 926             for( i = Sol.Lower(); i <= Sol.Upper(); i++) {
 927               Sol(i) = Sol(i) + Ambda * DH(i);
 928             }
 929             Sortbis = Bounds(InfBound, SupBound, Tol, Sol, SolSave,
 930                              Constraints, Delta, isNewSol);
 931
 932             DHSave = GH;
 933             if (isNewSol) {
 934 //              F_Dir.Value(Sol, FF, DF, GH, F2, Gnr1);
 935               if(!F_Dir.Value(Sol, FF, DF, GH, F2, Gnr1)) {
 936                 Done = Standard_False;
 937                 State = F.GetStateNumber();
 938                 return;
 939               }
 940             }
 941             Ambda2 = Gnr1;
 942             FSR_DEBUG("---  Iteration au bords : " << DescenteIter)
 943             FSR_DEBUG("---  F2 = " << F2)
 944           }
 945           else {
 946             Stop = Standard_True;
 947           }
 948
 949           while((F2/PreviousMinimum > Progres) && (F2<OldF) && (!Stop) ) {
 950             DescenteIter++;
 951             FSR_DEBUG("--- Iteration de descente conditionnel = " << DescenteIter)
 952             if (F2 < OldF && Dy < 0.0) {
 953               // On essaye de progresser dans cette direction.
 954               SolSave = Sol;
 955               OldF = F2;
 956               for( i = Sol.Lower(); i <= Sol.Upper(); i++) {
 957                 Sol(i) = Sol(i) + Ambda * DH(i);
 958               }
 959               Sortbis = Bounds(InfBound, SupBound, Tol, Sol, SolSave,
 960                                Constraints, Delta, isNewSol);
 961             }
 962             DHSave = GH;
 963             if (isNewSol) {
 964 //              F_Dir.Value(Sol, FF, DF, GH, F2, Gnr1);
 965               if(!F_Dir.Value(Sol, FF, DF, GH, F2, Gnr1)) {
 966                 Done = Standard_False;
 967                 State = F.GetStateNumber();
 968                 return;
 969               }
 970             }
 971             Ambda2 = Gnr1;
 972             Dy = GH*DH;
 973             Stop = ((Dy >=0) || (DescenteIter >= 10) || Sortbis);
 974           }
 975           Stop = ((Dy >=0) || (DescenteIter >= 10));
 976         }
 977         if (((F2/PreviousMinimum > Progres) &&
 978              (F2>=OldF))||(F2>=PreviousMinimum)) {
 979           // On minimise par Brent
 980           DescenteIter++;
 981           Good = MinimizeDirection(SolSave, Delta, OldF, F2,
 982                                    DHSave, GH, Tol, F_Dir);
 983           if (!Good) {
 984             Sol = SolSave;
 985             Sort = Standard_False;
 986           }
 987           else {
 988             Sol = SolSave + Delta;
 989             Sort = Bounds(InfBound, SupBound, Tol, Sol, SolSave,
 990               Constraints, Delta, isNewSol);
 991             if (isNewSol) {
 992 //              F_Dir.Value(Sol, FF, DF, GH, F2, Gnr1);
 993               if(!F_Dir.Value(Sol, FF, DF, GH, F2, Gnr1)) {
 994                 Done = Standard_False;
 995                 State = F.GetStateNumber();
 996                 return;
 997               }
 998             }
 999           }
1000           Dy = GH*DH;
1001         }
1002         FSR_DEBUG("--- Sortie du Traitement des Bords")
1003         FSR_DEBUG("--- DescenteIter = "<<DescenteIter << " F2 = " << F2)
1004       }
1005     }
1006
1007     // ---------------------------------------------
1008     //  on passe aux tests d'ARRET
1009     // ---------------------------------------------
1010     Save(Kount) = F2;
1011     // Est ce la solution ?
1012     if (ChangeDirection) Verif = Standard_True;
1013                         // Gradient : Il faut eviter de boucler
1014     else {
1015       Verif = Standard_False;
1016       if (Kount > 1) { // Pour accelerer les cas quasi-quadratique
1017         if (Save(Kount-1)<1.e-4*Save(Kount-2)) Verif = Standard_True;
1018       }
1019       else Verif = (F2 < 1.e-6*Save(0)); //Pour les cas dejas solutions
1020     }
1021     if (Verif) {
1022       for(i = Delta.Lower(); i <= Delta.Upper(); i++) {
1023         Delta(i) = PreviousSolution(i) - Sol(i);
1024       }
1025
1026       if (IsSolutionReached(F)) {
1027         if (PreviousMinimum < F2) {
1028           Sol = SolSave;
1029         }
1030         Done = Standard_True;
1031         ////modified by jgv, 31.08.2011////
1032         F.Value(Sol, FF); //update F before GetStateNumber
1033         ///////////////////////////////////
1034         State = F.GetStateNumber();
1035         return;
1036       }
1037     }
1038     //fin du test solution
1039
1040     // Analyse de la progression...
1041     if (F2 < PreviousMinimum) {
1042       if (Kount > 5) {
1043         // L'historique est il bon ?
1044         if (F2 >= 0.95*Save(Kount - 5)) {
1045           if (!ChangeDirection) ChangeDirection = Standard_True;
1046           else return; //  si un gain inf a 5% on sort
1047         }
1048         else ChangeDirection = Standard_False; //Si oui on recommence
1049       }
1050       else  ChangeDirection = Standard_False; //Pas d'historique on continue
1051       // Si le gradient ne diminue pas suffisemment par newton on essaie
1052       // le gradient sauf si f diminue (aussi bizarre que cela puisse
1053       // paraitre avec NEWTON le gradient de f peut augmenter alors que f
1054       // diminue: dans ce cas il faut garder NEWTON)
1055       if ((Gnr1 > 0.9*Oldgr) &&
1056           (F2 > 0.5*PreviousMinimum)) {
1057         ChangeDirection = Standard_True;
1058       }
1059
1060       // Si l'on ne decide pas de changer de strategie, on verifie,
1061       // si ce n'est dejas fait
1062       if ((!ChangeDirection) && (!Verif)) {
1063         for(i = Delta.Lower(); i <= Delta.Upper(); i++) {
1064           Delta(i) = PreviousSolution(i) - Sol(i);
1065         }
1066         if (IsSolutionReached(F)) {
1067           Done = Standard_True;
1068           ////modified by jgv, 31.08.2011////
1069           F.Value(Sol, FF); //update F before GetStateNumber
1070           ///////////////////////////////////
1071           State = F.GetStateNumber();
1072           return;
1073         }
1074       }
1075     }
1076     else { // Cas de regression
1077       if (!ChangeDirection) { // On passe au gradient
1078         ChangeDirection = Standard_True;
1079         Sol = PreviousSolution;
1080 //      F_Dir.Value(Sol, FF, DF, GH, F2, Gnr1);
1081         if(!F_Dir.Value(Sol, FF, DF, GH, F2, Gnr1)) {
1082           Done = Standard_False;
1083           State = F.GetStateNumber();
1084           return;
1085         }
1086       }
1087       else return; // y a plus d'issues
1088     }
1089   }
1090 }
1091
1092
1093
1094
1095 Standard_Boolean math_FunctionSetRoot::IsSolutionReached(math_FunctionSetWithDerivatives& ) {
1096    for(Standard_Integer i = 1; i<= Sol.Length(); i++) {
1097      if(Abs(Delta(i)) > Tol(i)) {return Standard_False;}
1098    }
1099    return Standard_True;
1100 }
1101
1102
1103 void math_FunctionSetRoot::Dump(Standard_OStream& o) const {
1104    o<<" math_FunctionSetRoot";
1105    if (Done) {
1106      o << " Status = Done\n";
1107      o << " Location value = " << Sol << "\n";
1108      o << " Number of iterations = " << Kount << "\n";
1109    }
1110    else {
1111      o<<"Status = Not Done\n";
1112    }
1113 }
1114
1115
1116 void math_FunctionSetRoot::Root(math_Vector& Root) const{
1117   StdFail_NotDone_Raise_if(!Done, " ");
1118   Standard_DimensionError_Raise_if(Root.Length() != Sol.Length(), " ");
1119   Root = Sol;
1120 }
1121
1122
1123 void math_FunctionSetRoot::FunctionSetErrors(math_Vector& Err) const{
1124   StdFail_NotDone_Raise_if(!Done, " ");
1125   Standard_DimensionError_Raise_if(Err.Length() != Sol.Length(), " ");
1126   Err = Delta;
1127 }