FunctionTools.h

Go to the documentation of this file.

#ifndef FunctionTools_h
#define FunctionTools_h

#include <math.h>
#include <cfloat>

#include <QList>
#include <QString>

#include "Constants.h"
#include "IException.h"
#include "IString.h"

namespace Isis {
  class FunctionTools {
    public:


    static QList <double> realLinearRoots(double coeffLinearTerm, double coeffConstTerm) {
      double m=coeffLinearTerm, b=coeffConstTerm;

      QList<double> roots;

      //if the slope is zero there are either 0 or infinite roots
      //  for the present I see no need to handle the infinite roots situation more elegantly...
      if (m==0.0) 
        return roots;

      roots << -b/m;

      return roots;    
    }

    static QList <double> realQuadraticRoots(double coeffQuadTerm, double coeffLinearTerm, 
                                     double coeffConstTerm) {
      double A=coeffQuadTerm, B=coeffLinearTerm, C=coeffConstTerm;
      double disc,q,temp; //helpers on the way to a solution 

      if (A==0.0)
        return realLinearRoots(coeffLinearTerm, coeffConstTerm);

      QList<double> roots;
    
      disc = B*B-4*A*C;
      if (disc < 0) return roots;  //no solution, return empty set
      q = -0.5*(B + (B < 0 ? -1.0 : 1.0)*sqrt(disc));
 
      if (q == 0) roots << q/A;
      else {
        roots << q/A;
        //after the first root make sure there are no duplicates
        temp = C/q;
        if (!roots.contains(temp)) roots << temp; 
      }      
      return roots;
    }



    static QList <double> realCubicRoots(double coeffCubicTerm,  double coeffQuadTerm, 
                                 double coeffLinearTerm, double coeffConstTerm) {
      double a=coeffQuadTerm, b=coeffLinearTerm, c=coeffConstTerm;
      double Q,R; //helpers on the way to a solution 

      //first verify this is really a cubic
      if (coeffCubicTerm == 0.0) 
        return realQuadraticRoots(coeffQuadTerm,coeffLinearTerm,coeffConstTerm);
      

      //algorithm wants the leading coefficient to be 1.0
      if ( coeffCubicTerm != 1.0) {
        a /= coeffCubicTerm;
        b /= coeffCubicTerm;
        c /= coeffCubicTerm;         
      }
      
      QList<double> roots;  

      Q = (a*a - 3.0*b) / 9.0;
      R = (2*a*a*a - 9.0*a*b + 27.0*c) / 54.0;
      //cout << "Q R: " << Q << " " << R << "\n";

      if ( a == 0.0 && b == 0.0 ) { //one simple root
        roots << (c < 0 ? 1.0 : -1.0)*pow(fabs(c),1.0/3.0);
      }
      else if (R*R <= Q*Q*Q) {  //there are three roots (one of them can be a double root)
        double theta = acos(R/sqrt(Q*Q*Q)),temp;
        //cout << "three roots, theta: " << theta << "\n";
        Q = -2.0*sqrt(Q); //just done to save some FLOPs
        roots << Q*cos(theta/3.0) - a /3.0;
        //after the first root make sure there are no duplicates
        temp = Q*cos((theta + TWOPI)/3.0) - a / 3.0;
        if (!roots.contains(temp)) roots << temp;
        temp = Q*cos((theta - TWOPI)/3.0) - a / 3.0;
        if (!roots.contains(temp)) roots << temp;
      }
      else {  //there is a single real root
        double A, B;
        A = (R < 0 ? 1.0 : -1.0 ) * pow(fabs(R) + sqrt(R*R - Q*Q*Q),1.0/3.0);
        B = A == 0.0 ? 0.0 : Q / A;
        
        roots << (A + B) - a / 3.0;
      }

      return roots;
    }

