outil/src/ot_root_find.cpp

//---------------------------------------------------------------------------


#include <stdio.h>
#include <math.h>

#pragma hdrstop
                    
#include "gestionversion.h"       
#include "ot_root_find.h"

//---------------------------------------------------------------------------
#pragma package(smart_init)

#define SMALL_NUM_1 1E-14
#define SMALL_NUM_2 1E-28

unsigned OT_ROOT_FIND_1D::ZeroBracOut (Function fn, double &x1, double &x2, void* pvData) {
  const double FACTOR = 1.6;
  const unsigned NTRY = 50;

  double f1 = fn (x1, pvData), f2 = fn (x2, pvData);
  unsigned j;

  if (x1 == x2)
  {
      //    printf ("you have to guess an initial range");
    return 1E308;
  }
  for (j = 1; j <= NTRY; j++) {
    if (f1*f2 < 0.) return 1;           // found a root
    if (fabs (f1) < fabs (f2)){
      x1 += FACTOR*(x1-x2);             // expand towards x1
      f1 = fn (x1, pvData); }
    else {
      x2 += FACTOR*(x2-x1);             // ... or towards x2 
      f2 = fn (x2, pvData); } }
  return 1E308;
}                               // no root bracketed

unsigned OT_ROOT_FIND_1D::ZeroBracOut (Function fn, double xStart, double xEnd, double &x1, double &x2, void* pvData) {
  const double FACTOR = 1.6;
  const unsigned NTRY = 50;

    if (x1>x2) {double tmp=x1;x1=x2;x2=tmp;}   
    if (xStart>xEnd) {double tmp=xStart;xEnd=xStart;xEnd=tmp;}   
    double deltaT=xEnd-xStart;

    if (x1 < xStart)
        x1 = xStart;
    else if (x1 > xEnd)
        x1 = xEnd;
    if (x2 < xStart)
        x2 = xStart;
    else if (x2 > xEnd)
        x2 = xEnd;

    if (x1 == x2)
    {
        x1 -= .1*deltaT;      
        x1 = (x1>xStart)?x1:xStart;
        x2 += .1*deltaT;
        x2 = (x2<xEnd)?x2:xEnd;
    }
    
  double f1 = fn (x1, pvData), f2 = fn (x2, pvData);
  unsigned j;

  if (x1 == x2)
  {
      //    printf ("you have to guess an initial range");
    return 1E308;
  }
  for (j = 1; j <= NTRY; j++) {
    if (f1*f2 < 0.) return 1;           // found a root
    if (fabs (f1) < fabs (f2) && x1 > xStart )
    {
      if (x1+FACTOR*(x1-x2) > xStart)
      {
        x1 += FACTOR*(x1-x2);             // expand towards x1
      }
      else
      {
        x1 = xStart;
      }              
      f1 = fn (x1, pvData);
    }
    else if (x2 < xEnd ) {
      if (x2+FACTOR*(x2-x1) < xEnd)
      {
        x2 += FACTOR*(x2-x1);             // ... or towards x2
      }
      else
      {
        x2 = xEnd;
      }
      f2 = fn (x2, pvData);
    }
    else
        break;
  }
  return 0;
}                               // no root bracketed

unsigned OT_ROOT_FIND_1D::ZeroBracIn (Function fn, const double &x1, const double &x2,
                     unsigned n, double xb1[], double xb2[], unsigned nb, void* pvData) {
  unsigned i, nbb = nb;
  double x = x1, dx = (x2-x1)/n,        // determine appropriate spacing    
         fp = fn (x, pvData);

  nb = 0;
  for (i = 1; i <= n; i++) {            // loop over all intervals          
    double fc;

    x += dx;
    fc = fn (x, pvData);
    if (fc*fp < 0.) {                   // if a sign change occurs, record  
      xb1[nb] = x-dx;                   // values for the bounds.           
      xb2[nb] = x;
      nb++;}
    fp = fc;
    if (nbb == nb) return nb; }
  return nb;
}


double OT_ROOT_FIND_1D::RootBisect (Function fn, const double &x1, const double &x2, const double &xacc, void* pvData)
{
  const unsigned JMAX = 50;
  double fmid = fn (x2, pvData), f = fn (x1, pvData), rtbis, dx;
  unsigned j;

  if (fmid*f >= 0)
  {
      //    printf ("root must be bracketed for bisection");
    return 1E308;
  }

  if (f < 0.) {                         // orient the search so that f > 0  
    rtbis = x1;                         // lies at x+dx.                    
    dx = x2-x1; }
  else {
    rtbis = x2;
    dx = x1-x2; }

  for (j = 1; j <= JMAX; j++) {         // bisection loop                   
    double xmid;

    dx *= .5;                           // here we halve the interval
    xmid = rtbis+dx;                    // and update boundaries and
    fmid = fn (xmid, pvData);                   // function value
    if (fmid <= 0.) rtbis = xmid;
    if ((fabs (dx) <= xacc)||(fmid == 0.)) return rtbis; }
  //  printf ("too many bisections");
  return 1E308;
}       // this should happen rarely (2^-50 ~ 10^-15)


