OCC22567 Speed up of math_FunctionSetRoot (used in Extrema)
[occt.git] / 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 }