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