double OT_ROOT_FIND_1D::RootFlsPos (Function fn, const double &x1, const double &x2,
                   const double &xacc, void* pvData) {
  const unsigned MAXIT = 40;
  double flo = fn (x1, pvData), fhi = fn (x2, pvData), xl, xh, dx, dummy;
  unsigned j;

  if (flo*fhi >= 0)
  {
      //    printf ("root must be bracketed for false position");
    return 1E308;
  }

  if (flo < 0.) {                       // let xl correspond to the low side
    xl = x1; xh = x2;
  }
  else {
    { dummy=flo;
      flo=fhi;
      fhi=dummy;
    }
    xl = x2; xh = x1;
  }
  dx = xh-xl;

  for (j = 1; j <= MAXIT; j++) {        // false position loop              
    double rtflsp = xl+dx*flo/(flo-fhi),// increment w/ respect to latest   
           f = fn (rtflsp, pvData),             // value                            
           del = (f < 0.)? xl-rtflsp: xh-rtflsp;

    if (f < 0.) {                       // replace appropriate limit        
      xl = rtflsp; flo = f; }
    else {
      xh = rtflsp; fhi = f; }
    dx = xh-xl;                 

    if ((fabs (del) <= xacc)||(f == 0.)) return rtflsp; }       // convergence

  //  printf  ("RootFlsPos exceeded maximum iterations");
  return 1E308;
}


double OT_ROOT_FIND_1D::RootNewton (Function fn, Function df,
                   const double &x1, const double &x2, const double &xacc, void* pvData) {
  const unsigned JMAX = 20;
  double rtnewt = .5*(x1+x2);           // initial guess                    
  unsigned j;

  for (j = 1; j <= JMAX; j++) {
    double f = fn (rtnewt, pvData), fs = df (rtnewt, pvData),
           dx = f/fs;

    rtnewt -= dx;
    if ((x1-rtnewt)*(rtnewt-x2) < 0.)
    {
        //      printf ("jumped out of brackets");
      return 1E308;
    }
    if (fabs (dx) < xacc) return rtnewt; }       // convergence

  //    printf("RootNewton exceeded maximum iterations");
    return 1E308;
}

double OT_ROOT_FIND_1D::RootSafe (Function fn, Function df,
                 const double &x1, const double &x2, const double &xacc, void* pvData) {
  const unsigned JMAX = 100;

  double fl = fn (x1, pvData), fh = fn (x2, pvData), xl, xh;

  if (fl*fh > SMALL_NUM_2)
  {
      //    printf ("root must be bracketed");
    return 1E308;
  }
  if (fl < 0) {                         // orient the search so that f (xl) < 0
    xl = x1; xh = x2; }
  else {
    xl = x2; xh = x1; }

  double rtsafe = .5*(x1+x2);           // initial guess
  double dxold = fabs (x2-x1),
         dx = dxold;
  double f = fn (rtsafe, pvData), fp = df (rtsafe, pvData);

  for (unsigned j = 1; j <= JMAX; j++) {
    if ( ( ((rtsafe-xh)*fp-f)*((rtsafe-xl)*fp-f) >= 0 ) ||  // bisect if newton out of brackets
         ( fabs (2*f) > fabs (dxold*fp) ) ) {                 // or not decreasing fast enough
      dxold = dx;
      dx = .5*(xh -xl);
      rtsafe = xl+dx;
      if (xl == rtsafe) return rtsafe; } // change in root is negligible
    else {
      dxold = dx;
      dx = f/fp;
      double temp = rtsafe;
      rtsafe -= dx;
      if (temp == rtsafe) return rtsafe; }
    if (fabs (dx) < xacc) return rtsafe;  // convergence
    f = fn (rtsafe, pvData); fp = df (rtsafe, pvData);
    if (f < 0) xl = rtsafe;
    else       xh = rtsafe; } 
  //   printf("too many iterations");
   return 1E308;
}

