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