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