htswanalysis/MACS/lib/gsl/gsl-1.11/multiroots/dogleg.c

   1 /* multiroots/dogleg.c
   2  *
   3  * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Brian Gough
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation; either version 3 of the License, or (at
   8  * your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful, but
  11  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13  * General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this program; if not, write to the Free Software
  17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
  18  */
  19
  20 #include "enorm.c"
  21
  22 static void compute_diag (const gsl_matrix * J, gsl_vector * diag);
  23 static void update_diag (const gsl_matrix * J, gsl_vector * diag);
  24 static double compute_delta (gsl_vector * diag, gsl_vector * x);
  25 static void compute_df (const gsl_vector * f_trial, const gsl_vector * f, gsl_vector * df);
  26 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);
  27
  28 static double scaled_enorm (const gsl_vector * d, const gsl_vector * f);
  29
  30 static double scaled_enorm (const gsl_vector * d, const gsl_vector * f) {
  31   double e2 = 0 ;
  32   size_t i, n = f->size ;
  33   for (i = 0; i < n ; i++) {
  34     double fi= gsl_vector_get(f, i);
  35     double di= gsl_vector_get(d, i);
  36     double u = di * fi;
  37     e2 += u * u ;
  38   }
  39   return sqrt(e2);
  40 }
  41
  42 static double enorm_sum (const gsl_vector * a, const gsl_vector * b);
  43
  44 static double enorm_sum (const gsl_vector * a, const gsl_vector * b) {
  45   double e2 = 0 ;
  46   size_t i, n = a->size ;
  47   for (i = 0; i < n ; i++) {
  48     double ai= gsl_vector_get(a, i);
  49     double bi= gsl_vector_get(b, i);
  50     double u = ai + bi;
  51     e2 += u * u ;
  52   }
  53   return sqrt(e2);
  54 }
  55
  56 static void
  57 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)
  58 {
  59   size_t i, n = qtdf->size;
  60
  61   for (i = 0; i < n; i++)
  62     {
  63       double qtdfi = gsl_vector_get (qtdf, i);
  64       double rdxi = gsl_vector_get (rdx, i);
  65       double dxi = gsl_vector_get (dx, i);
  66       double diagi = gsl_vector_get (diag, i);
  67
  68       gsl_vector_set (w, i, (qtdfi - rdxi) / pnorm);
  69       gsl_vector_set (v, i, diagi * diagi * dxi / pnorm);
  70     }
  71 }
  72
  73
  74 static void
  75 compute_df (const gsl_vector * f_trial, const gsl_vector * f, gsl_vector * df)
  76 {
  77   size_t i, n = f->size;
  78
  79   for (i = 0; i < n; i++)
  80     {
  81       double dfi = gsl_vector_get (f_trial, i) - gsl_vector_get (f, i);
  82       gsl_vector_set (df, i, dfi);
  83     }
  84 }
  85
  86 static void
  87 compute_diag (const gsl_matrix * J, gsl_vector * diag)
  88 {
  89   size_t i, j, n = diag->size;
  90
  91   for (j = 0; j < n; j++)
  92     {
  93       double sum = 0;
  94       for (i = 0; i < n; i++)
  95         {
  96           double Jij = gsl_matrix_get (J, i, j);
  97           sum += Jij * Jij;
  98         }
  99       if (sum == 0)
 100         sum = 1.0;
 101
 102       gsl_vector_set (diag, j, sqrt (sum));
 103     }
 104 }
 105
 106 static void
 107 update_diag (const gsl_matrix * J, gsl_vector * diag)
 108 {
 109   size_t i, j, n = diag->size;
 110
 111   for (j = 0; j < n; j++)
 112     {
 113       double cnorm, diagj, sum = 0;
 114       for (i = 0; i < n; i++)
 115         {
 116           double Jij = gsl_matrix_get (J, i, j);
 117           sum += Jij * Jij;
 118         }
 119       if (sum == 0)
 120         sum = 1.0;
 121
 122       cnorm = sqrt (sum);
 123       diagj = gsl_vector_get (diag, j);
 124
 125       if (cnorm > diagj)
 126         gsl_vector_set (diag, j, cnorm);
 127     }
 128 }
 129
 130 static double
 131 compute_delta (gsl_vector * diag, gsl_vector * x)
 132 {
 133   double Dx = scaled_enorm (diag, x);
 134   double factor = 100;
 135
 136   return (Dx > 0) ? factor * Dx : factor;
 137 }
 138
 139 static double
 140 compute_actual_reduction (double fnorm, double fnorm1)
 141 {
 142   double actred;
 143
 144   if (fnorm1 < fnorm)
 145     {
 146       double u = fnorm1 / fnorm;
 147       actred = 1 - u * u;
 148     }
 149   else
 150     {
 151       actred = -1;
 152     }
 153
 154   return actred;
 155 }
 156
 157 static double
 158 compute_predicted_reduction (double fnorm, double fnorm1)
 159 {
 160   double prered;
 161
 162   if (fnorm1 < fnorm)
 163     {
 164       double u = fnorm1 / fnorm;
 165       prered = 1 - u * u;
 166     }
 167   else
 168     {
 169       prered = 0;
 170     }
 171
 172   return prered;
 173 }
 174
 175 static void
 176 compute_qtf (const gsl_matrix * q, const gsl_vector * f, gsl_vector * qtf)
 177 {
 178   size_t i, j, N = f->size ;
 179
 180   for (j = 0; j < N; j++)
 181     {
 182       double sum = 0;
 183       for (i = 0; i < N; i++)
 184         sum += gsl_matrix_get (q, i, j) * gsl_vector_get (f, i);
 185
 186       gsl_vector_set (qtf, j, sum);
 187     }
 188 }
 189
 190 static void
 191 compute_rdx (const gsl_matrix * r, const gsl_vector * dx, gsl_vector * rdx)
 192 {
 193   size_t i, j, N = dx->size ;
 194
 195   for (i = 0; i < N; i++)
 196     {
 197       double sum = 0;
 198
 199       for (j = i; j < N; j++)
 200         {
 201           sum += gsl_matrix_get (r, i, j) * gsl_vector_get (dx, j);
 202         }
 203
 204       gsl_vector_set (rdx, i, sum);
 205     }
 206 }
 207
 208
 209 static void
 210 compute_trial_step (gsl_vector *x, gsl_vector * dx, gsl_vector * x_trial)
 211 {
 212   size_t i, N = x->size;
 213
 214   for (i = 0; i < N; i++)
 215     {
 216       double pi = gsl_vector_get (dx, i);
 217       double xi = gsl_vector_get (x, i);
 218       gsl_vector_set (x_trial, i, xi + pi);
 219     }
 220 }
 221
 222 static int
 223 newton_direction (const gsl_matrix * r, const gsl_vector * qtf, gsl_vector * p)
 224 {
 225   const size_t N = r->size2;
 226   size_t i;
 227   int status;
 228
 229   status = gsl_linalg_R_solve (r, qtf, p);
 230
 231 #ifdef DEBUG
 232   printf("rsolve status = %d\n", status);
 233 #endif
 234
 235   for (i = 0; i < N; i++)
 236     {
 237       double pi = gsl_vector_get (p, i);
 238       gsl_vector_set (p, i, -pi);
 239     }
 240
 241   return status;
 242 }
 243
 244 static void
 245 gradient_direction (const gsl_matrix * r, const gsl_vector * qtf,
 246                     const gsl_vector * diag, gsl_vector * g)
 247 {
 248   const size_t M = r->size1;
 249   const size_t N = r->size2;
 250
 251   size_t i, j;
 252
 253   for (j = 0; j < M; j++)
 254     {
 255       double sum = 0;
 256       double dj;
 257
 258       for (i = 0; i < N; i++)
 259         {
 260           sum += gsl_matrix_get (r, i, j) * gsl_vector_get (qtf, i);
 261         }
 262
 263       dj = gsl_vector_get (diag, j);
 264       gsl_vector_set (g, j, -sum / dj);
 265     }
 266 }
 267
 268 static void
 269 minimum_step (double gnorm, const gsl_vector * diag, gsl_vector * g)
 270 {
 271   const size_t N = g->size;
 272   size_t i;
 273
 274   for (i = 0; i < N; i++)
 275     {
 276       double gi = gsl_vector_get (g, i);
 277       double di = gsl_vector_get (diag, i);
 278       gsl_vector_set (g, i, (gi / gnorm) / di);
 279     }
 280 }
 281
 282 static void
 283 compute_Rg (const gsl_matrix * r, const gsl_vector * gradient, gsl_vector * Rg)
 284 {
 285   const size_t N = r->size2;
 286   size_t i, j;
 287
 288   for (i = 0; i < N; i++)
 289     {
 290       double sum = 0;
 291
 292       for (j = i; j < N; j++)
 293         {
 294           double gj = gsl_vector_get (gradient, j);
 295           double rij = gsl_matrix_get (r, i, j);
 296           sum += rij * gj;
 297         }
 298
 299       gsl_vector_set (Rg, i, sum);
 300     }
 301 }
 302
 303 static void
 304 scaled_addition (double alpha, gsl_vector * newton, double beta, gsl_vector * gradient, gsl_vector * p)
 305 {
 306   const size_t N = p->size;
 307   size_t i;
 308
 309   for (i = 0; i < N; i++)
 310     {
 311       double ni = gsl_vector_get (newton, i);
 312       double gi = gsl_vector_get (gradient, i);
 313       gsl_vector_set (p, i, alpha * ni + beta * gi);
 314     }
 315 }
 316
 317 static int
 318 dogleg (const gsl_matrix * r, const gsl_vector * qtf,
 319         const gsl_vector * diag, double delta,
 320         gsl_vector * newton, gsl_vector * gradient, gsl_vector * p)
 321 {
 322   double qnorm, gnorm, sgnorm, bnorm, temp;
 323
 324   newton_direction (r, qtf, newton);
 325
 326 #ifdef DEBUG
 327   printf("newton = "); gsl_vector_fprintf(stdout, newton, "%g"); printf("\n");
 328 #endif
 329
 330   qnorm = scaled_enorm (diag, newton);
 331
 332   if (qnorm <= delta)
 333     {
 334       gsl_vector_memcpy (p, newton);
 335 #ifdef DEBUG
 336       printf("took newton (qnorm = %g  <=   delta = %g)\n", qnorm, delta);
 337 #endif
 338       return GSL_SUCCESS;
 339     }
 340
 341   gradient_direction (r, qtf, diag, gradient);
 342
 343 #ifdef DEBUG
 344   printf("grad = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n");
 345 #endif
 346
 347   gnorm = enorm (gradient);
 348
 349   if (gnorm == 0)
 350     {
 351       double alpha = delta / qnorm;
 352       double beta = 0;
 353       scaled_addition (alpha, newton, beta, gradient, p);
 354 #ifdef DEBUG
 355       printf("took scaled newton because gnorm = 0\n");
 356 #endif
 357       return GSL_SUCCESS;
 358     }
 359
 360   minimum_step (gnorm, diag, gradient);
 361
 362   compute_Rg (r, gradient, p);  /* Use p as temporary space to compute Rg */
 363
 364 #ifdef DEBUG
 365   printf("mingrad = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n");
 366   printf("Rg = "); gsl_vector_fprintf(stdout, p, "%g"); printf("\n");
 367 #endif
 368
 369   temp = enorm (p);
 370   sgnorm = (gnorm / temp) / temp;
 371
 372   if (sgnorm > delta)
 373     {
 374       double alpha = 0;
 375       double beta = delta;
 376       scaled_addition (alpha, newton, beta, gradient, p);
 377 #ifdef DEBUG
 378       printf("took gradient\n");
 379 #endif
 380       return GSL_SUCCESS;
 381     }
 382
 383   bnorm = enorm (qtf);
 384
 385   {
 386     double bg = bnorm / gnorm;
 387     double bq = bnorm / qnorm;
 388     double dq = delta / qnorm;
 389     double dq2 = dq * dq;
 390     double sd = sgnorm / delta;
 391     double sd2 = sd * sd;
 392
 393     double t1 = bg * bq * sd;
 394     double u = t1 - dq;
 395     double t2 = t1 - dq * sd2 + sqrt (u * u + (1-dq2) * (1 - sd2));
 396
 397     double alpha = dq * (1 - sd2) / t2;
 398     double beta = (1 - alpha) * sgnorm;
 399
 400 #ifdef DEBUG
 401     printf("bnorm = %g\n", bnorm);
 402     printf("gnorm = %g\n", gnorm);
 403     printf("qnorm = %g\n", qnorm);
 404     printf("delta = %g\n", delta);
 405     printf("alpha = %g   beta = %g\n", alpha, beta);
 406     printf("took scaled combination of newton and gradient\n");
 407 #endif
 408
 409     scaled_addition (alpha, newton, beta, gradient, p);
 410   }
 411
 412   return GSL_SUCCESS;
 413 }