b2nonlinear_system_solver.H

//------------------------------------------------------------------------
// b2simple_nonlinear_system_solver.H --
//
//
// written by Mathias Doreille
//
// Copyright (c) 2009 SMR Engineering & Development SA
//               2502 Bienne, Switzerland
//
// All Rights Reserved.  Proprietary source code.  The contents of
// this file may not be disclosed to third parties, copied or
// duplicated in any form, in whole or in part, without the prior
// written permission of SMR.
//------------------------------------------------------------------------
 
#ifndef B2SIMPLE_NONLINEAR_SYSTEM_SOLVER_H_
#define B2SIMPLE_NONLINEAR_SYSTEM_SOLVER_H_
 
#include <cmath>
#include <iostream>
#include <limits>
 
#include "b2blaslapack.H"
#include "b2line_search.H"
#include "b2logging.H"
#include "linear_algebra/b2linear_algebra_utils.H"
 
namespace b2000 {
 
/* Solve a nonlinear system using Newton-Raphson method
 *
 * struct SYSTEM {
 *   typename value_type;
 *   static const int size;
 *   void value(const value_type[size] x, value_type[size] y, value_type[size * size] jacobien);
 * };
 */
template <typename SYSTEM>
bool solve_newton_raphson(
      SYSTEM& system, typename SYSTEM::value_type x[SYSTEM::size], double tol = 1e-5,
      int maxiter = 50) {
    typename SYSTEM::value_type y[SYSTEM::size];
    int ipiv[SYSTEM::size];
    typename SYSTEM::value_type jacobien[SYSTEM::size * SYSTEM::size];
    int info;
    tol = tol * tol;
    while (--maxiter) {
        system.value(x, y, jacobien);
        double norm = 0;
        for (int i = 0; i != SYSTEM::size; ++i) { norm += y[i] * y[i]; }
        if (norm < tol) { return true; }
        lapack::gesv(SYSTEM::size, 1, jacobien, SYSTEM::size, ipiv, y, SYSTEM::size, info);
        if (info) { return false; }
        for (int i = 0; i != SYSTEM::size; ++i) { x[i] -= y[i]; }
    }
    return false;
}
 
/* Line search using the Wolfe-Powell Rule using the transformed problem
 * f = 0.5 * g * g
 * g: R^n -> R^n
 * f: R^n -> R
 */
template <typename g_t>
struct f_line_search_t {
    typedef typename g_t::x_type x_t;
    typedef typename g_t::r_type g_x_t;
 
    f_line_search_t(g_t& g_, x_t& xv_, x_t& dv_) : g(g_), xv(xv_), dv(dv_) {}
 
    double search(double rho, double sigma, double min_f, unsigned long max_iter) {
        double der0;
        double f0 = (*this)(0, der0);
        return line_search<f_line_search_t>(*this, f0, der0, rho, sigma, min_f, max_iter);
 
        /*
         double xmin;
         double rmin = std::numeric_limits<double>::max();
         for (double x = 1; x >= -1; x -= 0.001) {
         double r = (*this)(x);
         if (r < rmin) {
         rmin = r;
         xmin = x;
         }
         }
         std::cout << "x min = " << xmin << std::endl;
         return xmin;
         */
    }
 
    double operator()(double x) const {
        x_t v = xv;
        v += x * dv;
        g_x_t rv;
        g(v, rv);
        return 0.5 * (rv * rv);
    }
 
    double operator()(double x, double& der) const {
        x_t v = xv;
        if (x != 0) { v += x * dv; }
        g_x_t rv, drv;
        g(v, rv, dv, drv);
        der = rv * drv;
        // std::cout << "  " << x << " " << 0.5 * (rv * rv) << " " << der
        //    << std::endl;
        return 0.5 * (rv * rv);
    }
    g_t& g;
    x_t& xv;
    x_t& dv;
};
 
class Sha1NonlinearSolver {
public:
    Sha1NonlinearSolver(
          logging::Logger& logger_ = logging::get_logger("solver.nonlinear_system.sha1"))
        : logger(logger_) {
        // termination test
        conv_epsilon = 1e-6;
 
        // Sha parameter
        alpha = 0.9;
        epsilon = 0.1;
 
        // Wolf-Powell rule
        wp_rho = 0.001;
        wp_sigma = 0.9;
        max_iter_line_search = 10;
    }
 
