Rollback integration OCC22567 Speed up of math_FunctionSetRoot (used in Extrema)
[occt.git] / src / math / math_FunctionSetRoot.cxx
CommitLineData
7fd59977 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
fbadd2cc
RK
52static Standard_Boolean mydebug = Standard_False;
53
7fd59977 54class 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
63public :
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
83MyDirFunction::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
96void MyDirFunction::Initialize(const math_Vector& p0,
97 const math_Vector& dir) const
98{
99 *P0 = p0;
100 *Dir = dir;
101}
102
103
104Standard_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
134Standard_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//--------------------------------------------------------------
171static 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
fbadd2cc 205 if (mydebug)cout << " minimisation dans la direction" << endl;
7fd59977 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//----------------------------------------------------------------------
221static 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;
fbadd2cc
RK
246 if (mydebug) { cout << " essai d interpolation" << endl;}
247
7fd59977 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
fbadd2cc 267 Delta = Sqrt(Delta);
7fd59977 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;
fbadd2cc 288 if (mydebug) cout << "t= "<<tsol<<" F = " << fsol << " OldF = "<<PValue <<endl;
7fd59977 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 }
fbadd2cc 301 if (mydebug) cout << " minimisation dans la direction" << endl;
7fd59977 302 math_BrentMinimum Sol(F, ax, bx, cx, tol1d, 100, tol1d);
303 if(Sol.IsDone()) {
304 if (Sol.Minimum() <= Result) {
fbadd2cc
RK
305 tsol = Sol.Location();
306 good = Standard_True;
307 if (mydebug) cout << "t= "<<tsol<<" F ="<< Sol.Minimum()
308 << " OldF = "<<Result <<endl;
7fd59977 309 }
310 }
311 }
312 if (good) {
313 // mise a jour du Delta
314 Dir.Multiply(tsol);
315 }
316 return good;
317}
318
319//------------------------------------------------------
320static 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.
fbadd2cc
RK
339 if (mydebug) cout << " Matrice singuliere : On prend SVD" << endl;
340 math_SVD SolvebySVD(DF);
7fd59977 341 if (SolvebySVD.IsDone()) SolvebySVD.Solve(-1*FF, Direction);
fbadd2cc
RK
342 else ChangeDirection = Standard_True;
343 }
7fd59977 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//=====================================================================
385static 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//====================================================
fbadd2cc
RK
460Standard_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)
7fd59977 467//
fbadd2cc
RK
468// Purpose : Troncate un pas d'optimisation pour rester
469// dans le domaine, Delta donne le pas final
7fd59977 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;
fbadd2cc
RK
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)) );
7fd59977 489 }
490 else if (Sol(i) > SupBound(i)) {
491 Constraints(i) = 1;
492 Out = Standard_True;
fbadd2cc
RK
493 if (Abs(Delta(i)) > Tol(i))
494 monratio = Min(monratio, Abs( (SolSave(i)-SupBound(i))/Delta(i)) );
7fd59977 495 }
496 }
497
498 if (Out){ // Troncature et derniers recadrage pour blinder (pb numeriques)
fbadd2cc
RK
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);
7fd59977 509 }
510 }
511 }
512 return Out;
513}
514
515
516
517
518
519math_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
551math_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
581math_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
619math_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
655void math_FunctionSetRoot::Delete()
656{}
657
658void 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
665void 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;
fbadd2cc 679 Standard_Boolean ChangeDirection = Standard_False, Sort = Standard_False;
7fd59977 680 Standard_Boolean Good, Verif;
681 Standard_Boolean Stop;
fbadd2cc 682 Standard_Real Eps = 1.e-32, Progres = 0.005;
7fd59977 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...
fbadd2cc
RK
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 }
7fd59977 723
fbadd2cc 724 if ((F2 <= Eps) || (Sqrt(Gnr1) <= Eps)) {
7fd59977 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) {
fbadd2cc 743 Ambda = Ambda2/Sqrt(Abs(Dy));
7fd59977 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,
fbadd2cc 755 Constraints, Delta);
7fd59977 756
757
758 DHSave = GH;
fbadd2cc
RK
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;
7fd59977 771 }
fbadd2cc 772#endif
7fd59977 773
fbadd2cc 774 if ((F2 <= Eps) || (Sqrt(Gnr1) <= Eps)) {
7fd59977 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.
fbadd2cc 795 if (mydebug) cout << " iteration de descente = " << DescenteIter<<endl;
7fd59977 796 DescenteIter++;
797 SolSave = Sol;
798 OldF = F2;
799 for( i = Sol.Lower(); i <= Sol.Upper(); i++) {
fbadd2cc
RK
800 Sol(i) = Sol(i) + Ambda * DH(i);
801 }
7fd59977 802 Stop = Bounds(InfBound, SupBound, Tol, Sol, SolSave,
fbadd2cc
RK
803 Constraints, Delta);
804 if (mydebug) { cout << " Augmentation de lambda" << endl;}
7fd59977 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 {
fbadd2cc 829 Sol = SolSave+Delta;
7fd59977 830 }
fbadd2cc
RK
831 Sort = Bounds(InfBound, SupBound, Tol, Sol, SolSave,
832 Constraints, Delta);
7fd59977 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;
fbadd2cc
RK
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 }
7fd59977 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 }
fbadd2cc
RK
855 if (mydebug) {
856 cout << "--- Sortie du Traitement Standard"<<endl;
857 cout << " DescenteIter = "<<DescenteIter << " F2 = " << F2 << endl;
858 }
7fd59977 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);
fbadd2cc 875 if (mydebug) { cout << " Conditional Direction = " << ChangeDirection << endl;}
7fd59977 876 if (Dy<-Eps) { //Pour eviter des calculs inutiles et des /0...
877 if (ChangeDirection) {
878
fbadd2cc
RK
879// Ambda = Ambda2/Sqrt(Abs(Dy));
880 Ambda = Ambda2/Sqrt(-Dy);
7fd59977 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,
fbadd2cc 892 Constraints, Delta);
7fd59977 893
894 DHSave = GH;
fbadd2cc
RK
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 }
7fd59977 901 Ambda2 = Gnr1;
fbadd2cc
RK
902 if (mydebug) {
903 cout << "--- Iteration au bords : " << DescenteIter << endl;
904 cout << "--- F2 = " << F2 << endl;
905 }
7fd59977 906 }
907 else {
908 Stop = Standard_True;
909 }
910
911 while((F2/PreviousMinimum > Progres) && (F2<OldF) && (!Stop) ) {
912 DescenteIter++;
fbadd2cc
RK
913 if (mydebug) cout << "--- Iteration de descente conditionnel = "
914 << DescenteIter<<endl;
7fd59977 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,
fbadd2cc 923 Constraints, Delta );
7fd59977 924 }
925 DHSave = GH;
fbadd2cc
RK
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 }
7fd59977 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;
fbadd2cc
RK
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;
7fd59977 957 }
958 Dy = GH*DH;
959 }
fbadd2cc
RK
960 if (mydebug) {
961 cout << "--- Sortie du Traitement des Bords"<<endl;
962 cout << "--- DescenteIter = "<<DescenteIter << " F2 = " << F2 << endl;
963 }
7fd59977 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
1048Standard_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
1056void 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
1069void 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
1076void 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}
fbadd2cc
RK
1081
1082
1083
1084
1085
1086
1087