htswanalysis/MACS/lib/gsl/gsl-1.11/linalg/svdstep.c

   1 /* linalg/svdstep.c
   2  *
   3  * Copyright (C) 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 static void
  21 chop_small_elements (gsl_vector * d, gsl_vector * f)
  22 {
  23   const size_t N = d->size;
  24   double d_i = gsl_vector_get (d, 0);
  25
  26   size_t i;
  27
  28   for (i = 0; i < N - 1; i++)
  29     {
  30       double f_i = gsl_vector_get (f, i);
  31       double d_ip1 = gsl_vector_get (d, i + 1);
  32
  33       if (fabs (f_i) < GSL_DBL_EPSILON * (fabs (d_i) + fabs (d_ip1)))
  34         {
  35           gsl_vector_set (f, i, 0.0);
  36         }
  37       d_i = d_ip1;
  38     }
  39
  40 }
  41
  42 static double
  43 trailing_eigenvalue (const gsl_vector * d, const gsl_vector * f)
  44 {
  45   const size_t n = d->size;
  46
  47   double da = gsl_vector_get (d, n - 2);
  48   double db = gsl_vector_get (d, n - 1);
  49   double fa = (n > 2) ? gsl_vector_get (f, n - 3) : 0.0;
  50   double fb = gsl_vector_get (f, n - 2);
  51
  52   double ta = da * da + fa * fa;
  53   double tb = db * db + fb * fb;
  54   double tab = da * fb;
  55
  56   double dt = (ta - tb) / 2.0;
  57
  58   double mu;
  59
  60   if (dt >= 0)
  61     {
  62       mu = tb - (tab * tab) / (dt + hypot (dt, tab));
  63     }
  64   else
  65     {
  66       mu = tb + (tab * tab) / ((-dt) + hypot (dt, tab));
  67     }
  68
  69   return mu;
  70 }
  71
  72 static void
  73 create_schur (double d0, double f0, double d1, double * c, double * s)
  74 {
  75   double apq = 2.0 * d0 * f0;
  76
  77   if (d0 == 0 || f0 == 0)
  78     {
  79       *c = 1.0;
  80       *s = 0.0;
  81       return;
  82     }
  83
  84   /* Check if we need to rescale to avoid underflow/overflow */
  85   if (fabs(d0) < GSL_SQRT_DBL_MIN || fabs(d0) > GSL_SQRT_DBL_MAX
  86       || fabs(f0) < GSL_SQRT_DBL_MIN || fabs(f0) > GSL_SQRT_DBL_MAX
  87       || fabs(d1) < GSL_SQRT_DBL_MIN || fabs(d1) > GSL_SQRT_DBL_MAX)
  88     {
  89       double scale;
  90       int d0_exp, f0_exp;
  91       frexp(d0, &d0_exp);
  92       frexp(f0, &f0_exp);
  93       /* Bring |d0*f0| into the range GSL_DBL_MIN to GSL_DBL_MAX */
  94       scale = ldexp(1.0, -(d0_exp + f0_exp)/4);
  95       d0 *= scale;
  96       f0 *= scale;
  97       d1 *= scale;
  98       apq = 2.0 * d0 * f0;
  99     }
 100
 101   if (apq != 0.0)
 102     {
 103       double t;
 104       double tau = (f0*f0 + (d1 + d0)*(d1 - d0)) / apq;
 105
 106       if (tau >= 0.0)
 107         {
 108           t = 1.0/(tau + hypot(1.0, tau));
 109         }
 110       else
 111         {
 112           t = -1.0/(-tau + hypot(1.0, tau));
 113         }
 114
 115       *c = 1.0 / hypot(1.0, t);
 116       *s = t * (*c);
 117     }
 118   else
 119     {
 120       *c = 1.0;
 121       *s = 0.0;
 122     }
 123 }
 124
 125 static void
 126 svd2 (gsl_vector * d, gsl_vector * f, gsl_matrix * U, gsl_matrix * V)
 127 {
 128   size_t i;
 129   double c, s, a11, a12, a21, a22;
 130
 131   const size_t M = U->size1;
 132   const size_t N = V->size1;
 133
 134   double d0 = gsl_vector_get (d, 0);
 135   double f0 = gsl_vector_get (f, 0);
 136
 137   double d1 = gsl_vector_get (d, 1);
 138
 139   if (d0 == 0.0)
 140     {
 141       /* Eliminate off-diagonal element in [0,f0;0,d1] to make [d,0;0,0] */
 142
 143       create_givens (f0, d1, &c, &s);
 144
 145       /* compute B <= G^T B X,  where X = [0,1;1,0] */
 146
 147       gsl_vector_set (d, 0, c * f0 - s * d1);
 148       gsl_vector_set (f, 0, s * f0 + c * d1);
 149       gsl_vector_set (d, 1, 0.0);
 150
 151       /* Compute U <= U G */
 152
 153       for (i = 0; i < M; i++)
 154         {
 155           double Uip = gsl_matrix_get (U, i, 0);
 156           double Uiq = gsl_matrix_get (U, i, 1);
 157           gsl_matrix_set (U, i, 0, c * Uip - s * Uiq);
 158           gsl_matrix_set (U, i, 1, s * Uip + c * Uiq);
 159         }
 160
 161       /* Compute V <= V X */
 162
 163       gsl_matrix_swap_columns (V, 0, 1);
 164
 165       return;
 166     }
 167   else if (d1 == 0.0)
 168     {
 169       /* Eliminate off-diagonal element in [d0,f0;0,0] */
 170
 171       create_givens (d0, f0, &c, &s);
 172
 173       /* compute B <= B G */
 174
 175       gsl_vector_set (d, 0, d0 * c - f0 * s);
 176       gsl_vector_set (f, 0, 0.0);
 177
 178       /* Compute V <= V G */
 179
 180       for (i = 0; i < N; i++)
 181         {
 182           double Vip = gsl_matrix_get (V, i, 0);
 183           double Viq = gsl_matrix_get (V, i, 1);
 184           gsl_matrix_set (V, i, 0, c * Vip - s * Viq);
 185           gsl_matrix_set (V, i, 1, s * Vip + c * Viq);
 186         }
 187
 188       return;
 189     }
 190   else
 191     {
 192       /* Make columns orthogonal, A = [d0, f0; 0, d1] * G */
 193
 194       create_schur (d0, f0, d1, &c, &s);
 195
 196       /* compute B <= B G */
 197
 198       a11 = c * d0 - s * f0;
 199       a21 = - s * d1;
 200
 201       a12 = s * d0 + c * f0;
 202       a22 = c * d1;
 203
 204       /* Compute V <= V G */
 205
 206       for (i = 0; i < N; i++)
 207         {
 208           double Vip = gsl_matrix_get (V, i, 0);
 209           double Viq = gsl_matrix_get (V, i, 1);
 210           gsl_matrix_set (V, i, 0, c * Vip - s * Viq);
 211           gsl_matrix_set (V, i, 1, s * Vip + c * Viq);
 212         }
 213
 214       /* Eliminate off-diagonal elements, bring column with largest
 215          norm to first column */
 216
 217       if (hypot(a11, a21) < hypot(a12,a22))
 218         {
 219           double t1, t2;
 220
 221           /* B <= B X */
 222
 223           t1 = a11; a11 = a12; a12 = t1;
 224           t2 = a21; a21 = a22; a22 = t2;
 225
 226           /* V <= V X */
 227
 228           gsl_matrix_swap_columns(V, 0, 1);
 229         }
 230
 231       create_givens (a11, a21, &c, &s);
 232
 233       /* compute B <= G^T B */
 234
 235       gsl_vector_set (d, 0, c * a11 - s * a21);
 236       gsl_vector_set (f, 0, c * a12 - s * a22);
 237       gsl_vector_set (d, 1, s * a12 + c * a22);
 238
 239       /* Compute U <= U G */
 240
 241       for (i = 0; i < M; i++)
 242         {
 243           double Uip = gsl_matrix_get (U, i, 0);
 244           double Uiq = gsl_matrix_get (U, i, 1);
 245           gsl_matrix_set (U, i, 0, c * Uip - s * Uiq);
 246           gsl_matrix_set (U, i, 1, s * Uip + c * Uiq);
 247         }
 248
 249       return;
 250     }
 251 }
 252
 253
 254 static void
 255 chase_out_intermediate_zero (gsl_vector * d, gsl_vector * f, gsl_matrix * U, size_t k0)
 256 {
 257 #if !USE_BLAS
 258   const size_t M = U->size1;
 259 #endif
 260   const size_t n = d->size;
 261   double c, s;
 262   double x, y;
 263   size_t k;
 264
 265   x = gsl_vector_get (f, k0);
 266   y = gsl_vector_get (d, k0+1);
 267
 268   for (k = k0; k < n - 1; k++)
 269     {
 270       create_givens (y, -x, &c, &s);
 271
 272       /* Compute U <= U G */
 273
 274 #ifdef USE_BLAS
 275       {
 276         gsl_vector_view Uk0 = gsl_matrix_column(U,k0);
 277         gsl_vector_view Ukp1 = gsl_matrix_column(U,k+1);
 278         gsl_blas_drot(&Uk0.vector, &Ukp1.vector, c, -s);
 279       }
 280 #else
 281       {
 282         size_t i;
 283
 284         for (i = 0; i < M; i++)
 285           {
 286             double Uip = gsl_matrix_get (U, i, k0);
 287             double Uiq = gsl_matrix_get (U, i, k + 1);
 288             gsl_matrix_set (U, i, k0, c * Uip - s * Uiq);
 289             gsl_matrix_set (U, i, k + 1, s * Uip + c * Uiq);
 290           }
 291       }
 292 #endif
 293
 294       /* compute B <= G^T B */
 295
 296       gsl_vector_set (d, k + 1, s * x + c * y);
 297
 298       if (k == k0)
 299         gsl_vector_set (f, k, c * x - s * y );
 300
 301       if (k < n - 2)
 302         {
 303           double z = gsl_vector_get (f, k + 1);
 304           gsl_vector_set (f, k + 1, c * z);
 305
 306           x = -s * z ;
 307           y = gsl_vector_get (d, k + 2);
 308         }
 309     }
 310 }
 311
 312 static void
 313 chase_out_trailing_zero (gsl_vector * d, gsl_vector * f, gsl_matrix * V)
 314 {
 315 #if !USE_BLAS
 316   const size_t N = V->size1;
 317 #endif
 318   const size_t n = d->size;
 319   double c, s;
 320   double x, y;
 321   size_t k;
 322
 323   x = gsl_vector_get (d, n - 2);
 324   y = gsl_vector_get (f, n - 2);
 325
 326   for (k = n - 1; k > 0 && k--;)
 327     {
 328       create_givens (x, y, &c, &s);
 329
 330       /* Compute V <= V G where G = [c, s ; -s, c] */
 331
 332 #ifdef USE_BLAS
 333       {
 334         gsl_vector_view Vp = gsl_matrix_column(V,k);
 335         gsl_vector_view Vq = gsl_matrix_column(V,n-1);
 336         gsl_blas_drot(&Vp.vector, &Vq.vector, c, -s);
 337       }
 338 #else
 339       {
 340         size_t i;
 341
 342         for (i = 0; i < N; i++)
 343           {
 344             double Vip = gsl_matrix_get (V, i, k);
 345             double Viq = gsl_matrix_get (V, i, n - 1);
 346             gsl_matrix_set (V, i, k, c * Vip - s * Viq);
 347             gsl_matrix_set (V, i, n - 1, s * Vip + c * Viq);
 348           }
 349       }
 350 #endif
 351
 352       /* compute B <= B G */
 353
 354       gsl_vector_set (d, k, c * x - s * y);
 355
 356       if (k == n - 2)
 357         gsl_vector_set (f, k, s * x + c * y );
 358
 359       if (k > 0)
 360         {
 361           double z = gsl_vector_get (f, k - 1);
 362           gsl_vector_set (f, k - 1, c * z);
 363
 364           x = gsl_vector_get (d, k - 1);
 365           y = s * z ;
 366         }
 367     }
 368 }
 369
 370 static void
 371 qrstep (gsl_vector * d, gsl_vector * f, gsl_matrix * U, gsl_matrix * V)
 372 {
 373 #if !USE_BLAS
 374   const size_t M = U->size1;
 375   const size_t N = V->size1;
 376 #endif
 377   const size_t n = d->size;
 378   double y, z;
 379   double ak, bk, zk, ap, bp, aq, bq;
 380   size_t i, k;
 381
 382   if (n == 1)
 383     return;  /* shouldn't happen */
 384
 385   /* Compute 2x2 svd directly */
 386
 387   if (n == 2)
 388     {
 389       svd2 (d, f, U, V);
 390       return;
 391     }
 392
 393   /* Chase out any zeroes on the diagonal */
 394
 395   for (i = 0; i < n - 1; i++)
 396     {
 397       double d_i = gsl_vector_get (d, i);
 398
 399       if (d_i == 0.0)
 400         {
 401           chase_out_intermediate_zero (d, f, U, i);
 402           return;
 403         }
 404     }
 405
 406   /* Chase out any zero at the end of the diagonal */
 407
 408   {
 409     double d_nm1 = gsl_vector_get (d, n - 1);
 410
 411     if (d_nm1 == 0.0)
 412       {
 413         chase_out_trailing_zero (d, f, V);
 414         return;
 415       }
 416   }
 417
 418
 419   /* Apply QR reduction steps to the diagonal and offdiagonal */
 420
 421   {
 422     double d0 = gsl_vector_get (d, 0);
 423     double f0 = gsl_vector_get (f, 0);
 424
 425     double d1 = gsl_vector_get (d, 1);
 426     double f1 = gsl_vector_get (f, 1);
 427
 428     {
 429       double mu = trailing_eigenvalue (d, f);
 430
 431       y = d0 * d0 - mu;
 432       z = d0 * f0;
 433     }
 434
 435     /* Set up the recurrence for Givens rotations on a bidiagonal matrix */
 436
 437     ak = 0;
 438     bk = 0;
 439
 440     ap = d0;
 441     bp = f0;
 442
 443     aq = d1;
 444     bq = f1;
 445   }
 446
 447   for (k = 0; k < n - 1; k++)
 448     {
 449       double c, s;
 450       create_givens (y, z, &c, &s);
 451
 452       /* Compute V <= V G */
 453
 454 #ifdef USE_BLAS
 455       {
 456         gsl_vector_view Vk = gsl_matrix_column(V,k);
 457         gsl_vector_view Vkp1 = gsl_matrix_column(V,k+1);
 458         gsl_blas_drot(&Vk.vector, &Vkp1.vector, c, -s);
 459       }
 460 #else
 461       for (i = 0; i < N; i++)
 462         {
 463           double Vip = gsl_matrix_get (V, i, k);
 464           double Viq = gsl_matrix_get (V, i, k + 1);
 465           gsl_matrix_set (V, i, k, c * Vip - s * Viq);
 466           gsl_matrix_set (V, i, k + 1, s * Vip + c * Viq);
 467         }
 468 #endif
 469
 470       /* compute B <= B G */
 471
 472       {
 473         double bk1 = c * bk - s * z;
 474
 475         double ap1 = c * ap - s * bp;
 476         double bp1 = s * ap + c * bp;
 477         double zp1 = -s * aq;
 478
 479         double aq1 = c * aq;
 480
 481         if (k > 0)
 482           {
 483             gsl_vector_set (f, k - 1, bk1);
 484           }
 485
 486         ak = ap1;
 487         bk = bp1;
 488         zk = zp1;
 489
 490         ap = aq1;
 491
 492         if (k < n - 2)
 493           {
 494             bp = gsl_vector_get (f, k + 1);
 495           }
 496         else
 497           {
 498             bp = 0.0;
 499           }
 500
 501         y = ak;
 502         z = zk;
 503       }
 504
 505       create_givens (y, z, &c, &s);
 506
 507       /* Compute U <= U G */
 508
 509 #ifdef USE_BLAS
 510       {
 511         gsl_vector_view Uk = gsl_matrix_column(U,k);
 512         gsl_vector_view Ukp1 = gsl_matrix_column(U,k+1);
 513         gsl_blas_drot(&Uk.vector, &Ukp1.vector, c, -s);
 514       }
 515 #else
 516       for (i = 0; i < M; i++)
 517         {
 518           double Uip = gsl_matrix_get (U, i, k);
 519           double Uiq = gsl_matrix_get (U, i, k + 1);
 520           gsl_matrix_set (U, i, k, c * Uip - s * Uiq);
 521           gsl_matrix_set (U, i, k + 1, s * Uip + c * Uiq);
 522         }
 523 #endif
 524
 525       /* compute B <= G^T B */
 526
 527       {
 528         double ak1 = c * ak - s * zk;
 529         double bk1 = c * bk - s * ap;
 530         double zk1 = -s * bp;
 531
 532         double ap1 = s * bk + c * ap;
 533         double bp1 = c * bp;
 534
 535         gsl_vector_set (d, k, ak1);
 536
 537         ak = ak1;
 538         bk = bk1;
 539         zk = zk1;
 540
 541         ap = ap1;
 542         bp = bp1;
 543
 544         if (k < n - 2)
 545           {
 546             aq = gsl_vector_get (d, k + 2);
 547           }
 548         else
 549           {
 550             aq = 0.0;
 551           }
 552
 553         y = bk;
 554         z = zk;
 555       }
 556     }
 557
 558   gsl_vector_set (f, n - 2, bk);
 559   gsl_vector_set (d, n - 1, ap);
 560 }
 561
 562