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