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