    template <typename g_t, typename x_t>
    bool solve(g_t& g, x_t& xk) {
        typedef typename g_t::r_type g_x_t;
 
        g_x_t g_xk;
        g_x_t g_xk1;
        x_t d1;
        x_t d2;
        x_t xk1;
        g(xk, g_xk, true);
        double f_xk = 0.5 * g_xk * g_xk;
        for (int iter = 0;; ++iter) {
            {
                double norm_g_xk = std::sqrt(2 * f_xk);
                logger << logging::debug << "iteration " << iter + 1 << ", norm = " << norm_g_xk
                       << ", termination = " << conv_epsilon << logging::LOGFLUSH;
                if (norm_g_xk < conv_epsilon) { break; }
            }
            bool inv_h = g.nd(xk, g_xk, d1);
            if (inv_h) {
                xk1 = xk;
                xk1 += d1;
                g(xk1, g_xk1, true);
                double f_xk1 = 0.5 * (g_xk1 * g_xk1);
                if (f_xk1 <= alpha * f_xk) {
                    xk = xk1;
                    g_xk = g_xk1;
                    f_xk = f_xk1;
                    continue;
                }
            }
 
            logger << logging::debug << "need line search procedure" << logging::LOGFLUSH;
            g.gd(xk, g_xk, d2);
            if (inv_h) {
                double d1d1 = d1 * d1;
                double d2d2 = d2 * d2;
                double d1d2 = d1 * d2;
                if (d1d2 < epsilon * std::sqrt(d1d1 * d2d2)) {
                    double beta = get_beta(d1d1, d1d2, d2d2);
                    // std::cout << "beta=" << beta << std::endl;
                    d1 *= (1 - beta);
                    d1 += beta * d2;
                }
            } else {
                d1 = d2;
            }
            double lambda =
                  f_line_search_t<g_t>(g, xk, d1).search(wp_rho, wp_sigma, 0, max_iter_line_search);
            if (lambda == 0) { return false; }
            xk += lambda * d1;
            g(xk, g_xk, true);
            f_xk = 0.5 * g_xk * g_xk;
        }
        return true;
    }
 
protected:
    double get_beta(double d1d1, double d1d2, double d2d2) {
        const double a[2] = {d2d2 - d1d2, d1d2};
        const double b[3] = {
              d2d2 * (d2d2 - 2 * d1d2 + d1d1), d2d2 * 2 * (d1d2 - d1d1), d2d2 * d1d1};
        double r = 0;
        while (r < 1 && (a[0] * r + a[1]) / std::sqrt(b[0] * r * r + b[1] * r + b[2]) < epsilon) {
            r += 0.001;
        }
 
        return std::min(r, 1.0);
    }
 
    logging::Logger logger;
    double conv_epsilon;
    double alpha;
    double epsilon;
    double wp_rho;
    double wp_sigma;
    int max_iter_line_search;
};
 
class Sha2NonlinearSolver : public Sha1NonlinearSolver {
public:
    Sha2NonlinearSolver(
          int _n, logging::Logger& logger_ = logging::get_logger("solver.nonlinear_system.sha2"))
        : Sha1NonlinearSolver(logger_) {
        n = _n;
        nu = 0.1;
    }
 
