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