src/math/math_FunctionSetRoot.cxx

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