diff options
Diffstat (limited to 'gsl-1.9/multiroots/dogleg.c')
-rw-r--r-- | gsl-1.9/multiroots/dogleg.c | 413 |
1 files changed, 413 insertions, 0 deletions
diff --git a/gsl-1.9/multiroots/dogleg.c b/gsl-1.9/multiroots/dogleg.c new file mode 100644 index 0000000..4a120ef --- /dev/null +++ b/gsl-1.9/multiroots/dogleg.c @@ -0,0 +1,413 @@ +/* multiroots/dogleg.c + * + * Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough + * + * This program 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. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + */ + +#include "enorm.c" + +static void compute_diag (const gsl_matrix * J, gsl_vector * diag); +static void update_diag (const gsl_matrix * J, gsl_vector * diag); +static double compute_delta (gsl_vector * diag, gsl_vector * x); +static void compute_df (const gsl_vector * f_trial, const gsl_vector * f, gsl_vector * df); +static void compute_wv (const gsl_vector * qtdf, const gsl_vector *rdx, const gsl_vector *dx, const gsl_vector *diag, double pnorm, gsl_vector * w, gsl_vector * v); + +static double scaled_enorm (const gsl_vector * d, const gsl_vector * f); + +static double scaled_enorm (const gsl_vector * d, const gsl_vector * f) { + double e2 = 0 ; + size_t i, n = f->size ; + for (i = 0; i < n ; i++) { + double fi= gsl_vector_get(f, i); + double di= gsl_vector_get(d, i); + double u = di * fi; + e2 += u * u ; + } + return sqrt(e2); +} + +static double enorm_sum (const gsl_vector * a, const gsl_vector * b); + +static double enorm_sum (const gsl_vector * a, const gsl_vector * b) { + double e2 = 0 ; + size_t i, n = a->size ; + for (i = 0; i < n ; i++) { + double ai= gsl_vector_get(a, i); + double bi= gsl_vector_get(b, i); + double u = ai + bi; + e2 += u * u ; + } + return sqrt(e2); +} + +static void +compute_wv (const gsl_vector * qtdf, const gsl_vector *rdx, const gsl_vector *dx, const gsl_vector *diag, double pnorm, gsl_vector * w, gsl_vector * v) +{ + size_t i, n = qtdf->size; + + for (i = 0; i < n; i++) + { + double qtdfi = gsl_vector_get (qtdf, i); + double rdxi = gsl_vector_get (rdx, i); + double dxi = gsl_vector_get (dx, i); + double diagi = gsl_vector_get (diag, i); + + gsl_vector_set (w, i, (qtdfi - rdxi) / pnorm); + gsl_vector_set (v, i, diagi * diagi * dxi / pnorm); + } +} + + +static void +compute_df (const gsl_vector * f_trial, const gsl_vector * f, gsl_vector * df) +{ + size_t i, n = f->size; + + for (i = 0; i < n; i++) + { + double dfi = gsl_vector_get (f_trial, i) - gsl_vector_get (f, i); + gsl_vector_set (df, i, dfi); + } +} + +static void +compute_diag (const gsl_matrix * J, gsl_vector * diag) +{ + size_t i, j, n = diag->size; + + for (j = 0; j < n; j++) + { + double sum = 0; + for (i = 0; i < n; i++) + { + double Jij = gsl_matrix_get (J, i, j); + sum += Jij * Jij; + } + if (sum == 0) + sum = 1.0; + + gsl_vector_set (diag, j, sqrt (sum)); + } +} + +static void +update_diag (const gsl_matrix * J, gsl_vector * diag) +{ + size_t i, j, n = diag->size; + + for (j = 0; j < n; j++) + { + double cnorm, diagj, sum = 0; + for (i = 0; i < n; i++) + { + double Jij = gsl_matrix_get (J, i, j); + sum += Jij * Jij; + } + if (sum == 0) + sum = 1.0; + + cnorm = sqrt (sum); + diagj = gsl_vector_get (diag, j); + + if (cnorm > diagj) + gsl_vector_set (diag, j, cnorm); + } +} + +static double +compute_delta (gsl_vector * diag, gsl_vector * x) +{ + double Dx = scaled_enorm (diag, x); + double factor = 100; + + return (Dx > 0) ? factor * Dx : factor; +} + +static double +compute_actual_reduction (double fnorm, double fnorm1) +{ + double actred; + + if (fnorm1 < fnorm) + { + double u = fnorm1 / fnorm; + actred = 1 - u * u; + } + else + { + actred = -1; + } + + return actred; +} + +static double +compute_predicted_reduction (double fnorm, double fnorm1) +{ + double prered; + + if (fnorm1 < fnorm) + { + double u = fnorm1 / fnorm; + prered = 1 - u * u; + } + else + { + prered = 0; + } + + return prered; +} + +static void +compute_qtf (const gsl_matrix * q, const gsl_vector * f, gsl_vector * qtf) +{ + size_t i, j, N = f->size ; + + for (j = 0; j < N; j++) + { + double sum = 0; + for (i = 0; i < N; i++) + sum += gsl_matrix_get (q, i, j) * gsl_vector_get (f, i); + + gsl_vector_set (qtf, j, sum); + } +} + +static void +compute_rdx (const gsl_matrix * r, const gsl_vector * dx, gsl_vector * rdx) +{ + size_t i, j, N = dx->size ; + + for (i = 0; i < N; i++) + { + double sum = 0; + + for (j = i; j < N; j++) + { + sum += gsl_matrix_get (r, i, j) * gsl_vector_get (dx, j); + } + + gsl_vector_set (rdx, i, sum); + } +} + + +static void +compute_trial_step (gsl_vector *x, gsl_vector * dx, gsl_vector * x_trial) +{ + size_t i, N = x->size; + + for (i = 0; i < N; i++) + { + double pi = gsl_vector_get (dx, i); + double xi = gsl_vector_get (x, i); + gsl_vector_set (x_trial, i, xi + pi); + } +} + +static int +newton_direction (const gsl_matrix * r, const gsl_vector * qtf, gsl_vector * p) +{ + const size_t N = r->size2; + size_t i; + int status; + + status = gsl_linalg_R_solve (r, qtf, p); + +#ifdef DEBUG + printf("rsolve status = %d\n", status); +#endif + + for (i = 0; i < N; i++) + { + double pi = gsl_vector_get (p, i); + gsl_vector_set (p, i, -pi); + } + + return status; +} + +static void +gradient_direction (const gsl_matrix * r, const gsl_vector * qtf, + const gsl_vector * diag, gsl_vector * g) +{ + const size_t M = r->size1; + const size_t N = r->size2; + + size_t i, j; + + for (j = 0; j < M; j++) + { + double sum = 0; + double dj; + + for (i = 0; i < N; i++) + { + sum += gsl_matrix_get (r, i, j) * gsl_vector_get (qtf, i); + } + + dj = gsl_vector_get (diag, j); + gsl_vector_set (g, j, -sum / dj); + } +} + +static void +minimum_step (double gnorm, const gsl_vector * diag, gsl_vector * g) +{ + const size_t N = g->size; + size_t i; + + for (i = 0; i < N; i++) + { + double gi = gsl_vector_get (g, i); + double di = gsl_vector_get (diag, i); + gsl_vector_set (g, i, (gi / gnorm) / di); + } +} + +static void +compute_Rg (const gsl_matrix * r, const gsl_vector * gradient, gsl_vector * Rg) +{ + const size_t N = r->size2; + size_t i, j; + + for (i = 0; i < N; i++) + { + double sum = 0; + + for (j = i; j < N; j++) + { + double gj = gsl_vector_get (gradient, j); + double rij = gsl_matrix_get (r, i, j); + sum += rij * gj; + } + + gsl_vector_set (Rg, i, sum); + } +} + +static void +scaled_addition (double alpha, gsl_vector * newton, double beta, gsl_vector * gradient, gsl_vector * p) +{ + const size_t N = p->size; + size_t i; + + for (i = 0; i < N; i++) + { + double ni = gsl_vector_get (newton, i); + double gi = gsl_vector_get (gradient, i); + gsl_vector_set (p, i, alpha * ni + beta * gi); + } +} + +static int +dogleg (const gsl_matrix * r, const gsl_vector * qtf, + const gsl_vector * diag, double delta, + gsl_vector * newton, gsl_vector * gradient, gsl_vector * p) +{ + double qnorm, gnorm, sgnorm, bnorm, temp; + + newton_direction (r, qtf, newton); + +#ifdef DEBUG + printf("newton = "); gsl_vector_fprintf(stdout, newton, "%g"); printf("\n"); +#endif + + qnorm = scaled_enorm (diag, newton); + + if (qnorm <= delta) + { + gsl_vector_memcpy (p, newton); +#ifdef DEBUG + printf("took newton (qnorm = %g <= delta = %g)\n", qnorm, delta); +#endif + return GSL_SUCCESS; + } + + gradient_direction (r, qtf, diag, gradient); + +#ifdef DEBUG + printf("grad = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n"); +#endif + + gnorm = enorm (gradient); + + if (gnorm == 0) + { + double alpha = delta / qnorm; + double beta = 0; + scaled_addition (alpha, newton, beta, gradient, p); +#ifdef DEBUG + printf("took scaled newton because gnorm = 0\n"); +#endif + return GSL_SUCCESS; + } + + minimum_step (gnorm, diag, gradient); + + compute_Rg (r, gradient, p); /* Use p as temporary space to compute Rg */ + +#ifdef DEBUG + printf("mingrad = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n"); + printf("Rg = "); gsl_vector_fprintf(stdout, p, "%g"); printf("\n"); +#endif + + temp = enorm (p); + sgnorm = (gnorm / temp) / temp; + + if (sgnorm > delta) + { + double alpha = 0; + double beta = delta; + scaled_addition (alpha, newton, beta, gradient, p); +#ifdef DEBUG + printf("took gradient\n"); +#endif + return GSL_SUCCESS; + } + + bnorm = enorm (qtf); + + { + double bg = bnorm / gnorm; + double bq = bnorm / qnorm; + double dq = delta / qnorm; + double dq2 = dq * dq; + double sd = sgnorm / delta; + double sd2 = sd * sd; + + double t1 = bg * bq * sd; + double u = t1 - dq; + double t2 = t1 - dq * sd2 + sqrt (u * u + (1-dq2) * (1 - sd2)); + + double alpha = dq * (1 - sd2) / t2; + double beta = (1 - alpha) * sgnorm; + +#ifdef DEBUG + printf("bnorm = %g\n", bnorm); + printf("gnorm = %g\n", gnorm); + printf("qnorm = %g\n", qnorm); + printf("delta = %g\n", delta); + printf("alpha = %g beta = %g\n", alpha, beta); + printf("took scaled combination of newton and gradient\n"); +#endif + + scaled_addition (alpha, newton, beta, gradient, p); + } + + return GSL_SUCCESS; +} |