projects / occt.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
0030686: Visualization, SelectMgr_ViewerSelector - sorting issues of transformation...
[occt.git] / src / math / math_FunctionRoots.cxx
CommitLineData
b311480e 1// Copyright (c) 1997-1999 Matra Datavision
973c2be1 2// Copyright (c) 1999-2014 OPEN CASCADE SAS
b311480e 3//
973c2be1 4// This file is part of Open CASCADE Technology software library.
b311480e 5//
d5f74e42 6// This library is free software; you can redistribute it and/or modify it under
7// the terms of the GNU Lesser General Public License version 2.1 as published
973c2be1 8// by the Free Software Foundation, with special exception defined in the file
9// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
10// distribution for complete text of the license and disclaimer of any warranty.
b311480e 11//
973c2be1 12// Alternatively, this file may be used under the terms of Open CASCADE
13// commercial license or contractual agreement.
b311480e 14
0797d9d3 15//#ifndef OCCT_DEBUG
7fd59977 16#define No_Standard_RangeError
17#define No_Standard_OutOfRange
18#define No_Standard_DimensionError
42cf5bc1 19
7fd59977 20//#endif
21
7fd59977 22#include <math_DirectPolynomialRoots.hxx>
42cf5bc1 23#include <math_FunctionRoots.hxx>
7fd59977 24#include <math_FunctionWithDerivative.hxx>
4bc805bf 25#include <math_BracketedRoot.hxx>
42cf5bc1 26#include <Standard_RangeError.hxx>
27#include <StdFail_NotDone.hxx>
232a3086 28#include <TColStd_Array1OfReal.hxx>
42cf5bc1 29
7fd59977 30#include <stdio.h>
7fd59977 31#define ITMAX 100
32#define EPS 1e-14
33#define EPSEPS 2e-14
34#define MAXBIS 100
35
0797d9d3 36#ifdef OCCT_DEBUG
7fd59977 37static Standard_Boolean myDebug = 0;
38static Standard_Integer nbsolve = 0;
0797d9d3 39#endif
7fd59977 40
4bc805bf 41class DerivFunction: public math_Function
42{
43 math_FunctionWithDerivative *myF;
44
45public:
46 DerivFunction(math_FunctionWithDerivative& theF)
47 : myF (&theF)
48 {}
49
50 virtual Standard_Boolean Value(const Standard_Real theX, Standard_Real& theFval)
51 {
52 return myF->Derivative(theX, theFval);
53 }
54};
55
7fd59977 56static void AppendRoot(TColStd_SequenceOfReal& Sol,
57 TColStd_SequenceOfInteger& NbStateSol,
58 const Standard_Real X,
59 math_FunctionWithDerivative& F,
60// const Standard_Real K,
61 const Standard_Real ,
62 const Standard_Real dX) {
63
64 Standard_Integer n=Sol.Length();
65 Standard_Real t;
0797d9d3 66#ifdef OCCT_DEBUG
7fd59977 67 if (myDebug) {
68 cout << " Ajout de la solution numero : " << n+1 << endl;
69 cout << " Valeur de la racine :" << X << endl;
70 }
71#endif
72
73 if(n==0) {
74 Sol.Append(X);
75 F.Value(X,t);
76 NbStateSol.Append(F.GetStateNumber());
77 }
78 else {
79 Standard_Integer i=1;
80 Standard_Integer pl=n+1;
81 while(i<=n) {
82 t=Sol.Value(i);
83 if(t >= X) {
84 pl=i;
85 i=n;
86 }
87 if(Abs(X-t) <= dX) {
88 pl=0;
89 i=n;
90 }
91 i++;
92 } //-- while
93 if(pl>n) {
94 Sol.Append(X);
95 F.Value(X,t);
96 NbStateSol.Append(F.GetStateNumber());
97 }
98 else if(pl>0) {
99 Sol.InsertBefore(pl,X);
100 F.Value(X,t);
101 NbStateSol.InsertBefore(pl,F.GetStateNumber());
102 }
103 }
104}
105
106static void Solve(math_FunctionWithDerivative& F,
107 const Standard_Real K,
108 const Standard_Real x1,
109 const Standard_Real y1,
110 const Standard_Real x2,
111 const Standard_Real y2,
112 const Standard_Real tol,
113 const Standard_Real dX,
114 TColStd_SequenceOfReal& Sol,
115 TColStd_SequenceOfInteger& NbStateSol) {
0797d9d3 116#ifdef OCCT_DEBUG
7fd59977 117 if (myDebug) {
118 cout <<"--> Resolution :" << ++nbsolve << endl;
119 cout <<" x1 =" << x1 << " y1 =" << y1 << endl;
120 cout <<" x2 =" << x2 << " y2 =" << y2 << endl;
121 }
122#endif
123
124 Standard_Integer iter=0;
125 Standard_Real tols2 = 0.5*tol;
126 Standard_Real a,b,c,d=0,e=0,fa,fb,fc,p,q,r,s,tol1,xm,min1,min2;
127 a=x1;b=c=x2;fa=y1; fb=fc=y2;
128 for(iter=1;iter<=ITMAX;iter++) {
129 if((fb>0.0 && fc>0.0) || (fb<0.0 && fc<0.0)) {
130 c=a; fc=fa; e=d=b-a;
131 }
132 if(Abs(fc)<Abs(fb)) {
133 a=b; b=c; c=a; fa=fb; fb=fc; fc=fa;
134 }
135 tol1 = EPSEPS*Abs(b)+tols2;
136 xm=0.5*(c-b);
137 if(Abs(xm)<tol1 || fb==0) {
138 //-- On tente une iteration de newton
139 Standard_Real Xp,Yp,Dp;
140 Standard_Integer itern=5;
141 Standard_Boolean Ok;
142 Xp=b;
143 do {
144 Ok = F.Values(Xp,Yp,Dp);
145 if(Ok) {
146 Ok=Standard_False;
147 if(Dp>1e-10 || Dp<-1e-10) {
148 Xp = Xp-(Yp-K)/Dp;
149 }
150 if(Xp<=x2 && Xp>=x1) {
151 F.Value(Xp,Yp); Yp-=K;
152 if(Abs(Yp)<Abs(fb)) {
153 b=Xp;
154 fb=Yp;
155 Ok=Standard_True;
156 }
157 }
158 }
159 }
160 while(Ok && --itern>=0);
161
162 AppendRoot(Sol,NbStateSol,b,F,K,dX);
163 return;
164 }
165 if(Abs(e)>=tol1 && Abs(fa)>Abs(fb)) {
166 s=fb/fa;
167 if(a==c) {
168 p=xm*s; p+=p;
169 q=1.0-s;
170 }
171 else {
172 q=fa/fc;
173 r=fb/fc;
174 p=s*((xm+xm)*q*(q-r)-(b-a)*(r-1.0));
175 q=(q-1.0)*(r-1.0)*(s-1.0);
176 }
177 if(p>0.0) {
178 q=-q;
179 }
180 p=Abs(p);
181 min1=3.0*xm*q-Abs(tol1*q);
182 min2=Abs(e*q);
183 if((p+p)<( (min1<min2) ? min1 : min2)) {
184 e=d;
185 d=p/q;
186 }
187 else {
188 d=xm;
189 e=d;
190 }
191 }
192 else {
193 d=xm;
194 e=d;
195 }
196 a=b;
197 fa=fb;
198 if(Abs(d)>tol1) {
199 b+=d;
200 }
201 else {
202 if(xm>=0) b+=Abs(tol1);
203 else b+=-Abs(tol1);
204 }
205 F.Value(b,fb);
206 fb-=K;
207 }
0797d9d3 208#ifdef OCCT_DEBUG
7fd59977 209 cout<<" Non Convergence dans math_FunctionRoots.cxx "<<endl;
210#endif
211}
212
213#define NEWSEQ 1
214
3ed30348 215#define MATH_FUNCTIONROOTS_NEWCODE // Nv Traitement
216//#define MATH_FUNCTIONROOTS_OLDCODE // Ancien
217//#define MATH_FUNCTIONROOTS_CHECK // Check
7fd59977 218
219math_FunctionRoots::math_FunctionRoots(math_FunctionWithDerivative& F,
220 const Standard_Real A,
221 const Standard_Real B,
222 const Standard_Integer NbSample,
223 const Standard_Real _EpsX,
224 const Standard_Real EpsF,
225 const Standard_Real EpsNull,
226 const Standard_Real K )
227{
0797d9d3 228#ifdef OCCT_DEBUG
7fd59977 229 if (myDebug) {
230 cout << "---- Debut de math_FunctionRoots ----" << endl;
231 nbsolve = 0;
232 }
233#endif
234
1ef32e96
RL		 235#if NEWSEQ
236 TColStd_SequenceOfReal StaticSol;
237#endif
7fd59977 238 Sol.Clear();
239 NbStateSol.Clear();
3ed30348 240 #ifdef MATH_FUNCTIONROOTS_NEWCODE
241 {
7fd59977 242 Done = Standard_True;
243 Standard_Real X0=A;
244 Standard_Real XN=B;
245 Standard_Integer N=NbSample;
246 //-- ------------------------------------------------------------
247 //-- Verifications de bas niveau
248 if(B<A) {
249 X0=B;
250 XN=A;
251 }
252 N*=2;
253 if(N < 20) {
254 N=20;
255 }
256 //-- On teste si EpsX est trop petit (ie : U+Nn*EpsX == U )
257 Standard_Real EpsX = _EpsX;
258 Standard_Real DeltaU = Abs(X0)+Abs(XN);
259 Standard_Real NEpsX = 0.0000000001 * DeltaU;
260 if(EpsX < NEpsX) {
261 EpsX = NEpsX;
262 }
263
264 //-- recherche d un intervalle ou F(xi) et F(xj) sont de signes differents
265 //-- A .............................................................. B
266 //-- X0 X1 X2 ........................................ Xn-1 Xn
267 Standard_Integer i;
268 Standard_Real X=X0;
269 Standard_Boolean Ok;
270 double dx = (XN-X0)/N;
232a3086 271 TColStd_Array1OfReal ptrval(0, N);
7fd59977 272 Standard_Integer Nvalid = -1;
273 Standard_Real aux = 0;
274 for(i=0; i<=N ; i++,X+=dx) {
275 if( X > XN) X=XN;
276 Ok=F.Value(X,aux);
232a3086
PD		 277 if(Ok) ptrval(++Nvalid) = aux - K;
278// ptrval(i)-=K;
7fd59977 279 }
280 //-- Toute la fonction est nulle ?
281
282 if( Nvalid < N ) {
283 Done = Standard_False;
284 return;
285 }
286
287 AllNull=Standard_True;
288// for(i=0;AllNull && i<=N;i++) {
289 for(i=0;AllNull && i<=N;i++) {
232a3086 290 if(ptrval(i)>EpsNull || ptrval(i)<-EpsNull) {
7fd59977 291 AllNull=Standard_False;
292 }
293 }
294 if(AllNull) {
295 //-- tous les points echantillons sont dans la tolerance
296
297 }
298 else {
299 //-- Il y a des points hors tolerance
300 //-- on detecte les changements de signes STRICTS
301 Standard_Integer ip1;
302// Standard_Boolean chgtsign=Standard_False;
303 Standard_Real tol = EpsX;
304 Standard_Real X2;
305 for(i=0,ip1=1,X=X0;i<N; i++,ip1++,X+=dx) {
306 X2=X+dx;
307 if(X2 > XN) X2 = XN;
232a3086
PD		 308 if(ptrval(i)<0.0) {
309 if(ptrval(ip1)>0.0) {
7fd59977 310 //-- --------------------------------------------------
311 //-- changement de signe dans Xi Xi+1
232a3086 312 Solve(F,K,X,ptrval(i),X2,ptrval(ip1),tol,NEpsX,Sol,NbStateSol);
7fd59977 313 }
314 }
315 else {
232a3086 316 if(ptrval(ip1)<0.0) {
7fd59977 317 //-- --------------------------------------------------
318 //-- changement de signe dans Xi Xi+1
232a3086 319 Solve(F,K,X,ptrval(i),X2,ptrval(ip1),tol,NEpsX,Sol,NbStateSol);
7fd59977 320 }
321 }
322 }
323 //-- On detecte les cas ou la fct s annule sur des Xi et est
324 //-- non nulle au voisinage de Xi
325 //--
326 //-- On prend 2 points u0,u1 au voisinage de Xi
327 //-- Si (F(u0)-K)*(F(u1)-K) <0 on lance une recherche
328 //-- Sinon si (F(u0)-K)*(F(u1)-K) !=0 on insere le point X
329 for(i=0; i<=N; i++) {
232a3086 330 if(ptrval(i)==0) {
7fd59977 331// Standard_Real Val,Deriv;
332 X=X0+i*dx;
333 if (X>XN) X=XN;
334 Standard_Real u0,u1;
335 u0=dx*0.5; u1=X+u0; u0+=X;
336 if(u0<X0) u0=X0;
337 if(u0>XN) u0=XN;
338 if(u1<X0) u1=X0;
339 if(u1>XN) u1=XN;
340
341 Standard_Real y0,y1;
342 F.Value(u0,y0); y0-=K;
343 F.Value(u1,y1); y1-=K;
344 if(y0*y1 < 0.0) {
345 Solve(F,K,u0,y0,u1,y1,tol,NEpsX,Sol,NbStateSol);
346 }
347 else {
348 if(y0!=0.0 || y1!=0.0) {
349 AppendRoot(Sol,NbStateSol,X,F,K,NEpsX);
350 }
351 }
352 }
353 }
354 //-- --------------------------------------------------------------------------------
355 //-- Il faut traiter differement le cas des points en bout :
232a3086 356 if(ptrval(0)<=EpsF && ptrval(0)>=-EpsF) {
7fd59977 357 AppendRoot(Sol,NbStateSol,X0,F,K,NEpsX);
358 }
232a3086 359 if(ptrval(N)<=EpsF && ptrval(N)>=-EpsF) {
7fd59977 360 AppendRoot(Sol,NbStateSol,XN,F,K,NEpsX);
361 }
362
363 //-- --------------------------------------------------------------------------------
364 //-- --------------------------------------------------------------------------------
365 //-- On detecte les zones ou on a sur les points echantillons un minimum avec f(x)>0
366 //-- un maximum avec f(x)<0
367 //-- On reprend une discretisation plus fine au voisinage de ces extremums
368 //--
369 //-- Recherche d un minima positif
370 Standard_Real xm,ym,dym,xm1,xp1;
371 Standard_Real majdx = 5.0*dx;
372 Standard_Boolean Rediscr;
373// Standard_Real ptrvalbis[MAXBIS];
374 Standard_Integer im1=0;
375 ip1=2;
376 for(i=1,xm=X0+dx; i<N; xm+=dx,i++,im1++,ip1++) {
377 Rediscr = Standard_False;
378 if (xm > XN) xm=XN;
232a3086
PD		 379 if(ptrval(i)>0.0) {
380 if((ptrval(im1)>ptrval(i)) && (ptrval(ip1)>ptrval(i))) {
7fd59977 381 //-- Peut on traverser l axe Ox
382 //-- -------------- Estimation a partir de Xim1
383 xm1=xm-dx;
384 if(xm1 < X0) xm1=X0;
385 F.Values(xm1,ym,dym); ym-=K;
386 if(dym<-1e-10 || dym>1e-10) { // normalement dym < 0
387 Standard_Real t = ym / dym; //-- t=xm-x* = (ym-0)/dym
388 if(t<majdx && t > -majdx) {
389 Rediscr = Standard_True;
390 }
391 }
392 //-- -------------- Estimation a partir de Xip1
393 if(Rediscr==Standard_False) {
394 xp1=xm+dx;
395 if(xp1 > XN ) xp1=XN;
396 F.Values(xp1,ym,dym); ym-=K;
397 if(dym<-1e-10 || dym>1e-10) { // normalement dym > 0
398 Standard_Real t = ym / dym; //-- t=xm-x* = (ym-0)/dym
399 if(t<majdx && t > -majdx) {
400 Rediscr = Standard_True;
401 }
402 }
403 }
404 }
405 }
232a3086
PD		 406 else if(ptrval(i)<0.0) {
407 if((ptrval(im1)<ptrval(i)) && (ptrval(ip1)<ptrval(i))) {
7fd59977 408 //-- Peut on traverser l axe Ox
409 //-- -------------- Estimation a partir de Xim1
410 xm1=xm-dx;
411 if(xm1 < X0) xm1=X0;
412 F.Values(xm1,ym,dym); ym-=K;
413 if(dym>1e-10 || dym<-1e-10) { // normalement dym > 0
414 Standard_Real t = ym / dym; //-- t=xm-x* = (ym-0)/dym
415 if(t<majdx && t > -majdx) {
416 Rediscr = Standard_True;
417 }
418 }
419 //-- -------------- Estimation a partir de Xim1
420 if(Rediscr==Standard_False) {
421 xm1=xm-dx;
422 if(xm1 < X0) xm1=X0;
423 F.Values(xm1,ym,dym); ym-=K;
424 if(dym>1e-10 || dym<-1e-10) { // normalement dym < 0
425 Standard_Real t = ym / dym; //-- t=xm-x* = (ym-0)/dym
426 if(t<majdx && t > -majdx) {
427 Rediscr = Standard_True;
428 }
429 }
430 }
431 }
432 }
433 if(Rediscr) {
4bc805bf 434 Standard_Real x0 = xm - dx;
435 Standard_Real x3 = xm + dx;
436 if (x0 < X0) x0 = X0;
437 if (x3 > XN) x3 = XN;
438 Standard_Real aSolX1 = 0., aSolX2 = 0.;
439 Standard_Real aVal1 = 0., aVal2 = 0.;
440 Standard_Real aDer1 = 0., aDer2 = 0.;
441 Standard_Boolean isSol1 = Standard_False;
442 Standard_Boolean isSol2 = Standard_False;
443 //-- ----------------------------------------------------
444 //-- Find minimum of the function |F| between x0 and x3
445 //-- by searching for the zero of the function derivative
446 DerivFunction aDerF(F);
b6cf8ffa 447 math_BracketedRoot aBR(aDerF, x0, x3, _EpsX);
4bc805bf 448 if (aBR.IsDone())
449 {
450 aSolX1 = aBR.Root();
451 F.Value(aSolX1, aVal1);
452 aVal1 = Abs(aVal1);
453 if (aVal1 < EpsF)
454 {
455 isSol1 = Standard_True;
456 aDer1 = aBR.Value();
457 }
458 }
7fd59977 459
4bc805bf 460 //-- --------------------------------------------------
461 //-- On recherche un extrema entre x0 et x3
462 //-- x1 et x2 sont tels que x0<x1<x2<x3
463 //-- et |f(x0)| > |f(x1)| et |f(x3)| > |f(x2)|
464 //--
465 //-- En entree : a=xm-dx b=xm c=xm+dx
466 Standard_Real x1, x2, f0, f3;
467 Standard_Real R = 0.61803399;
468 Standard_Real C = 1.0 - R;
469 Standard_Real tolCR = NEpsX*10.0;
470 f0 = ptrval(im1);
471 f3 = ptrval(ip1);
472 Standard_Boolean recherche_minimum = (f0 > 0.0);
473
474 if (Abs(x3 - xm) > Abs(x0 - xm)) { x1 = xm; x2 = xm + C*(x3 - xm); }
475 else { x2 = xm; x1 = xm - C*(xm - x0); }
476 Standard_Real f1, f2;
477 F.Value(x1, f1); f1 -= K;
478 F.Value(x2, f2); f2 -= K;
479 //-- printf("\n *************** RECHERCHE MINIMUM **********\n");
480 Standard_Real tolX = 0.001 * NEpsX;
481 while (Abs(x3 - x0) > tolCR*(Abs(x1) + Abs(x2)) && (Abs(x1 - x2) > tolX)) {
482 //-- printf("\n (%10.5g,%10.5g) (%10.5g,%10.5g) (%10.5g,%10.5g) (%10.5g,%10.5g) ",
483 //-- x0,f0,x1,f1,x2,f2,x3,f3);
484 if (recherche_minimum) {
485 if (f2 < f1) {
486 x0 = x1; x1 = x2; x2 = R*x1 + C*x3;
487 f0 = f1; f1 = f2; F.Value(x2, f2); f2 -= K;
488 }
489 else {
490 x3 = x2; x2 = x1; x1 = R*x2 + C*x0;
491 f3 = f2; f2 = f1; F.Value(x1, f1); f1 -= K;
492 }
493 }
494 else {
495 if (f2 > f1) {
496 x0 = x1; x1 = x2; x2 = R*x1 + C*x3;
497 f0 = f1; f1 = f2; F.Value(x2, f2); f2 -= K;
498 }
499 else {
500 x3 = x2; x2 = x1; x1 = R*x2 + C*x0;
501 f3 = f2; f2 = f1; F.Value(x1, f1); f1 -= K;
502 }
503 }
504 //-- On ne fait pas que chercher des extremas. Il faut verifier
505 //-- si on ne tombe pas sur une racine
506 if (f1*f0 < 0.0) {
507 //-- printf("\n Recherche entre (%10.5g,%10.5g) (%10.5g,%10.5g) ",x0,f0,x1,f1);
508 Solve(F, K, x0, f0, x1, f1, tol, NEpsX, Sol, NbStateSol);
509 }
510 if (f2*f3 < 0.0) {
511 //-- printf("\n Recherche entre (%10.5g,%10.5g) (%10.5g,%10.5g) ",x2,f2,x3,f3);
512 Solve(F, K, x2, f2, x3, f3, tol, NEpsX, Sol, NbStateSol);
513 }
514 }
515 if ((recherche_minimum && f1<f2) || (!recherche_minimum && f1>f2)) {
516 //-- x1,f(x1) minimum
517 if (Abs(f1) < EpsF) {
518 isSol2 = Standard_True;
519 aSolX2 = x1;
520 aVal2 = Abs(f1);
521 }
522 }
523 else {
524 //-- x2.f(x2) minimum
525 if (Abs(f2) < EpsF) {
526 isSol2 = Standard_True;
527 aSolX2 = x2;
528 aVal2 = Abs(f2);
529 }
530 }
531 // Choose the best solution between aSolX1, aSolX2
532 if (isSol1 && isSol2)
533 {
534 if (aVal2 - aVal1 > EpsF)
535 AppendRoot(Sol, NbStateSol, aSolX1, F, K, NEpsX);
536 else if (aVal1 - aVal2 > EpsF)
537 AppendRoot(Sol, NbStateSol, aSolX2, F, K, NEpsX);
538 else
539 {
540 aDer1 = Abs(aDer1);
541 F.Derivative(aSolX2, aDer2);
542 aDer2 = Abs(aDer2);
543 if (aDer1 < aDer2)
544 AppendRoot(Sol, NbStateSol, aSolX1, F, K, NEpsX);
545 else
546 AppendRoot(Sol, NbStateSol, aSolX2, F, K, NEpsX);
547 }
548 }
549 else if (isSol1)
550 AppendRoot(Sol, NbStateSol, aSolX1, F, K, NEpsX);
551 else if(isSol2)
552 AppendRoot(Sol, NbStateSol, aSolX2, F, K, NEpsX);
553 } //-- Recherche d un extrema
7fd59977 554 } //-- for
555 }
556
7fd59977 557#if NEWSEQ
3ed30348 558 #ifdef MATH_FUNCTIONROOTS_CHECK
559 {
7fd59977 560 StaticSol.Clear();
561 Standard_Integer n=Sol.Length();
562 for(Standard_Integer ii=1;ii<=n;ii++) {
563 StaticSol.Append(Sol.Value(ii));
564 }
565 Sol.Clear();
566 NbStateSol.Clear();
567 }
3ed30348 568#endif
569#endif
7fd59977 570#endif
571 }
3ed30348 572#ifdef MATH_FUNCTIONROOTS_OLDCODE
573{
7fd59977 574 //-- ********************************************************************************
575 //-- ANCIEN TRAITEMENT
576 //-- ********************************************************************************
577
578
579 // calculate all the real roots of a function within the range
580 // A..B. whitout condition on A and B
581 // a solution X is found when
582 // abs(Xi - Xi-1) <= EpsX and abs(F(Xi)-K) <= Epsf.
583 // The function is considered as null between A and B if
584 // abs(F-K) <= EpsNull within this range.
585 Standard_Real EpsX = _EpsX; //-- Cas ou le parametre va de 100000000 a 1000000001
586 //-- Il ne faut pas EpsX = 0.000...001 car dans ce cas
587 //-- U + Nn*EpsX == U
588 Standard_Real Lowr,Upp;
589 Standard_Real Increment;
590 Standard_Real Null2;
591 Standard_Real FLowr,FUpp,DFLowr,DFUpp;
592 Standard_Real U,Xu;
593 Standard_Real Fxu,DFxu,FFxu,DFFxu;
594 Standard_Real Fyu,DFyu,FFyu,DFFyu;
595 Standard_Boolean Finish;
596 Standard_Real FFi;
597 Standard_Integer Nbiter = 30;
598 Standard_Integer Iter;
599 Standard_Real Ambda,T;
600 Standard_Real AA,BB,CC;
601 Standard_Integer Nn;
602 Standard_Real Alfa1=0,Alfa2;
603 Standard_Real OldDF = RealLast();
604 Standard_Real Standard_Underflow = 1e-32 ; //-- RealSmall();
605 Standard_Boolean Ok;
606
607 Done = Standard_False;
608
609 StdFail_NotDone_Raise_if(NbSample <= 0, " ");
610
611 // initialisation
612
613 if (A > B) {
614 Lowr=B;
615 Upp=A;
616 }
617 else {
618 Lowr=A;
619 Upp=B;
620 }
621
622 Increment = (Upp-Lowr)/NbSample;
623 StdFail_NotDone_Raise_if(Increment < EpsX, " ");
624 Done = Standard_True;
625 //-- On teste si EpsX est trop petit (ie : U+Nn*EpsX == U )
626 Standard_Real DeltaU = Abs(Upp)+Abs(Lowr);
627 Standard_Real NEpsX = 0.0000000001 * DeltaU;
628 if(EpsX < NEpsX) {
629 EpsX = NEpsX;
630 //-- cout<<" \n EpsX Init = "<<_EpsX<<" devient : (deltaU : "<<DeltaU<<" ) EpsX = "<<EpsX<<endl;
631 }
632 //--
633 Null2 = EpsNull*EpsNull;
634
635 Ok = F.Values(Lowr,FLowr,DFLowr);
636
637 if(!Ok) {
638 Done = Standard_False;
639 return;
640 }
641
642 FLowr = FLowr - K;
643
644 Ok = F.Values(Upp,FUpp,DFUpp);
645
646 if(!Ok) {
647 Done = Standard_False;
648 return;
649 }
650
651 FUpp = FUpp - K;
652
653 // Calcul sur U
654
655 U = Lowr-EpsX;
656 Fyu = FLowr-EpsX*DFLowr; // extrapolation lineaire
657 DFyu = DFLowr;
658 FFyu = Fyu*Fyu;
659 DFFyu = Fyu*DFyu; DFFyu+=DFFyu;
660 AllNull = ( FFyu <= Null2 );
661
662 while ( U < Upp) {
663
664 Xu = U;
665 Fxu = Fyu;
666 DFxu = DFyu;
667 FFxu = FFyu;
668 DFFxu = DFFyu;
669
670 U = Xu + Increment;
671 if (U <= Lowr) {
672 Fyu = FLowr + (U-Lowr)*DFLowr;
673 DFyu = DFLowr;
674 }
675 else if (U >= Upp) {
676 Fyu = FUpp + (U-Upp)*DFUpp;
677 DFyu = DFUpp;
678 }
679 else {
680 Ok = F.Values(U,Fyu,DFyu);
681
682 if(!Ok) {
683 Done = Standard_False;
684 return;
685 }
686
687 Fyu = Fyu - K;
688 }
689 FFyu = Fyu*Fyu;
690 DFFyu = Fyu*DFyu; DFFyu+=DFFyu; //-- DFFyu = 2.*Fyu*DFyu;
691
692 if ( !AllNull || ( FFyu > Null2 && U <= Upp) ){
693
694 if (AllNull) { //rechercher les vraix zeros depuis le debut
695
696 AllNull = Standard_False;
697 Xu = Lowr-EpsX;
698 Fxu = FLowr-EpsX*DFLowr;
699 DFxu = DFLowr;
700 FFxu = Fxu*Fxu;
701 DFFxu = Fxu*DFxu; DFFxu+=DFFxu; //-- DFFxu = 2.*Fxu*DFxu;
702 U = Xu + Increment;
703 Ok = F.Values(U,Fyu,DFyu);
704
705 if(!Ok) {
706 Done = Standard_False;
707 return;
708 }
709
710 Fyu = Fyu - K;
711 FFyu = Fyu*Fyu;
712 DFFyu = Fyu*DFyu; DFFyu+=DFFyu;//-- DFFyu = 2.*Fyu*DFyu;
713 }
714 Standard_Real FxuFyu=Fxu*Fyu;
715
716 if ( (DFFyu > 0. && DFFxu <= 0.)
717 || (DFFyu < 0. && FFyu >= FFxu && DFFxu <= 0.)
718 || (DFFyu > 0. && FFyu <= FFxu && DFFxu >= 0.)
719 || (FxuFyu <= 0.) ) {
720 // recherche d 1 minimun possible
721 Finish = Standard_False;
722 Ambda = Increment;
723 T = 0.;
724 Iter=0;
725 FFi=0.;
726
727 if (FxuFyu >0.) {
728 // chercher si f peut s annuler pour eviter
729 // des iterations inutiles
730 if ( Fxu*(Fxu + 2.*DFxu*Increment) > 0. &&
731 Fyu*(Fyu - 2.*DFyu*Increment) > 0. ) {
732
733 Finish = Standard_True;
734 FFi = Min ( FFxu , FFyu); //pour ne pas recalculer yu
735 }
736 else if ((DFFxu <= Standard_Underflow && -DFFxu <= Standard_Underflow) ||
737 (FFxu <= Standard_Underflow && -FFxu <= Standard_Underflow)) {
738
739 Finish = Standard_True;
740 FFxu = 0.0;
741 FFi = FFyu; // pour recalculer yu
742 }
743 else if ((DFFyu <= Standard_Underflow && -DFFyu <= Standard_Underflow) ||
744 (FFyu <= Standard_Underflow && -FFyu <= Standard_Underflow)) {
745
746 Finish = Standard_True;
747 FFyu =0.0;
748 FFi = FFxu; // pour recalculer U
749 }
750 }
751 else if (FFxu <= Standard_Underflow && -FFxu <= Standard_Underflow) {
752
753 Finish = Standard_True;
754 FFxu = 0.0;
755 FFi = FFyu;
756 }
757 else if (FFyu <= Standard_Underflow && -FFyu <= Standard_Underflow) {
758
759 Finish = Standard_True;
760 FFyu =0.0;
761 FFi = FFxu;
762 }
763 while (!Finish) {
764
765 // calcul des 2 solutions annulant la derivee de l interpolation cubique
766 // Ambda*t=(U-Xu) F(t)=aa*t*t*t/3+bb*t*t+cc*t+d
767 // df=aa*t*t+2*bb*t+cc
768
769 AA = 3.*(Ambda*(DFFxu+DFFyu)+2.*(FFxu-FFyu));
770 BB = -2*(Ambda*(DFFyu+2.*DFFxu)+3.*(FFxu-FFyu));
771 CC = Ambda*DFFxu;
772
773 if(Abs(AA)<1e-14 && Abs(BB)<1e-14 && Abs(CC)<1e-14) {
774 AA=BB=CC=0;
775 }
776
777
778 math_DirectPolynomialRoots Solv(AA,BB,CC);
779 if ( !Solv.InfiniteRoots() ) {
780 Nn=Solv.NbSolutions();
781 if (Nn <= 0) {
782 if (Fxu*Fyu < 0.) { Alfa1 = 0.5;}
783 else Finish = Standard_True;
784 }
785 else {
786 Alfa1 = Solv.Value(1);
787 if (Nn == 2) {
788 Alfa2 = Solv.Value(2);
789 if (Alfa1 > Alfa2){
790 AA = Alfa1;
791 Alfa1 = Alfa2;
792 Alfa2 = AA;
793 }
794 if (Alfa1 > 1. || Alfa2 < 0.){
795 // resolution par dichotomie
796 if (Fxu*Fyu < 0.) Alfa1 = 0.5;
797 else Finish = Standard_True;
798 }
799 else if ( Alfa1 < 0. ||
800 ( DFFxu > 0. && DFFyu >= 0.) ) {
801 // si 2 derivees >0
802 //(cas changement de signe de la distance signee sans
803 // changement de signe de la derivee:
804 //cas de 'presque'tangence avec 2
805 // solutions proches) ,on prend la plus grane racine
806 if (Alfa2 > 1.) {
807 if (Fxu*Fyu < 0.) Alfa1 = 0.5;
808 else Finish = Standard_True;
809 }
810 else Alfa1 = Alfa2;
811 }
812 }
813 }
814 }
815 else if (Fxu*Fyu < -1e-14) Alfa1 = 0.5;
816 //-- else if (Fxu*Fyu < 0.) Alfa1 = 0.5;
817 else Finish = Standard_True;
818
819 if (!Finish) {
820 // petits tests pour diminuer le nombre d iterations
821 if (Alfa1 <= EpsX ) {
822 Alfa1+=Alfa1;
823 }
824 else if (Alfa1 >= (1.-EpsX) ) {
825 Alfa1 = Alfa1+Alfa1-1.;
826 }
827 Alfa1 = Ambda*(1. - Alfa1);
828 U = U + T - Alfa1;
829 if ( U <= Lowr ) {
830 AA = FLowr + (U - Lowr)*DFLowr;
831 BB = DFLowr;
832 }
833 else if ( U >= Upp ) {
834 AA = FUpp + (U - Upp)*DFUpp;
835 BB = DFUpp;
836 }
837 else {
838 Ok = F.Values(U,AA,BB);
839
840 if(!Ok) {
841 Done = Standard_False;
842 return;
843 }
844
845 AA = AA - K;
846 }
847 FFi = AA*AA;
848 CC = AA*BB; CC+=CC;
849 if ( ( ( CC < 0. && FFi < FFxu ) || DFFxu > 0.)
850 && AA*Fxu > 0. ) {
851 FFxu = FFi;
852 DFFxu = CC;
853 Fxu = AA;
854 DFxu = BB;
855 T = Alfa1;
856 if (Alfa1 > Ambda*0.5) {
857 // remarque (1)
858 // determination d 1 autre borne pour diviser
859 //le nouvel intervalle par 2 au -
860 Xu = U + Alfa1*0.5;
861 if ( Xu <= Lowr ) {
862 AA = FLowr + (Xu - Lowr)*DFLowr;
863 BB = DFLowr;
864 }
865 else if ( Xu >= Upp ) {
866 AA = FUpp + (Xu - Upp)*DFUpp;
867 BB = DFUpp;
868 }
869 else {
870 Ok = F.Values(Xu,AA,BB);
871
872 if(!Ok) {
873 Done = Standard_False;
874 return;
875 }
876
877 AA = AA -K;
878 }
879 FFi = AA*AA;
880 CC = AA*BB; CC+=CC;
881 if ( (( CC >= 0. || FFi >= FFxu ) && DFFxu <0.)
882 || Fxu * AA < 0. ) {
883 Fyu = AA;
884 DFyu = BB;
885 FFyu = FFi;
886 DFFyu = CC;
887 T = Alfa1*0.5;
888 Ambda = Alfa1*0.5;
889 }
890 else if ( AA*Fyu < 0. && AA*Fxu >0.) {
891 // changement de signe sur l intervalle u,U
892 Fxu = AA;
893 DFxu = BB;
894 FFxu = FFi;
895 DFFxu = CC;
896 FFi = Min(FFxu,FFyu);
897 T = Alfa1*0.5;
898 Ambda = Alfa1*0.5;
899 U = Xu;
900 }
901 else { Ambda = Alfa1;}
902 }
903 else { Ambda = Alfa1;}
904 }
905 else {
906 Fyu = AA;
907 DFyu = BB;
908 FFyu = FFi;
909 DFFyu = CC;
910 if ( (Ambda-Alfa1) > Ambda*0.5 ) {
911 // meme remarque (1)
912 Xu = U - (Ambda-Alfa1)*0.5;
913 if ( Xu <= Lowr ) {
914 AA = FLowr + (Xu - Lowr)*DFLowr;
915 BB = DFLowr;
916 }
917 else if ( Xu >= Upp ) {
918 AA = FUpp + (Xu - Upp)*DFUpp;
919 BB = DFUpp;
920 }
921 else {
922 Ok = F.Values(Xu,AA,BB);
923
924 if(!Ok) {
925 Done = Standard_False;
926 return;
927 }
928
929 AA = AA - K;
930 }
931 FFi = AA*AA;
932 CC = AA*BB; CC+=CC;
933 if ( AA*Fyu <=0. && AA*Fxu > 0.) {
934 FFxu = FFi;
935 DFFxu = CC;
936 Fxu = AA;
937 DFxu = BB;
938 Ambda = ( Ambda - Alfa1 )*0.5;
939 T = 0.;
940 }
941 else if ( AA*Fxu < 0. && AA*Fyu > 0.) {
942 FFyu = FFi;
943 DFFyu = CC;
944 Fyu = AA;
945 DFyu = BB;
946 Ambda = ( Ambda - Alfa1 )*0.5;
947 T = 0.;
948 FFi = Min(FFxu , FFyu);
949 U = Xu;
950 }
951 else {
952 T =0.;
953 Ambda = Ambda - Alfa1;
954 }
955 }
956 else {
957 T =0.;
958 Ambda = Ambda - Alfa1;
959 }
960 }
961 // tests d arrets
962 if (Abs(FFxu) <= Standard_Underflow ||
963 (Abs(DFFxu) <= Standard_Underflow && Fxu*Fyu > 0.)
964 ){
965 Finish = Standard_True;
966 if (Abs(FFxu) <= Standard_Underflow ) {FFxu =0.0;}
967 FFi = FFyu;
968 }
969 else if (Abs(FFyu) <= Standard_Underflow ||
970 (Abs(DFFyu) <= Standard_Underflow && Fxu*Fyu > 0.)
971 ){
972 Finish = Standard_True;
973 if (Abs(FFyu) <= Standard_Underflow ) {FFyu =0.0;}
974 FFi = FFxu;
975 }
976 else {
977 Iter = Iter + 1;
978 Finish = Iter >= Nbiter || (Ambda <= EpsX &&
979 ( Fxu*Fyu >= 0. || FFi <= EpsF*EpsF));
980 }
981 }
982 } // fin interpolation cubique
983
984 // restitution du meilleur resultat
985
986 if ( FFxu < FFi ) { U = U + T -Ambda;}
987 else if ( FFyu < FFi ) { U = U + T;}
988
989 if ( U >= (Lowr -EpsX) && U <= (Upp + EpsX) ) {
990 U = Max( Lowr , Min( U , Upp));
991 Ok = F.Value(U,FFi);
992 FFi = FFi - K;
993 if ( Abs(FFi) < EpsF ) {
994 //coherence
995 if (Abs(Fxu) <= Standard_Underflow) { AA = DFxu;}
996 else if (Abs(Fyu) <= Standard_Underflow) { AA = DFyu;}
997 else if (Fxu*Fyu > 0.) { AA = 0.;}
998 else { AA = Fyu-Fxu;}
999 if (!Sol.IsEmpty()) {
1000 if (Abs(Sol.Last() - U) > 5.*EpsX
f4dee9bb 1001 || (OldDF != RealLast() && OldDF*AA < 0.) ) {
7fd59977 1002 Sol.Append(U);
1003 NbStateSol.Append(F.GetStateNumber());
1004 }
1005 }
1006 else {
1007 Sol.Append(U);
1008 NbStateSol.Append(F.GetStateNumber());
1009 }
1010 OldDF = AA ;
1011 }
1012 }
1013 DFFyu = 0.;
1014 Fyu = 0.;
1015 Nn = 1;
1016 while ( Nn < 1000000 && DFFyu <= 0.) {
1017 // on repart d 1 pouyem plus loin
1018 U = U + Nn*EpsX;
1019 if ( U <= Lowr ) {
1020 AA = FLowr + (U - Lowr)*DFLowr;
1021 BB = DFLowr;
1022 }
1023 else if ( U >= Upp ) {
1024 AA = FUpp + (U - Upp)*DFUpp;
1025 BB = DFUpp;
1026 }
1027 else {
1028 Ok = F.Values(U,AA,BB);
1029 AA = AA - K;
1030 }
1031 if ( AA*Fyu < 0.) {
1032 U = U - Nn*EpsX;
1033 Nn = 1000001;
1034 }
1035 else {
1036 Fyu = AA;
1037 DFyu = BB;
1038 FFyu = AA*AA;
1039 DFFyu = 2.*DFyu*Fyu;
1040 Nn = 2*Nn;
1041 }
1042 }
1043 }
1044 }
1045 }
1046#if NEWSEQ
3ed30348 1047 #ifdef MATH_FUNCTIONROOTS_CHECK
1048 {
7fd59977 1049 Standard_Integer n1=StaticSol.Length();
1050 Standard_Integer n2=Sol.Length();
1051 if(n1!=n2) {
1052 printf("\n mathFunctionRoots : n1=%d n2=%d EpsF=%g EpsX=%g\n",n1,n2,EpsF,NEpsX);
1053 for(Standard_Integer x1=1; x1<=n1;x1++) {
1054 Standard_Real v; F.Value(StaticSol(x1),v); v-=K;
1055 printf(" (%+13.8g:%+13.8g) ",StaticSol(x1),v);
1056 }
1057 printf("\n");
1058 for(Standard_Integer x2=1; x2<=n2; x2++) {
1059 Standard_Real v; F.Value(Sol(x2),v); v-=K;
1060 printf(" (%+13.8g:%+13.8g) ",Sol(x2),v);
1061 }
1062 printf("\n");
1063 }
1064 Standard_Integer n=n1;
1065 if(n1>n2) n=n2;
1066 for(Standard_Integer i=1;i<=n;i++) {
1067 Standard_Real t = Sol(i)-StaticSol(i);
7fd59977 1068 if(Abs(t)>NEpsX) {
1069 printf("\n mathFunctionRoots : i:%d/%d delta: %g",i,n,t);
1070 }
1071 }
1072 }
1073#endif
3ed30348 1074#endif
7fd59977 1075}
3ed30348 1076#endif
7fd59977 1077}
1078
1079
1080void math_FunctionRoots::Dump(Standard_OStream& o) const
1081{
1082
1083 o << "math_FunctionRoots ";
1084 if(Done) {
1085 o << " Status = Done \n";
1086 o << " Number of solutions = " << Sol.Length() << endl;
1087 for (Standard_Integer i = 1; i <= Sol.Length(); i++) {
1088 o << " Solution Number " << i << "= " << Sol.Value(i) << endl;
1089 }
1090 }
1091 else {
1092 o<< " Status = not Done \n";
1093 }
1094}
Open CASCADE Technology repository