    template <typename g_t, typename x_t>
    bool solve(g_t& g, x_t& xk) {
        typedef typename g_t::r_type g_x_t;
 
        g_x_t g_xk;
        g_x_t g_xk1;
        x_t d1;
        x_t d2;
        x_t xk1;
        int flag = 0;
        double omega = 0;
        std::vector<double> f_xl(n, std::numeric_limits<double>::max());
        g(xk, g_xk, true);
        double f_xk = 0.5 * g_xk * g_xk;
        for (int iter = 0;; ++iter) {
            {
                {
                    size_t s = g_xk.size();
                    b2linalg::Interval a(0, s / 2);
                    b2linalg::Interval b(s / 2, s - 1);
                    std::cout << "rrrrr " << g_xk(b2linalg::Interval(0, s / 2)).norm2() << " "
                              << g_xk(b2linalg::Interval(s / 2, s - 1)).norm2() << " "
                              << g_xk[s - 1] << std::endl;
                }
                double norm_g_xk = std::sqrt(2 * f_xk);
                logger << logging::debug << "iteration " << iter + 1 << ", norm = " << norm_g_xk
                       << ", termination = " << conv_epsilon << logging::LOGFLUSH;
                if (norm_g_xk < conv_epsilon) { break; }
            }
            double f_min = f_xk;
            for (size_t i = n - 1; i > 0; --i) {
                f_xl[i] = f_xl[i - 1];
                f_min = std::min(f_xl[i], f_min);
            }
            f_xl[0] = f_xk;
            bool inv_h = g.nd(xk, g_xk, d1);
            if (inv_h) {
                xk1 = xk;
                xk1 += d1;
                g(xk1, g_xk1, true);
                double f_xk1 = 0.5 * (g_xk1 * g_xk1);
                if (f_xk1 <= alpha * (omega == 0 ? f_min : f_xk)) {
                    xk = xk1;
                    g_xk = g_xk1;
                    f_xk = f_xk1;
                    flag = 0;
                    if (f_xk <= alpha * omega) { omega = 0; }
                    continue;
                }
                if (omega == 0 && flag < n && f_xk1 <= (1 + nu) * f_xk) {
                    xk = xk1;
                    g_xk = g_xk1;
                    f_xk = f_xk1;
                    ++flag;
                    continue;
                }
            }
 
            logger << logging::debug << "need line search procedure" << logging::LOGFLUSH;
            if (omega == 0) {
                if (flag == n) {
                    omega = f_min;
                } else if (flag > 0) {
                    ++flag;
                }
            }
 
            g.gd(xk, g_xk, d2);
            if (inv_h) {
                double d1d1 = d1 * d1;
                double d2d2 = d2 * d2;
                double d1d2 = d1 * d2;
                if (d1d2 < epsilon * std::sqrt(d1d1 * d2d2)) {
                    double beta = get_beta(d1d1, d1d2, d2d2);
                    beta = 0;
                    // std::cout << "beta=" << beta << std::endl;
                    d1 *= (1 - beta);
                    d1 += beta * d2;
                }
            } else {
                d1 = d2;
            }
            double lambda =
                  f_line_search_t<g_t>(g, xk, d1).search(wp_rho, wp_sigma, 0, max_iter_line_search);
            std::cout << "lambda = " << lambda << std::endl;
            if (lambda == 0) { lambda = 0.1; }
            xk += lambda * d1;
            g(xk, g_xk, true);
            f_xk = 0.5 * g_xk * g_xk;
        }
        return true;
    }
 
protected:
    int n;
    double nu;
};
 
class AlaNonlinearSolver {
public:
    AlaNonlinearSolver() {
        epsilon = 1e-6;
        delta0 = 0.001;
        Delta0 = 1;
        nu = 0.99;
        rho = 0.001;
        sigma = 0.9;
        gamma1 = gamma2 = 10000;
        max_iter_line_search = 10;
        beta1 = 0.01;
        beta2 = 1 / beta1;
        beta3 = 1.1;
        tau = 1e-10;
        Tau = 1e10;
        norm_g_xk_1 = -1;
    }
 