Revision:	253
Committed:	Tue Jul 13 19:40:46 2010 UTC (14 years, 11 months ago) by francois
Original Path:	*magic/lib/outil/src/ot_root_find.cpp*
File size:	7434 byte(s)
Log Message:	changement de hiearchie et utilisation de ccmake + mise a jour
#	User	Rev	Content
1	foucault	27	//---------------------------------------------------------------------------
2
3
4			#include <stdio.h>
5			#include <math.h>
6
7			#pragma hdrstop
8
9			#include "gestionversion.h"
10			#include "ot_root_find.h"
11
12			//---------------------------------------------------------------------------
13			#pragma package(smart_init)
14
15			#define SMALL_NUM_1 1E-14
16			#define SMALL_NUM_2 1E-28
17
18			unsigned OT_ROOT_FIND_1D::ZeroBracOut (Function fn, double &x1, double &x2, void* pvData) {
19			const double FACTOR = 1.6;
20			const unsigned NTRY = 50;
21
22			double f1 = fn (x1, pvData), f2 = fn (x2, pvData);
23			unsigned j;
24
25			if (x1 == x2)
26			{
27			// printf ("you have to guess an initial range");
28			return 1E308;
29			}
30			for (j = 1; j <= NTRY; j++) {
31			if (f1*f2 < 0.) return 1; // found a root
32			if (fabs (f1) < fabs (f2)){
33			x1 += FACTOR*(x1-x2); // expand towards x1
34			f1 = fn (x1, pvData); }
35			else {
36			x2 += FACTOR*(x2-x1); // ... or towards x2
37			f2 = fn (x2, pvData); } }
38			return 1E308;
39			} // no root bracketed
40
41			unsigned OT_ROOT_FIND_1D::ZeroBracOut (Function fn, double xStart, double xEnd, double &x1, double &x2, void* pvData) {
42			const double FACTOR = 1.6;
43			const unsigned NTRY = 50;
44
45			if (x1>x2) {double tmp=x1;x1=x2;x2=tmp;}
46			if (xStart>xEnd) {double tmp=xStart;xEnd=xStart;xEnd=tmp;}
47			double deltaT=xEnd-xStart;
48
49			if (x1 < xStart)
50			x1 = xStart;
51			else if (x1 > xEnd)
52			x1 = xEnd;
53			if (x2 < xStart)
54			x2 = xStart;
55			else if (x2 > xEnd)
56			x2 = xEnd;
57
58			if (x1 == x2)
59			{
60			x1 -= .1*deltaT;
61			x1 = (x1>xStart)?x1:xStart;
62			x2 += .1*deltaT;
63			x2 = (x2<xEnd)?x2:xEnd;
64			}
65
66			double f1 = fn (x1, pvData), f2 = fn (x2, pvData);
67			unsigned j;
68
69			if (x1 == x2)
70			{
71			// printf ("you have to guess an initial range");
72			return 1E308;
73			}
74			for (j = 1; j <= NTRY; j++) {
75			if (f1*f2 < 0.) return 1; // found a root
76			if (fabs (f1) < fabs (f2) && x1 > xStart )
77			{
78			if (x1+FACTOR*(x1-x2) > xStart)
79			{
80			x1 += FACTOR*(x1-x2); // expand towards x1
81			}
82			else
83			{
84			x1 = xStart;
85			}
86			f1 = fn (x1, pvData);
87			}
88			else if (x2 < xEnd ) {
89			if (x2+FACTOR*(x2-x1) < xEnd)
90			{
91			x2 += FACTOR*(x2-x1); // ... or towards x2
92			}
93			else
94			{
95			x2 = xEnd;
96			}
97			f2 = fn (x2, pvData);
98			}
99			else
100			break;
101			}
102			return 0;
103			} // no root bracketed
104
105			unsigned OT_ROOT_FIND_1D::ZeroBracIn (Function fn, const double &x1, const double &x2,
106			unsigned n, double xb1[], double xb2[], unsigned nb, void* pvData) {
107			unsigned i, nbb = nb;
108			double x = x1, dx = (x2-x1)/n, // determine appropriate spacing
109			fp = fn (x, pvData);
110
111			nb = 0;
112			for (i = 1; i <= n; i++) { // loop over all intervals
113			double fc;
114
115			x += dx;
116			fc = fn (x, pvData);
117			if (fc*fp < 0.) { // if a sign change occurs, record
118			xb1[nb] = x-dx; // values for the bounds.
119			xb2[nb] = x;
120			nb++;}
121			fp = fc;
122			if (nbb == nb) return nb; }
123			return nb;
124			}
125
126
127			double OT_ROOT_FIND_1D::RootBisect (Function fn, const double &x1, const double &x2, const double &xacc, void* pvData)
128			{
129			const unsigned JMAX = 50;
130			double fmid = fn (x2, pvData), f = fn (x1, pvData), rtbis, dx;
131			unsigned j;
132
133			if (fmid*f >= 0)
134			{
135			// printf ("root must be bracketed for bisection");
136			return 1E308;
137			}
138
139			if (f < 0.) { // orient the search so that f > 0
140			rtbis = x1; // lies at x+dx.
141			dx = x2-x1; }
142			else {
143			rtbis = x2;
144			dx = x1-x2; }
145
146			for (j = 1; j <= JMAX; j++) { // bisection loop
147			double xmid;
148
149			dx *= .5; // here we halve the interval
150			xmid = rtbis+dx; // and update boundaries and
151			fmid = fn (xmid, pvData); // function value
152			if (fmid <= 0.) rtbis = xmid;
153			if ((fabs (dx) <= xacc)\|\|(fmid == 0.)) return rtbis; }
154			// printf ("too many bisections");
155			return 1E308;
156			} // this should happen rarely (2^-50 ~ 10^-15)
157
158
159			double OT_ROOT_FIND_1D::RootFlsPos (Function fn, const double &x1, const double &x2,
160			const double &xacc, void* pvData) {
161			const unsigned MAXIT = 40;
162			double flo = fn (x1, pvData), fhi = fn (x2, pvData), xl, xh, dx, dummy;
163			unsigned j;
164
165			if (flo*fhi >= 0)
166			{
167			// printf ("root must be bracketed for false position");
168			return 1E308;
169			}
170
171			if (flo < 0.) { // let xl correspond to the low side
172			xl = x1; xh = x2;
173			}
174			else {
175			{ dummy=flo;
176			flo=fhi;
177			fhi=dummy;
178			}
179			xl = x2; xh = x1;
180			}
181			dx = xh-xl;
182
183			for (j = 1; j <= MAXIT; j++) { // false position loop
184			double rtflsp = xl+dx*flo/(flo-fhi),// increment w/ respect to latest
185			f = fn (rtflsp, pvData), // value
186			del = (f < 0.)? xl-rtflsp: xh-rtflsp;
187
188			if (f < 0.) { // replace appropriate limit
189			xl = rtflsp; flo = f; }
190			else {
191			xh = rtflsp; fhi = f; }
192			dx = xh-xl;
193
194			if ((fabs (del) <= xacc)\|\|(f == 0.)) return rtflsp; } // convergence
195
196			// printf ("RootFlsPos exceeded maximum iterations");
197			return 1E308;
198			}
199
200
201			double OT_ROOT_FIND_1D::RootNewton (Function fn, Function df,
202			const double &x1, const double &x2, const double &xacc, void* pvData) {
203			const unsigned JMAX = 20;
204			double rtnewt = .5*(x1+x2); // initial guess
205			unsigned j;
206
207			for (j = 1; j <= JMAX; j++) {
208			double f = fn (rtnewt, pvData), fs = df (rtnewt, pvData),
209			dx = f/fs;
210
211			rtnewt -= dx;
212			if ((x1-rtnewt)*(rtnewt-x2) < 0.)
213			{
214			// printf ("jumped out of brackets");
215			return 1E308;
216			}
217			if (fabs (dx) < xacc) return rtnewt; } // convergence
218
219			// printf("RootNewton exceeded maximum iterations");
220			return 1E308;
221			}
222
223			double OT_ROOT_FIND_1D::RootSafe (Function fn, Function df,
224			const double &x1, const double &x2, const double &xacc, void* pvData) {
225			const unsigned JMAX = 100;
226
227			double fl = fn (x1, pvData), fh = fn (x2, pvData), xl, xh;
228
229			if (fl*fh > SMALL_NUM_2)
230			{
231			// printf ("root must be bracketed");
232			return 1E308;
233			}
234			if (fl < 0) { // orient the search so that f (xl) < 0
235			xl = x1; xh = x2; }
236			else {
237			xl = x2; xh = x1; }
238
239			double rtsafe = .5*(x1+x2); // initial guess
240			double dxold = fabs (x2-x1),
241			dx = dxold;
242			double f = fn (rtsafe, pvData), fp = df (rtsafe, pvData);
243
244			for (unsigned j = 1; j <= JMAX; j++) {
245			if ( ( ((rtsafe-xh)fp-f)((rtsafe-xl)*fp-f) >= 0 ) \|\| // bisect if newton out of brackets
246			( fabs (2f) > fabs (dxoldfp) ) ) { // or not decreasing fast enough
247			dxold = dx;
248			dx = .5*(xh -xl);
249			rtsafe = xl+dx;
250			if (xl == rtsafe) return rtsafe; } // change in root is negligible
251			else {
252			dxold = dx;
253			dx = f/fp;
254			double temp = rtsafe;
255			rtsafe -= dx;
256			if (temp == rtsafe) return rtsafe; }
257			if (fabs (dx) < xacc) return rtsafe; // convergence
258			f = fn (rtsafe, pvData); fp = df (rtsafe, pvData);
259			if (f < 0) xl = rtsafe;
260			else xh = rtsafe; }
261			// printf("too many iterations");
262			return 1E308;
263			}
264