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