    template <typename g_t, typename x_t>
    bool next(const g_t& g, int k, x_t& xk) {
        // typedef typename g_t::x_type x_t;
        typedef typename g_t::r_type g_x_t;
 
        double delta = delta0;
        double Delta = Delta0;
        double alphak;
 
        g_x_t d1;
        g_x_t d2;
        x_t xk_sk;
        g_x_t sk;
 
        // l1_and_l2:
        double norm_g_xk;
        const bool has_h = g(xk, d2, norm_g_xk, epsilon, d1);
        std::cout << k << " " << xk[0] << " " << norm_g_xk << std::endl;
        if (norm_g_xk < epsilon) { return true; }
 
        double f_xk = 0.5 * norm_g_xk * norm_g_xk;
        const double f_xk_1 = norm_g_xk_1 < 0 ? f_xk : 0.5 * norm_g_xk_1 * norm_g_xk_1;
 
        g_x_t d_xi;
 
        // l3:
        if (!has_h) { goto l8; }
 
        // l4:
        if (std::abs(f_xk - f_xk_1) > gamma1 && norm_g_xk > gamma2) {
            delta = beta2 * delta0;
            goto l7;
        }
 
        // l5:
        if (k == 1 || norm_g_xk < norm_g_xk_1) {
            x_t xbar;
            xbar = xk + d1;
            // l6:
            g_x_t g_xbar;
            g(xbar, g_xbar);
            double norm_g_xbar = g_xbar.norm2();
            double f_xbar = 0.5 * g_xbar * g_xbar;
            if (f_xbar < f_xk && norm_g_xbar <= nu * norm_g_xk) { delta = beta1 * delta0; }
        }
 
    l7:
        if (d1 * d2 >= 0) { goto l9; }
 
    l8:
        alphak = f_line_search_t<g_t>(g, xk, d2).search(rho, sigma, 0, max_iter_line_search);
        sk = alphak * d2;
        goto l12;
 
    l9 : {
        double xi = 1 / (Delta + std::abs(f_xk - f_xk_1));
        d_xi = (1 - xi) * d2;
        d_xi += xi * d1;
 
        // l10:
        if (d_xi * d2 < delta * d_xi.norm2() * d2.norm2()) {
            Delta = beta3 * Delta;
            goto l9;
        }
 
        // l11:
#define USEA
#ifdef USEA
        alphak = f_line_search_t<g_t>(g, xk, d2).search(rho, sigma, 0, max_iter_line_search);
        if (alphak * d2.norm2() < Tau * d1.norm2()) {
            sk = (alphak * (1 - xi)) * d2 + xi * d1;
            xk_sk = xk + sk;
            g_x_t g_xk_sk;
            g(xk_sk, g_xk_sk);
            double norm_g_xk_sk = g_xk_sk.norm2();
            if (0.5 * norm_g_xk_sk * norm_g_xk_sk <= f_xk - tau * sk.norm2()) {
                xk = xk_sk;
                return false;
            }
        }
#else
        alphak = f_line_search_t<g_t>(g, xk, d_xi).search(rho, sigma, 0, max_iter_line_search);
        xk += alphak * d_xi;
        return false;
#endif
    }
 
    l12:
        xk += alphak * d2;
 
        return false;
    }
 
    template <typename g_t, typename x_t>
    void solve(const g_t& g, x_t& x0) {
        for (int k = 1; !next(g, k, x0); ++k) { ; }
    }
 
    double epsilon;
    double delta0;
    double Delta0;
    double nu;
    double rho;
    double sigma;
    int max_iter_line_search;
    double gamma1;
    double gamma2;
    double beta1;
    double beta2;
    double beta3;
    double tau;
    double Tau;
    double norm_g_xk_1;
};
 
}  // namespace b2000
 
#endif /* B2SIMPLE_NONLINEAR_SYSTEM_SOLVER_H_ */