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