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

   1 /* multiroots/broyden.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 <config.h>
  21
  22 #include <stddef.h>
  23 #include <stdlib.h>
  24 #include <stdio.h>
  25 #include <math.h>
  26 #include <float.h>
  27
  28 #include <gsl/gsl_math.h>
  29 #include <gsl/gsl_errno.h>
  30 #include <gsl/gsl_multiroots.h>
  31 #include <gsl/gsl_linalg.h>
  32
  33 #include "enorm.c"
  34
  35 /* Broyden's method. It is not an efficient or modern algorithm but
  36    gives an example of a rank-1 update.
  37
  38    C.G. Broyden, "A Class of Methods for Solving Nonlinear
  39    Simultaneous Equations", Mathematics of Computation, vol 19 (1965),
  40    p 577-593
  41
  42  */
  43
  44 typedef struct
  45   {
  46     gsl_matrix *H;
  47     gsl_matrix *lu;
  48     gsl_permutation *permutation;
  49     gsl_vector *v;
  50     gsl_vector *w;
  51     gsl_vector *y;
  52     gsl_vector *p;
  53     gsl_vector *fnew;
  54     gsl_vector *x_trial;
  55     double phi;
  56   }
  57 broyden_state_t;
  58
  59 static int broyden_alloc (void *vstate, size_t n);
  60 static int broyden_set (void *vstate, gsl_multiroot_function * function, gsl_vector * x, gsl_vector * f, gsl_vector * dx);
  61 static int broyden_iterate (void *vstate, gsl_multiroot_function * function, gsl_vector * x, gsl_vector * f, gsl_vector * dx);
  62 static void broyden_free (void *vstate);
  63
  64
  65 static int
  66 broyden_alloc (void *vstate, size_t n)
  67 {
  68   broyden_state_t *state = (broyden_state_t *) vstate;
  69   gsl_vector *v, *w, *y, *fnew, *x_trial, *p;
  70   gsl_permutation *perm;
  71   gsl_matrix *m, *H;
  72
  73   m = gsl_matrix_calloc (n, n);
  74
  75   if (m == 0)
  76     {
  77       GSL_ERROR ("failed to allocate space for lu", GSL_ENOMEM);
  78     }
  79
  80   state->lu = m;
  81
  82   perm = gsl_permutation_calloc (n);
  83
  84   if (perm == 0)
  85     {
  86       gsl_matrix_free (m);
  87
  88       GSL_ERROR ("failed to allocate space for permutation", GSL_ENOMEM);
  89     }
  90
  91   state->permutation = perm;
  92
  93   H = gsl_matrix_calloc (n, n);
  94
  95   if (H == 0)
  96     {
  97       gsl_permutation_free (perm);
  98       gsl_matrix_free (m);
  99
 100       GSL_ERROR ("failed to allocate space for d", GSL_ENOMEM);
 101     }
 102
 103   state->H = H;
 104
 105   v = gsl_vector_calloc (n);
 106
 107   if (v == 0)
 108     {
 109       gsl_matrix_free (H);
 110       gsl_permutation_free (perm);
 111       gsl_matrix_free (m);
 112
 113       GSL_ERROR ("failed to allocate space for v", GSL_ENOMEM);
 114     }
 115
 116   state->v = v;
 117
 118   w = gsl_vector_calloc (n);
 119
 120   if (w == 0)
 121     {
 122       gsl_vector_free (v);
 123       gsl_matrix_free (H);
 124       gsl_permutation_free (perm);
 125       gsl_matrix_free (m);
 126
 127       GSL_ERROR ("failed to allocate space for w", GSL_ENOMEM);
 128     }
 129
 130   state->w = w;
 131
 132   y = gsl_vector_calloc (n);
 133
 134   if (y == 0)
 135     {
 136       gsl_vector_free (w);
 137       gsl_vector_free (v);
 138       gsl_matrix_free (H);
 139       gsl_permutation_free (perm);
 140       gsl_matrix_free (m);
 141
 142       GSL_ERROR ("failed to allocate space for y", GSL_ENOMEM);
 143     }
 144
 145   state->y = y;
 146
 147   fnew = gsl_vector_calloc (n);
 148
 149   if (fnew == 0)
 150     {
 151       gsl_vector_free (y);
 152       gsl_vector_free (w);
 153       gsl_vector_free (v);
 154       gsl_matrix_free (H);
 155       gsl_permutation_free (perm);
 156       gsl_matrix_free (m);
 157
 158       GSL_ERROR ("failed to allocate space for fnew", GSL_ENOMEM);
 159     }
 160
 161   state->fnew = fnew;
 162
 163   x_trial = gsl_vector_calloc (n);
 164
 165   if (x_trial == 0)
 166     {
 167       gsl_vector_free (fnew);
 168       gsl_vector_free (y);
 169       gsl_vector_free (w);
 170       gsl_vector_free (v);
 171       gsl_matrix_free (H);
 172       gsl_permutation_free (perm);
 173       gsl_matrix_free (m);
 174
 175       GSL_ERROR ("failed to allocate space for x_trial", GSL_ENOMEM);
 176     }
 177
 178   state->x_trial = x_trial;
 179
 180   p = gsl_vector_calloc (n);
 181
 182   if (p == 0)
 183     {
 184       gsl_vector_free (x_trial);
 185       gsl_vector_free (fnew);
 186       gsl_vector_free (y);
 187       gsl_vector_free (w);
 188       gsl_vector_free (v);
 189       gsl_matrix_free (H);
 190       gsl_permutation_free (perm);
 191       gsl_matrix_free (m);
 192
 193       GSL_ERROR ("failed to allocate space for p", GSL_ENOMEM);
 194     }
 195
 196   state->p = p;
 197
 198   return GSL_SUCCESS;
 199 }
 200
 201 static int
 202 broyden_set (void *vstate, gsl_multiroot_function * function, gsl_vector * x, gsl_vector * f, gsl_vector * dx)
 203 {
 204   broyden_state_t *state = (broyden_state_t *) vstate;
 205   size_t i, j, n = function->n;
 206   int signum = 0;
 207
 208   GSL_MULTIROOT_FN_EVAL (function, x, f);
 209
 210   gsl_multiroot_fdjacobian (function, x, f, GSL_SQRT_DBL_EPSILON, state->lu);
 211   gsl_linalg_LU_decomp (state->lu, state->permutation, &signum);
 212   gsl_linalg_LU_invert (state->lu, state->permutation, state->H);
 213
 214   for (i = 0; i < n; i++)
 215     for (j = 0; j < n; j++)
 216       gsl_matrix_set(state->H,i,j,-gsl_matrix_get(state->H,i,j));
 217
 218   for (i = 0; i < n; i++)
 219     {
 220       gsl_vector_set (dx, i, 0.0);
 221     }
 222
 223   state->phi = enorm (f);
 224
 225   return GSL_SUCCESS;
 226 }
 227
 228 static int
 229 broyden_iterate (void *vstate, gsl_multiroot_function * function, gsl_vector * x, gsl_vector * f, gsl_vector * dx)
 230 {
 231   broyden_state_t *state = (broyden_state_t *) vstate;
 232
 233   double phi0, phi1, t, lambda;
 234
 235   gsl_matrix *H = state->H;
 236   gsl_vector *p = state->p;
 237   gsl_vector *y = state->y;
 238   gsl_vector *v = state->v;
 239   gsl_vector *w = state->w;
 240   gsl_vector *fnew = state->fnew;
 241   gsl_vector *x_trial = state->x_trial;
 242   gsl_matrix *lu = state->lu;
 243   gsl_permutation *perm = state->permutation;
 244
 245   size_t i, j, iter;
 246
 247   size_t n = function->n;
 248
 249   /* p = H f */
 250
 251   for (i = 0; i < n; i++)
 252     {
 253       double sum = 0;
 254
 255       for (j = 0; j < n; j++)
 256         {
 257           sum += gsl_matrix_get (H, i, j) * gsl_vector_get (f, j);
 258         }
 259       gsl_vector_set (p, i, sum);
 260     }
 261
 262   t = 1;
 263   iter = 0;
 264
 265   phi0 = state->phi;
 266
 267 new_step:
 268
 269   for (i = 0; i < n; i++)
 270     {
 271       double pi = gsl_vector_get (p, i);
 272       double xi = gsl_vector_get (x, i);
 273       gsl_vector_set (x_trial, i, xi + t * pi);
 274     }
 275
 276   {
 277     int status = GSL_MULTIROOT_FN_EVAL (function, x_trial, fnew);
 278
 279     if (status != GSL_SUCCESS)
 280       {
 281         return GSL_EBADFUNC;
 282       }
 283   }
 284
 285   phi1 = enorm (fnew);
 286
 287   iter++ ;
 288
 289   if (phi1 > phi0 && iter < 10 && t > 0.1)
 290     {
 291       /* full step goes uphill, take a reduced step instead */
 292
 293       double theta = phi1 / phi0;
 294       t *= (sqrt (1.0 + 6.0 * theta) - 1.0) / (3.0 * theta);
 295       goto new_step;
 296     }
 297
 298   if (phi1 > phi0)
 299     {
 300       /* need to recompute Jacobian */
 301       int signum = 0;
 302
 303       gsl_multiroot_fdjacobian (function, x, f, GSL_SQRT_DBL_EPSILON, lu);
 304
 305       for (i = 0; i < n; i++)
 306         for (j = 0; j < n; j++)
 307           gsl_matrix_set(lu,i,j,-gsl_matrix_get(lu,i,j));
 308
 309       gsl_linalg_LU_decomp (lu, perm, &signum);
 310       gsl_linalg_LU_invert (lu, perm, H);
 311
 312       gsl_linalg_LU_solve (lu, perm, f, p);
 313
 314       t = 1;
 315
 316       for (i = 0; i < n; i++)
 317         {
 318           double pi = gsl_vector_get (p, i);
 319           double xi = gsl_vector_get (x, i);
 320           gsl_vector_set (x_trial, i, xi + t * pi);
 321         }
 322
 323       {
 324         int status = GSL_MULTIROOT_FN_EVAL (function, x_trial, fnew);
 325
 326         if (status != GSL_SUCCESS)
 327           {
 328             return GSL_EBADFUNC;
 329           }
 330       }
 331
 332       phi1 = enorm (fnew);
 333     }
 334
 335   /* y = f' - f */
 336
 337   for (i = 0; i < n; i++)
 338     {
 339       double yi = gsl_vector_get (fnew, i) - gsl_vector_get (f, i);
 340       gsl_vector_set (y, i, yi);
 341     }
 342
 343   /* v = H y */
 344
 345   for (i = 0; i < n; i++)
 346     {
 347       double sum = 0;
 348
 349       for (j = 0; j < n; j++)
 350         {
 351           sum += gsl_matrix_get (H, i, j) * gsl_vector_get (y, j);
 352         }
 353
 354       gsl_vector_set (v, i, sum);
 355     }
 356
 357   /* lambda = p . v */
 358
 359   lambda = 0;
 360
 361   for (i = 0; i < n; i++)
 362     {
 363       lambda += gsl_vector_get (p, i) * gsl_vector_get (v, i);
 364     }
 365
 366   if (lambda == 0)
 367     {
 368       GSL_ERROR ("approximation to Jacobian has collapsed", GSL_EZERODIV) ;
 369     }
 370
 371   /* v' = v + t * p */
 372
 373   for (i = 0; i < n; i++)
 374     {
 375       double vi = gsl_vector_get (v, i) + t * gsl_vector_get (p, i);
 376       gsl_vector_set (v, i, vi);
 377     }
 378
 379   /* w^T = p^T H */
 380
 381   for (i = 0; i < n; i++)
 382     {
 383       double sum = 0;
 384
 385       for (j = 0; j < n; j++)
 386         {
 387           sum += gsl_matrix_get (H, j, i) * gsl_vector_get (p, j);
 388         }
 389
 390       gsl_vector_set (w, i, sum);
 391     }
 392
 393   /* Hij -> Hij - (vi wj / lambda) */
 394
 395   for (i = 0; i < n; i++)
 396     {
 397       double vi = gsl_vector_get (v, i);
 398
 399       for (j = 0; j < n; j++)
 400         {
 401           double wj = gsl_vector_get (w, j);
 402           double Hij = gsl_matrix_get (H, i, j) - vi * wj / lambda;
 403           gsl_matrix_set (H, i, j, Hij);
 404         }
 405     }
 406
 407   /* copy fnew into f */
 408
 409   gsl_vector_memcpy (f, fnew);
 410
 411   /* copy x_trial into x */
 412
 413   gsl_vector_memcpy (x, x_trial);
 414
 415   for (i = 0; i < n; i++)
 416     {
 417       double pi = gsl_vector_get (p, i);
 418       gsl_vector_set (dx, i, t * pi);
 419     }
 420
 421   state->phi = phi1;
 422
 423   return GSL_SUCCESS;
 424 }
 425
 426
 427 static void
 428 broyden_free (void *vstate)
 429 {
 430   broyden_state_t *state = (broyden_state_t *) vstate;
 431
 432   gsl_matrix_free (state->H);
 433
 434   gsl_matrix_free (state->lu);
 435   gsl_permutation_free (state->permutation);
 436
 437   gsl_vector_free (state->v);
 438   gsl_vector_free (state->w);
 439   gsl_vector_free (state->y);
 440   gsl_vector_free (state->p);
 441
 442   gsl_vector_free (state->fnew);
 443   gsl_vector_free (state->x_trial);
 444
 445 }
 446
 447
 448 static const gsl_multiroot_fsolver_type broyden_type =
 449 {"broyden",                     /* name */
 450  sizeof (broyden_state_t),
 451  &broyden_alloc,
 452  &broyden_set,
 453  &broyden_iterate,
 454  &broyden_free};
 455
 456 const gsl_multiroot_fsolver_type *gsl_multiroot_fsolver_broyden = &broyden_type;