/* 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., 675 Mass Ave, Cambridge, MA 02139, 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 voidcompute_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 voidcompute_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 voidcompute_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 voidupdate_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 doublecompute_delta (gsl_vector * diag, gsl_vector * x){  double Dx = scaled_enorm (diag, x);  double factor = 100;  return (Dx > 0) ? factor * Dx : factor;}static doublecompute_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 doublecompute_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 voidcompute_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 intnewton_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 voidgradient_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 voidminimum_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 voidcompute_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 voidscaled_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 intdogleg (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;}