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

   1 /* multiroots/hybrid.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 "dogleg.c"
  34
  35 typedef struct
  36 {
  37   size_t iter;
  38   size_t ncfail;
  39   size_t ncsuc;
  40   size_t nslow1;
  41   size_t nslow2;
  42   double fnorm;
  43   double delta;
  44   gsl_matrix *J;
  45   gsl_matrix *q;
  46   gsl_matrix *r;
  47   gsl_vector *tau;
  48   gsl_vector *diag;
  49   gsl_vector *qtf;
  50   gsl_vector *newton;
  51   gsl_vector *gradient;
  52   gsl_vector *x_trial;
  53   gsl_vector *f_trial;
  54   gsl_vector *df;
  55   gsl_vector *qtdf;
  56   gsl_vector *rdx;
  57   gsl_vector *w;
  58   gsl_vector *v;
  59 }
  60 hybrid_state_t;
  61
  62 static int hybrid_alloc (void *vstate, size_t n);
  63 static int hybrid_set (void *vstate, gsl_multiroot_function * func,
  64                        gsl_vector * x, gsl_vector * f, gsl_vector * dx);
  65 static int hybrids_set (void *vstate, gsl_multiroot_function * func,
  66                         gsl_vector * x, gsl_vector * f, gsl_vector * dx);
  67 static int set (void *vstate, gsl_multiroot_function * func, gsl_vector * x,
  68                 gsl_vector * f, gsl_vector * dx, int scale);
  69 static int hybrid_iterate (void *vstate, gsl_multiroot_function * func,
  70                            gsl_vector * x, gsl_vector * f, gsl_vector * dx);
  71 static void hybrid_free (void *vstate);
  72 static int iterate (void *vstate, gsl_multiroot_function * func,
  73                     gsl_vector * x, gsl_vector * f, gsl_vector * dx,
  74                     int scale);
  75
  76 static int
  77 hybrid_alloc (void *vstate, size_t n)
  78 {
  79   hybrid_state_t *state = (hybrid_state_t *) vstate;
  80   gsl_matrix *J, *q, *r;
  81   gsl_vector *tau, *diag, *qtf, *newton, *gradient, *x_trial, *f_trial,
  82     *df, *qtdf, *rdx, *w, *v;
  83
  84   J = gsl_matrix_calloc (n, n);
  85
  86   if (J == 0)
  87     {
  88       GSL_ERROR ("failed to allocate space for J", GSL_ENOMEM);
  89     }
  90
  91   state->J = J;
  92
  93   q = gsl_matrix_calloc (n, n);
  94
  95   if (q == 0)
  96     {
  97       gsl_matrix_free (J);
  98
  99       GSL_ERROR ("failed to allocate space for q", GSL_ENOMEM);
 100     }
 101
 102   state->q = q;
 103
 104   r = gsl_matrix_calloc (n, n);
 105
 106   if (r == 0)
 107     {
 108       gsl_matrix_free (J);
 109       gsl_matrix_free (q);
 110
 111       GSL_ERROR ("failed to allocate space for r", GSL_ENOMEM);
 112     }
 113
 114   state->r = r;
 115
 116   tau = gsl_vector_calloc (n);
 117
 118   if (tau == 0)
 119     {
 120       gsl_matrix_free (J);
 121       gsl_matrix_free (q);
 122       gsl_matrix_free (r);
 123
 124       GSL_ERROR ("failed to allocate space for tau", GSL_ENOMEM);
 125     }
 126
 127   state->tau = tau;
 128
 129   diag = gsl_vector_calloc (n);
 130
 131   if (diag == 0)
 132     {
 133       gsl_matrix_free (J);
 134       gsl_matrix_free (q);
 135       gsl_matrix_free (r);
 136       gsl_vector_free (tau);
 137
 138       GSL_ERROR ("failed to allocate space for diag", GSL_ENOMEM);
 139     }
 140
 141   state->diag = diag;
 142
 143   qtf = gsl_vector_calloc (n);
 144
 145   if (qtf == 0)
 146     {
 147       gsl_matrix_free (J);
 148       gsl_matrix_free (q);
 149       gsl_matrix_free (r);
 150       gsl_vector_free (tau);
 151       gsl_vector_free (diag);
 152
 153       GSL_ERROR ("failed to allocate space for qtf", GSL_ENOMEM);
 154     }
 155
 156   state->qtf = qtf;
 157
 158   newton = gsl_vector_calloc (n);
 159
 160   if (newton == 0)
 161     {
 162       gsl_matrix_free (J);
 163       gsl_matrix_free (q);
 164       gsl_matrix_free (r);
 165       gsl_vector_free (tau);
 166       gsl_vector_free (diag);
 167       gsl_vector_free (qtf);
 168
 169       GSL_ERROR ("failed to allocate space for newton", GSL_ENOMEM);
 170     }
 171
 172   state->newton = newton;
 173
 174   gradient = gsl_vector_calloc (n);
 175
 176   if (gradient == 0)
 177     {
 178       gsl_matrix_free (J);
 179       gsl_matrix_free (q);
 180       gsl_matrix_free (r);
 181       gsl_vector_free (tau);
 182       gsl_vector_free (diag);
 183       gsl_vector_free (qtf);
 184       gsl_vector_free (newton);
 185
 186       GSL_ERROR ("failed to allocate space for gradient", GSL_ENOMEM);
 187     }
 188
 189   state->gradient = gradient;
 190
 191   x_trial = gsl_vector_calloc (n);
 192
 193   if (x_trial == 0)
 194     {
 195       gsl_matrix_free (J);
 196       gsl_matrix_free (q);
 197       gsl_matrix_free (r);
 198       gsl_vector_free (tau);
 199       gsl_vector_free (diag);
 200       gsl_vector_free (qtf);
 201       gsl_vector_free (newton);
 202       gsl_vector_free (gradient);
 203
 204       GSL_ERROR ("failed to allocate space for x_trial", GSL_ENOMEM);
 205     }
 206
 207   state->x_trial = x_trial;
 208
 209   f_trial = gsl_vector_calloc (n);
 210
 211   if (f_trial == 0)
 212     {
 213       gsl_matrix_free (J);
 214       gsl_matrix_free (q);
 215       gsl_matrix_free (r);
 216       gsl_vector_free (tau);
 217       gsl_vector_free (diag);
 218       gsl_vector_free (qtf);
 219       gsl_vector_free (newton);
 220       gsl_vector_free (gradient);
 221       gsl_vector_free (x_trial);
 222
 223       GSL_ERROR ("failed to allocate space for f_trial", GSL_ENOMEM);
 224     }
 225
 226   state->f_trial = f_trial;
 227
 228   df = gsl_vector_calloc (n);
 229
 230   if (df == 0)
 231     {
 232       gsl_matrix_free (J);
 233       gsl_matrix_free (q);
 234       gsl_matrix_free (r);
 235       gsl_vector_free (tau);
 236       gsl_vector_free (diag);
 237       gsl_vector_free (qtf);
 238       gsl_vector_free (newton);
 239       gsl_vector_free (gradient);
 240       gsl_vector_free (x_trial);
 241       gsl_vector_free (f_trial);
 242
 243       GSL_ERROR ("failed to allocate space for df", GSL_ENOMEM);
 244     }
 245
 246   state->df = df;
 247
 248   qtdf = gsl_vector_calloc (n);
 249
 250   if (qtdf == 0)
 251     {
 252       gsl_matrix_free (J);
 253       gsl_matrix_free (q);
 254       gsl_matrix_free (r);
 255       gsl_vector_free (tau);
 256       gsl_vector_free (diag);
 257       gsl_vector_free (qtf);
 258       gsl_vector_free (newton);
 259       gsl_vector_free (gradient);
 260       gsl_vector_free (x_trial);
 261       gsl_vector_free (f_trial);
 262       gsl_vector_free (df);
 263
 264       GSL_ERROR ("failed to allocate space for qtdf", GSL_ENOMEM);
 265     }
 266
 267   state->qtdf = qtdf;
 268
 269
 270   rdx = gsl_vector_calloc (n);
 271
 272   if (rdx == 0)
 273     {
 274       gsl_matrix_free (J);
 275       gsl_matrix_free (q);
 276       gsl_matrix_free (r);
 277       gsl_vector_free (tau);
 278       gsl_vector_free (diag);
 279       gsl_vector_free (qtf);
 280       gsl_vector_free (newton);
 281       gsl_vector_free (gradient);
 282       gsl_vector_free (x_trial);
 283       gsl_vector_free (f_trial);
 284       gsl_vector_free (df);
 285       gsl_vector_free (qtdf);
 286
 287       GSL_ERROR ("failed to allocate space for rdx", GSL_ENOMEM);
 288     }
 289
 290   state->rdx = rdx;
 291
 292   w = gsl_vector_calloc (n);
 293
 294   if (w == 0)
 295     {
 296       gsl_matrix_free (J);
 297       gsl_matrix_free (q);
 298       gsl_matrix_free (r);
 299       gsl_vector_free (tau);
 300       gsl_vector_free (diag);
 301       gsl_vector_free (qtf);
 302       gsl_vector_free (newton);
 303       gsl_vector_free (gradient);
 304       gsl_vector_free (x_trial);
 305       gsl_vector_free (f_trial);
 306       gsl_vector_free (df);
 307       gsl_vector_free (qtdf);
 308       gsl_vector_free (rdx);
 309
 310       GSL_ERROR ("failed to allocate space for w", GSL_ENOMEM);
 311     }
 312
 313   state->w = w;
 314
 315   v = gsl_vector_calloc (n);
 316
 317   if (v == 0)
 318     {
 319       gsl_matrix_free (J);
 320       gsl_matrix_free (q);
 321       gsl_matrix_free (r);
 322       gsl_vector_free (tau);
 323       gsl_vector_free (diag);
 324       gsl_vector_free (qtf);
 325       gsl_vector_free (newton);
 326       gsl_vector_free (gradient);
 327       gsl_vector_free (x_trial);
 328       gsl_vector_free (f_trial);
 329       gsl_vector_free (df);
 330       gsl_vector_free (qtdf);
 331       gsl_vector_free (rdx);
 332       gsl_vector_free (w);
 333
 334       GSL_ERROR ("failed to allocate space for v", GSL_ENOMEM);
 335     }
 336
 337   state->v = v;
 338
 339   return GSL_SUCCESS;
 340 }
 341
 342 static int
 343 hybrid_set (void *vstate, gsl_multiroot_function * func, gsl_vector * x,
 344             gsl_vector * f, gsl_vector * dx)
 345 {
 346   int status = set (vstate, func, x, f, dx, 0);
 347   return status;
 348 }
 349
 350 static int
 351 hybrids_set (void *vstate, gsl_multiroot_function * func, gsl_vector * x,
 352              gsl_vector * f, gsl_vector * dx)
 353 {
 354   int status = set (vstate, func, x, f, dx, 1);
 355   return status;
 356 }
 357
 358 static int
 359 set (void *vstate, gsl_multiroot_function * func, gsl_vector * x,
 360      gsl_vector * f, gsl_vector * dx, int scale)
 361 {
 362   hybrid_state_t *state = (hybrid_state_t *) vstate;
 363
 364   gsl_matrix *J = state->J;
 365   gsl_matrix *q = state->q;
 366   gsl_matrix *r = state->r;
 367   gsl_vector *tau = state->tau;
 368   gsl_vector *diag = state->diag;
 369
 370   int status;
 371
 372   status = GSL_MULTIROOT_FN_EVAL (func, x, f);
 373
 374   if (status)
 375     {
 376       return status;
 377     }
 378
 379   status = gsl_multiroot_fdjacobian (func, x, f, GSL_SQRT_DBL_EPSILON, J);
 380
 381   if (status)
 382     {
 383       return status;
 384     }
 385
 386   state->iter = 1;
 387   state->fnorm = enorm (f);
 388   state->ncfail = 0;
 389   state->ncsuc = 0;
 390   state->nslow1 = 0;
 391   state->nslow2 = 0;
 392
 393   gsl_vector_set_all (dx, 0.0);
 394
 395   /* Store column norms in diag */
 396
 397   if (scale)
 398     compute_diag (J, diag);
 399   else
 400     gsl_vector_set_all (diag, 1.0);
 401
 402   /* Set delta to factor |D x| or to factor if |D x| is zero */
 403
 404   state->delta = compute_delta (diag, x);
 405
 406   /* Factorize J into QR decomposition */
 407
 408   status = gsl_linalg_QR_decomp (J, tau);
 409
 410   if (status)
 411     {
 412       return status;
 413     }
 414
 415   status = gsl_linalg_QR_unpack (J, tau, q, r);
 416
 417   return status;
 418 }
 419
 420 static int
 421 hybrid_iterate (void *vstate, gsl_multiroot_function * func, gsl_vector * x,
 422                 gsl_vector * f, gsl_vector * dx)
 423 {
 424   int status = iterate (vstate, func, x, f, dx, 0);
 425   return status;
 426 }
 427
 428 static int
 429 hybrids_iterate (void *vstate, gsl_multiroot_function * func, gsl_vector * x,
 430                  gsl_vector * f, gsl_vector * dx)
 431 {
 432   int status = iterate (vstate, func, x, f, dx, 1);
 433   return status;
 434 }
 435
 436 static int
 437 iterate (void *vstate, gsl_multiroot_function * func, gsl_vector * x,
 438          gsl_vector * f, gsl_vector * dx, int scale)
 439 {
 440   hybrid_state_t *state = (hybrid_state_t *) vstate;
 441
 442   const double fnorm = state->fnorm;
 443
 444   gsl_matrix *J = state->J;
 445   gsl_matrix *q = state->q;
 446   gsl_matrix *r = state->r;
 447   gsl_vector *tau = state->tau;
 448   gsl_vector *diag = state->diag;
 449   gsl_vector *qtf = state->qtf;
 450   gsl_vector *x_trial = state->x_trial;
 451   gsl_vector *f_trial = state->f_trial;
 452   gsl_vector *df = state->df;
 453   gsl_vector *qtdf = state->qtdf;
 454   gsl_vector *rdx = state->rdx;
 455   gsl_vector *w = state->w;
 456   gsl_vector *v = state->v;
 457
 458   double prered, actred;
 459   double pnorm, fnorm1, fnorm1p;
 460   double ratio;
 461   double p1 = 0.1, p5 = 0.5, p001 = 0.001, p0001 = 0.0001;
 462
 463   /* Compute qtf = Q^T f */
 464
 465   compute_qtf (q, f, qtf);
 466
 467   /* Compute dogleg step */
 468
 469   dogleg (r, qtf, diag, state->delta, state->newton, state->gradient, dx);
 470
 471   /* Take a trial step */
 472
 473   compute_trial_step (x, dx, state->x_trial);
 474
 475   pnorm = scaled_enorm (diag, dx);
 476
 477   if (state->iter == 1)
 478     {
 479       if (pnorm < state->delta)
 480         {
 481           state->delta = pnorm;
 482         }
 483     }
 484
 485   /* Evaluate function at x + p */
 486
 487   {
 488     int status = GSL_MULTIROOT_FN_EVAL (func, x_trial, f_trial);
 489
 490     if (status != GSL_SUCCESS)
 491       {
 492         return GSL_EBADFUNC;
 493       }
 494   }
 495
 496   /* Set df = f_trial - f */
 497
 498   compute_df (f_trial, f, df);
 499
 500   /* Compute the scaled actual reduction */
 501
 502   fnorm1 = enorm (f_trial);
 503
 504   actred = compute_actual_reduction (fnorm, fnorm1);
 505
 506   /* Compute rdx = R dx */
 507
 508   compute_rdx (r, dx, rdx);
 509
 510   /* Compute the scaled predicted reduction phi1p = |Q^T f + R dx| */
 511
 512   fnorm1p = enorm_sum (qtf, rdx);
 513
 514   prered = compute_predicted_reduction (fnorm, fnorm1p);
 515
 516   /* Compute the ratio of the actual to predicted reduction */
 517
 518   if (prered > 0)
 519     {
 520       ratio = actred / prered;
 521     }
 522   else
 523     {
 524       ratio = 0;
 525     }
 526
 527   /* Update the step bound */
 528
 529   if (ratio < p1)
 530     {
 531       state->ncsuc = 0;
 532       state->ncfail++;
 533       state->delta *= p5;
 534     }
 535   else
 536     {
 537       state->ncfail = 0;
 538       state->ncsuc++;
 539
 540       if (ratio >= p5 || state->ncsuc > 1)
 541         state->delta = GSL_MAX (state->delta, pnorm / p5);
 542       if (fabs (ratio - 1) <= p1)
 543         state->delta = pnorm / p5;
 544     }
 545
 546   /* Test for successful iteration */
 547
 548   if (ratio >= p0001)
 549     {
 550       gsl_vector_memcpy (x, x_trial);
 551       gsl_vector_memcpy (f, f_trial);
 552       state->fnorm = fnorm1;
 553       state->iter++;
 554     }
 555
 556   /* Determine the progress of the iteration */
 557
 558   state->nslow1++;
 559   if (actred >= p001)
 560     state->nslow1 = 0;
 561
 562   if (actred >= p1)
 563     state->nslow2 = 0;
 564
 565   if (state->ncfail == 2)
 566     {
 567       gsl_multiroot_fdjacobian (func, x, f, GSL_SQRT_DBL_EPSILON, J);
 568
 569       state->nslow2++;
 570
 571       if (state->iter == 1)
 572         {
 573           if (scale)
 574             compute_diag (J, diag);
 575           state->delta = compute_delta (diag, x);
 576         }
 577       else
 578         {
 579           if (scale)
 580             update_diag (J, diag);
 581         }
 582
 583       /* Factorize J into QR decomposition */
 584
 585       gsl_linalg_QR_decomp (J, tau);
 586       gsl_linalg_QR_unpack (J, tau, q, r);
 587
 588       return GSL_SUCCESS;
 589     }
 590
 591   /* Compute qtdf = Q^T df, w = (Q^T df - R dx)/|dx|,  v = D^2 dx/|dx| */
 592
 593   compute_qtf (q, df, qtdf);
 594
 595   compute_wv (qtdf, rdx, dx, diag, pnorm, w, v);
 596
 597   /* Rank-1 update of the jacobian Q'R' = Q(R + w v^T) */
 598
 599   gsl_linalg_QR_update (q, r, w, v);
 600
 601   /* No progress as measured by jacobian evaluations */
 602
 603   if (state->nslow2 == 5)
 604     {
 605       return GSL_ENOPROGJ;
 606     }
 607
 608   /* No progress as measured by function evaluations */
 609
 610   if (state->nslow1 == 10)
 611     {
 612       return GSL_ENOPROG;
 613     }
 614
 615   return GSL_SUCCESS;
 616 }
 617
 618
 619 static void
 620 hybrid_free (void *vstate)
 621 {
 622   hybrid_state_t *state = (hybrid_state_t *) vstate;
 623
 624   gsl_vector_free (state->v);
 625   gsl_vector_free (state->w);
 626   gsl_vector_free (state->rdx);
 627   gsl_vector_free (state->qtdf);
 628   gsl_vector_free (state->df);
 629   gsl_vector_free (state->f_trial);
 630   gsl_vector_free (state->x_trial);
 631   gsl_vector_free (state->gradient);
 632   gsl_vector_free (state->newton);
 633   gsl_vector_free (state->qtf);
 634   gsl_vector_free (state->diag);
 635   gsl_vector_free (state->tau);
 636   gsl_matrix_free (state->r);
 637   gsl_matrix_free (state->q);
 638   gsl_matrix_free (state->J);
 639 }
 640
 641 static const gsl_multiroot_fsolver_type hybrid_type = {
 642   "hybrid",                     /* name */
 643   sizeof (hybrid_state_t),
 644   &hybrid_alloc,
 645   &hybrid_set,
 646   &hybrid_iterate,
 647   &hybrid_free
 648 };
 649
 650 static const gsl_multiroot_fsolver_type hybrids_type = {
 651   "hybrids",                    /* name */
 652   sizeof (hybrid_state_t),
 653   &hybrid_alloc,
 654   &hybrids_set,
 655   &hybrids_iterate,
 656   &hybrid_free
 657 };
 658
 659 const gsl_multiroot_fsolver_type *gsl_multiroot_fsolver_hybrid = &hybrid_type;
 660 const gsl_multiroot_fsolver_type *gsl_multiroot_fsolver_hybrids =
 661   &hybrids_type;