findroot.cpp

Go to the documentation of this file.

/*
    Sheep - A Rigid Body Dynamics Engine
    Copyright (C) 2001-2004 Francois Beaune
    Contact: http://toxicengine.sourceforge.net/

    This file is part of Sheep.

    Sheep is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    Sheep is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Sheep; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "findroot.h"   // include first

#include <cassert>
#include <cmath>    // std::fabs()

using namespace sheep;

Real sheep::FindRoot(const Function1 &f, Real left, Real right, Real tolerance) {
    assert(tolerance >= 0);

    if(left > right) {
        Real dummy = left;
        left = right;
        right = dummy;
    }

    Real fl = f.EvaluateAt(left);
    Real fr = f.EvaluateAt(right);

    if(fabs(fl) < tolerance)
        return fl;
    else if(fabs(fr) < tolerance)
        return fr;

    assert(fl * fr < 0);

    int sign = fr > 0 ? 1 : -1;

    while(right - left > tolerance) {
        Real mid = (left + right) / 2;
        Real fm = f.EvaluateAt(mid);

        if(sign * fm > 0)
            right = mid;
        else left = mid;
    }

    return (left + right) / 2;
}

//  A zero of the function f(x) is computed in the interval left, right.
//
//  Input.
//
//  left        left endpoint of initial interval
//  right       right endpoint of initial interval
//  f           function subprogram which evaluates f(x) for any x in
//              the interval left, right
//  tolerance   desired length of the interval of uncertainty of the
//              final result (>= 0.0)
//
//  Output.
//
//  zeroin abcissa approximating a zero of f in the interval left, right.
//
//
//  It is assumed  that   f(left)   and   f(right)   have  opposite  signs
//  without  a  check.  zeroin  returns a zero x in the given interval
//  left, right to within a tolerance 4 * macheps * abs(x) + tolerance,
//  where macheps is the relative machine precision.
//
//  This function subprogram is a slightly  modified  translation of
//  the algol 60 procedure  zero  given in  richard brent, algorithms for
//  minimization without derivatives, prentice - hall, inc. (1973).

/*
Real FindRoot(const Function1 &f, Real left, Real right, Real tolerance) {
    assert(tolerance >= 0);

    // Compute eps, the relative machine precision.
    //!\todo Move that part out of this function.

    Real eps = 1;
    Real tol1;

    do {
        eps /= 2;
        tol1 = 1 + eps;
    } while(tol1 > 1);

    // Initialization.

    Real a = left;
    Real b = right;
    Real fa = f.EvaluateAt(a);
    Real fb = f.EvaluateAt(b);

    //!\todo Throw an exception instead.
    assert(fa * fb < 0);

    // Begin step.

    Real c = a;
    Real fc = fa;
    Real d = b - a;
    Real e = d;

    while(1) {
        if(fabs(fc) < fabs(fb)) {
            a = b;
            b = c;
            c = a;

            fa = fb;
            fb = fc;
            fc = fa;
        }

        // Convergence test.

        Real mid = 0.5 * (c - b);
        Real tol = 2 * eps * fabs(b) + 0.5 * tolerance;

        if(fabs(mid) <= tol)
            break;

        if(fb == 0)
            break;

        if(fabs(e) < tol || fabs(fa) <= fabs(fb)) { // is bisection necessary?
            // Bisection.
            e = d = mid;
        } else {
            Real p, q, r, s;

            if(a != c) {    // is quadratic interpolation possible?
                // Inverse quadratic interpolation.
                q = fa / fc;
                r = fb / fc;
                s = fb / fa;
                p = s * (2 * mid * q * (q - r) - (b - a) * (r - 1));
                q = (q - 1) * (r - 1) * (s - 1);
            } else {
                // Linear interpolation.
                s = fb / fa;
                p = 2 * mid * s;
                q = 1 - s;
            }

            // Adjust signs.

            if(p > 0)
                q = -q;

            p = fabs(p);

            // Is interpolation acceptable?

            if(2 * p >= 3 * mid * q - fabs(tol * q) || p >= fabs(0.5 * e * q)) {
                // Bisection.
                e = d = mid;
            } else {
                e = d;
                d = p / q;
            }
        }

        // Complete step.

        a = b;
        fa = fb;

        if(fabs(d) > tol)
            b += d;
        else b += fabs(tol) * (mid >= 0 ? 1 : -1);

        fb = f.EvaluateAt(b);

        if(fb * (fc / fabs(fc)) > 0) {
            c = a;
            fc = fa;
            e = d = b - a;
        }
    }

    return b;
}
*/

---