NumA/root_finding/Brent.h

0001 #include <ElementaryUtils/logger/LoggerManager.h>
0002 #include <stddef.h>
0003 #include <cmath>
0004 //#include <stdexcept>
0005 #include <vector>
0006
0007 #define MIN(a, b) ((a) < (b) ? (a) : (b))
0008
0009 namespace NumA {
0010
0011 //TODO refactoring code from python library ; more explicit variable and documentation ; add status return if max iteration reached
0012
0013 class Brent {
0014 public:
0015
0016     template<class OBJ, typename FUNC>
0017     double solve(OBJ* object, FUNC function,
0018             std::vector<double> &funcParameters, double a, double b,
0019             const double absTolerance = 1e-8, const double relTolerance = 1e-10,
0020             const size_t maxIteration = 50) {
0021         double xpre = a, xcur = b;
0022         double xblk = 0., fpre = 0., fcur = 0., fblk = 0., spre = 0., scur = 0.,
0023                 sbis = 0., tol = 0.;
0024         double stry = 0., dpre = 0., dblk = 0.;
0025
0026         fpre = ((*object).*function)(xpre, funcParameters);
0027         fcur = ((*object).*function)(xcur, funcParameters);
0028
0029         if ((fpre * fcur) > 0.) {
0030             throw ElemUtils::CustomException("Brent", __func__,
0031                     "The root is not bracketed ; verify your interval [a,b]");
0032         }
0033         if (fpre == 0.) {
0034             return xpre;
0035         }
0036         if (fcur == 0.) {
0037             return xcur;
0038         }
0039
0040         for (size_t i = 0; i < maxIteration; i++) {
0041             if ((fpre * fcur) < 0.) {
0042                 xblk = xpre;
0043                 fblk = fpre;
0044                 spre = scur = xcur - xpre;
0045             }
0046             if (fabs(fblk) < fabs(fcur)) {
0047                 xpre = xcur;
0048                 xcur = xblk;
0049                 xblk = xpre;
0050
0051                 fpre = fcur;
0052                 fcur = fblk;
0053                 fblk = fpre;
0054             }
0055
0056             tol = absTolerance + relTolerance * fabs(xcur);
0057             sbis = (xblk - xcur) / 2;
0058             if (fcur == 0 || fabs(sbis) < tol) {
0059                 return xcur;
0060             }
0061
0062             if (fabs(spre) > tol && fabs(fcur) < fabs(fpre)) {
0063                 if (xpre == xblk) {
0064                     /* interpolate */
0065                     stry = -fcur * (xcur - xpre) / (fcur - fpre);
0066                 } else {
0067                     /* extrapolate */
0068                     dpre = (fpre - fcur) / (xpre - xcur);
0069                     dblk = (fblk - fcur) / (xblk - xcur);
0070                     stry = -fcur * (fblk * dblk - fpre * dpre)
0071                             / (dblk * dpre * (fblk - fpre));
0072                 }
0073                 if (2 * fabs(stry) < MIN(fabs(spre), 3 * fabs(sbis) - tol)) {
0074                     /* good short step */
0075                     spre = scur;
0076                     scur = stry;
0077                 } else {
0078                     /* bisect */
0079                     spre = sbis;
0080                     scur = sbis;
0081                 }
0082             } else {
0083                 /* bisect */
0084                 spre = sbis;
0085                 scur = sbis;
0086             }
0087
0088             xpre = xcur;
0089             fpre = fcur;
0090             if (fabs(scur) > tol) {
0091                 xcur += scur;
0092             } else {
0093                 xcur += (sbis > 0 ? tol : -tol);
0094             }
0095
0096             fcur = ((*object).*function)(xcur, funcParameters);
0097         }
0098
0099         return xcur;
0100     }
0101 };
0102
0103 } // namespace NumA