    template <typename Functor> 
    static bool brentsRootFinder(Functor &func, const QList<double> pt1, 
                          const QList<double> pt2, double tol, int maxIter, double &root) {
      double a=pt1[0], b=pt2[0], c, d=0, fa = pt1[1], fb = pt2[1], fc, p, q, r, s, t,tol1, bnew, fbnew, temp, deltaI,deltaF;

      //offset used for improved numerical stability
      double offset = (a + b) / 2.0;
      a -= offset;
      b -= offset;

      //check to see if the points bracket a root(s), if the signs are equal they don't
      if ( (fa > 0) - (fa < 0) == (fb > 0) - (fb < 0) ) {
        QString msg = "The function evaluations of two bounding points passed to Brents Method "
                      "have the same sign.  Therefore, they don't necessary bound a root.  No "
                      "root finding will be attempted.\n";
        throw IException(IException::Programmer, msg, _FILEINFO_);
        return false;
      }

      bool mflag=true;

      if (fabs(fa) < fabs(fb)) {
        //if a is a better guess for the root than b, switch them
          //b is always the current best guess for the root
        temp = a;
        a = b;
        b = temp;
        temp = fa;
        fa = fb;
        fb = temp;
      }
   
      c=a;
      fc=fa;

      for (int iter=0;iter<maxIter; iter++) {
        tol1 = DBL_EPSILON*2.0*fabs(b);  //numerical tolerance
        if (a != c && b != c) {
          //inverse quadratic interpolation
          r = fb/fc;
          s = fb/fa;
          t = fa/fc;
          p = s*(t*(r-t)*(c-b)-(1-r)*(b-a));
          q = (t-1)*(r-1)*(s-1);
          bnew = b + p/q;     
        }
        else {
          //secant rule
          bnew = b - fb * (b - a) / (fb - fa);
        }

        //five tests follow to determine if the iterpolation methods are working better than 
          //bisection. p and q are setup as the bounds we want the new root guess to fall within.
          //this enforces that the new root guess be withing the 3/4 of the interval closest to
          //b, the current best guess.
        temp = (3*a+b)/4.0;
        p = temp < b ? temp : b;
        q = b > temp ? b : temp;
        deltaI = fabs(b - bnew); //magnitude of the interpolated correctio
        if ( //if the root isn't within the 3/4 of the interval closest to b (the current best guess)
             (bnew < p || bnew > q )                     ||  
             //or if the last iteration was a bisection 
               //and the new correction is greater magnitude than half the magnitude of last
               //correction, ie it's doing less to narrow the root than a bisection would
             (mflag && deltaI >= fabs(b-c)/2.0)  ||
             //or if the last iteration was an interpolation
               //and the new correction magnitude is greater than 
               //the half the magnitude of the correction from two iterations ago,
               //i.e. it's not converging faster than bisection
             (!mflag && deltaI >= fabs(c-d)/2.0) ||
             //or if the last iteration was a bisection
               //and the last correction was less than the numerical tolerance  
               //ie we are reaching the limits of our numerical
               //ability to find a better root so lets do bisection, it's numerical safer
             (mflag && fabs(b-c) < tol1)                 ||
             //or it the last iteration was an interpolation
               //and the correction from two iteration ago was less than the current
               //numerical tolerance, ie we are reaching the limits of our numerical
               //ability to find a better root so lets do bisection, it's numerical safer
             (!mflag && fabs(c-d) < tol1)            ) {

          //bisection method
          bnew = (a + b)/2.0;
          mflag=true;
        }
        else {
          mflag=false;
        }
        try {
          fbnew = func(bnew + offset);
        } catch (IException &e) {
          QString msg = "Function evaluation failed at:" + toString(bnew) + 
                        ".  Function must be continuous and defined for the entire interval "
                        "inorder to gaurentee brentsRootFinder will work.";
          throw IException(e, IException::Programmer, msg, _FILEINFO_);
        }
        d = c;  //thus d always equals the best guess from two iterations ago
        c = b;  //thus c always equals the best giess from the previous iteration
        fc = fb;
        
        if ( (fa > 0) - (fa < 0) == (fbnew > 0) - (fbnew < 0) ) {
          //if b and bnew bracket the root
          deltaF = fabs(a - bnew); //the Final magnitude of the correction
          a = bnew;
          fa = fbnew;
        }
        else {
          //a and bnew bracket the root
          deltaF = fabs(b - bnew); //the Final magnitude of the correction
          b = bnew;
          fb = fbnew;
        }
    
        if (fabs(fa) < fabs(fb)) {
          //if a is a better guess for the root than b, switch them
          temp = a;
          a = b;
          b = temp;
          temp = fa;
          fa = fb;
          fb = temp;
        }
        if (fabs(fb) < tol) {
          //if the tolerance is meet
          root = b + offset;
          return true;  
        }
        else if ( deltaF < tol1 && fabs(b) < 100.0*tol) {
          //we've reach the numerical limit to how well the root can be defined, and the function
          //  is at least approaching zero
          //This was added specifically for the apollo pan camera, the camera classes (and maybe
          // NAIF as well can not actually converge to zero for the extreme edges of some pan
          // images (partial derivates WRT to line approach infinity).  They can get close 
          // 'enough' however
          root = b + offset;
          return true;  
        }
        else if (deltaF < tol1) {
          //we've reached the limit of the numerical ability to refine the root and the function
          // is not near zero.  This is a clasically ill defined root (nearly vertical function)
          // "This is not the [root] you're looking for"
          return false;
        }
      }
      //maximum number of iteration exceeded
      return false;
    } //end brentsRootFinder


    private:
      FunctionTools(); //no definition to be supplied
     
      ~FunctionTools(); //no definition to be supplied
   }; //end FuntionTools class definition

}; //End namespace Isis

#endif