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

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