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

   1 /* linalg/tridiag.c
   2  *
   3  * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2002, 2004, 2007 Gerard Jungman, Brian Gough, David Necas
   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 /* Author: G. Jungman */
  21
  22 #include <config.h>
  23 #include <stdlib.h>
  24 #include <math.h>
  25 #include <gsl/gsl_errno.h>
  26 #include "tridiag.h"
  27 #include <gsl/gsl_linalg.h>
  28
  29 /* for description of method see [Engeln-Mullges + Uhlig, p. 92]
  30  *
  31  *     diag[0]  offdiag[0]             0   .....
  32  *  offdiag[0]     diag[1]    offdiag[1]   .....
  33  *           0  offdiag[1]       diag[2]
  34  *           0           0    offdiag[2]   .....
  35  */
  36 static
  37 int
  38 solve_tridiag(
  39   const double diag[], size_t d_stride,
  40   const double offdiag[], size_t o_stride,
  41   const double b[], size_t b_stride,
  42   double x[], size_t x_stride,
  43   size_t N)
  44 {
  45   int status = GSL_SUCCESS;
  46   double *gamma = (double *) malloc (N * sizeof (double));
  47   double *alpha = (double *) malloc (N * sizeof (double));
  48   double *c = (double *) malloc (N * sizeof (double));
  49   double *z = (double *) malloc (N * sizeof (double));
  50
  51   if (gamma == 0 || alpha == 0 || c == 0 || z == 0)
  52     {
  53       GSL_ERROR("failed to allocate working space", GSL_ENOMEM);
  54     }
  55   else
  56     {
  57       size_t i, j;
  58
  59       /* Cholesky decomposition
  60          A = L.D.L^t
  61          lower_diag(L) = gamma
  62          diag(D) = alpha
  63        */
  64       alpha[0] = diag[0];
  65       gamma[0] = offdiag[0] / alpha[0];
  66
  67       if (alpha[0] == 0) {
  68         status = GSL_EZERODIV;
  69       }
  70
  71       for (i = 1; i < N - 1; i++)
  72         {
  73           alpha[i] = diag[d_stride * i] - offdiag[o_stride*(i - 1)] * gamma[i - 1];
  74           gamma[i] = offdiag[o_stride * i] / alpha[i];
  75           if (alpha[i] == 0) {
  76             status = GSL_EZERODIV;
  77           }
  78         }
  79
  80       if (N > 1)
  81         {
  82           alpha[N - 1] = diag[d_stride * (N - 1)] - offdiag[o_stride*(N - 2)] * gamma[N - 2];
  83         }
  84
  85       /* update RHS */
  86       z[0] = b[0];
  87       for (i = 1; i < N; i++)
  88         {
  89           z[i] = b[b_stride * i] - gamma[i - 1] * z[i - 1];
  90         }
  91       for (i = 0; i < N; i++)
  92         {
  93           c[i] = z[i] / alpha[i];
  94         }
  95
  96       /* backsubstitution */
  97       x[x_stride * (N - 1)] = c[N - 1];
  98       if (N >= 2)
  99         {
 100           for (i = N - 2, j = 0; j <= N - 2; j++, i--)
 101             {
 102               x[x_stride * i] = c[i] - gamma[i] * x[x_stride * (i + 1)];
 103             }
 104         }
 105     }
 106
 107   if (z != 0)
 108     free (z);
 109   if (c != 0)
 110     free (c);
 111   if (alpha != 0)
 112     free (alpha);
 113   if (gamma != 0)
 114     free (gamma);
 115
 116   if (status == GSL_EZERODIV) {
 117     GSL_ERROR ("matrix must be positive definite", status);
 118   }
 119
 120   return status;
 121 }
 122
 123 /* plain gauss elimination, only not bothering with the zeroes
 124  *
 125  *       diag[0]  abovediag[0]             0   .....
 126  *  belowdiag[0]       diag[1]  abovediag[1]   .....
 127  *             0  belowdiag[1]       diag[2]
 128  *             0             0  belowdiag[2]   .....
 129  */
 130 static
 131 int
 132 solve_tridiag_nonsym(
 133   const double diag[], size_t d_stride,
 134   const double abovediag[], size_t a_stride,
 135   const double belowdiag[], size_t b_stride,
 136   const double rhs[], size_t r_stride,
 137   double x[], size_t x_stride,
 138   size_t N)
 139 {
 140   int status = GSL_SUCCESS;
 141   double *alpha = (double *) malloc (N * sizeof (double));
 142   double *z = (double *) malloc (N * sizeof (double));
 143
 144   if (alpha == 0 || z == 0)
 145     {
 146       GSL_ERROR("failed to allocate working space", GSL_ENOMEM);
 147     }
 148   else
 149     {
 150       size_t i, j;
 151
 152       /* Bidiagonalization (eliminating belowdiag)
 153          & rhs update
 154          diag' = alpha
 155          rhs' = z
 156        */
 157       alpha[0] = diag[0];
 158       z[0] = rhs[0];
 159
 160       if (alpha[0] == 0) {
 161         status = GSL_EZERODIV;
 162       }
 163
 164       for (i = 1; i < N; i++)
 165         {
 166           const double t = belowdiag[b_stride*(i - 1)]/alpha[i-1];
 167           alpha[i] = diag[d_stride*i] - t*abovediag[a_stride*(i - 1)];
 168           z[i] = rhs[r_stride*i] - t*z[i-1];
 169           if (alpha[i] == 0) {
 170             status = GSL_EZERODIV;
 171           }
 172         }
 173
 174       /* backsubstitution */
 175       x[x_stride * (N - 1)] = z[N - 1]/alpha[N - 1];
 176       if (N >= 2)
 177         {
 178           for (i = N - 2, j = 0; j <= N - 2; j++, i--)
 179             {
 180               x[x_stride * i] = (z[i] - abovediag[a_stride*i] * x[x_stride * (i + 1)])/alpha[i];
 181             }
 182         }
 183     }
 184
 185   if (z != 0)
 186     free (z);
 187   if (alpha != 0)
 188     free (alpha);
 189
 190   if (status == GSL_EZERODIV) {
 191     GSL_ERROR ("matrix must be positive definite", status);
 192   }
 193
 194   return status;
 195 }
 196
 197 /* for description of method see [Engeln-Mullges + Uhlig, p. 96]
 198  *
 199  *      diag[0]  offdiag[0]             0   .....  offdiag[N-1]
 200  *   offdiag[0]     diag[1]    offdiag[1]   .....
 201  *            0  offdiag[1]       diag[2]
 202  *            0           0    offdiag[2]   .....
 203  *          ...         ...
 204  * offdiag[N-1]         ...
 205  *
 206  */
 207 static
 208 int
 209 solve_cyc_tridiag(
 210   const double diag[], size_t d_stride,
 211   const double offdiag[], size_t o_stride,
 212   const double b[], size_t b_stride,
 213   double x[], size_t x_stride,
 214   size_t N)
 215 {
 216   int status = GSL_SUCCESS;
 217   double * delta = (double *) malloc (N * sizeof (double));
 218   double * gamma = (double *) malloc (N * sizeof (double));
 219   double * alpha = (double *) malloc (N * sizeof (double));
 220   double * c = (double *) malloc (N * sizeof (double));
 221   double * z = (double *) malloc (N * sizeof (double));
 222
 223   if (delta == 0 || gamma == 0 || alpha == 0 || c == 0 || z == 0)
 224     {
 225       GSL_ERROR("failed to allocate working space", GSL_ENOMEM);
 226     }
 227   else
 228     {
 229       size_t i, j;
 230       double sum = 0.0;
 231
 232       /* factor */
 233
 234       if (N == 1)
 235         {
 236           x[0] = b[0] / diag[0];
 237           return GSL_SUCCESS;
 238         }
 239
 240       alpha[0] = diag[0];
 241       gamma[0] = offdiag[0] / alpha[0];
 242       delta[0] = offdiag[o_stride * (N-1)] / alpha[0];
 243
 244       if (alpha[0] == 0) {
 245         status = GSL_EZERODIV;
 246       }
 247
 248       for (i = 1; i < N - 2; i++)
 249         {
 250           alpha[i] = diag[d_stride * i] - offdiag[o_stride * (i-1)] * gamma[i - 1];
 251           gamma[i] = offdiag[o_stride * i] / alpha[i];
 252           delta[i] = -delta[i - 1] * offdiag[o_stride * (i-1)] / alpha[i];
 253           if (alpha[i] == 0) {
 254             status = GSL_EZERODIV;
 255           }
 256         }
 257
 258       for (i = 0; i < N - 2; i++)
 259         {
 260           sum += alpha[i] * delta[i] * delta[i];
 261         }
 262
 263       alpha[N - 2] = diag[d_stride * (N - 2)] - offdiag[o_stride * (N - 3)] * gamma[N - 3];
 264
 265       gamma[N - 2] = (offdiag[o_stride * (N - 2)] - offdiag[o_stride * (N - 3)] * delta[N - 3]) / alpha[N - 2];
 266
 267       alpha[N - 1] = diag[d_stride * (N - 1)] - sum - alpha[(N - 2)] * gamma[N - 2] * gamma[N - 2];
 268
 269       /* update */
 270       z[0] = b[0];
 271       for (i = 1; i < N - 1; i++)
 272         {
 273           z[i] = b[b_stride * i] - z[i - 1] * gamma[i - 1];
 274         }
 275       sum = 0.0;
 276       for (i = 0; i < N - 2; i++)
 277         {
 278           sum += delta[i] * z[i];
 279         }
 280       z[N - 1] = b[b_stride * (N - 1)] - sum - gamma[N - 2] * z[N - 2];
 281       for (i = 0; i < N; i++)
 282         {
 283           c[i] = z[i] / alpha[i];
 284         }
 285
 286       /* backsubstitution */
 287       x[x_stride * (N - 1)] = c[N - 1];
 288       x[x_stride * (N - 2)] = c[N - 2] - gamma[N - 2] * x[x_stride * (N - 1)];
 289       if (N >= 3)
 290         {
 291           for (i = N - 3, j = 0; j <= N - 3; j++, i--)
 292             {
 293               x[x_stride * i] = c[i] - gamma[i] * x[x_stride * (i + 1)] - delta[i] * x[x_stride * (N - 1)];
 294             }
 295         }
 296     }
 297
 298   if (z != 0)
 299     free (z);
 300   if (c != 0)
 301     free (c);
 302   if (alpha != 0)
 303     free (alpha);
 304   if (gamma != 0)
 305     free (gamma);
 306   if (delta != 0)
 307     free (delta);
 308
 309   if (status == GSL_EZERODIV) {
 310     GSL_ERROR ("matrix must be positive definite", status);
 311   }
 312
 313   return status;
 314 }
 315
 316 /* solve following system w/o the corner elements and then use
 317  * Sherman-Morrison formula to compensate for them
 318  *
 319  *        diag[0]  abovediag[0]             0   .....  belowdiag[N-1]
 320  *   belowdiag[0]       diag[1]  abovediag[1]   .....
 321  *              0  belowdiag[1]       diag[2]
 322  *              0             0  belowdiag[2]   .....
 323  *            ...           ...
 324  * abovediag[N-1]           ...
 325  */
 326 static
 327 int solve_cyc_tridiag_nonsym(
 328   const double diag[], size_t d_stride,
 329   const double abovediag[], size_t a_stride,
 330   const double belowdiag[], size_t b_stride,
 331   const double rhs[], size_t r_stride,
 332   double x[], size_t x_stride,
 333   size_t N)
 334 {
 335   int status = GSL_SUCCESS;
 336   double *alpha = (double *) malloc (N * sizeof (double));
 337   double *zb = (double *) malloc (N * sizeof (double));
 338   double *zu = (double *) malloc (N * sizeof (double));
 339   double *w = (double *) malloc (N * sizeof (double));
 340
 341   if (alpha == 0 || zb == 0 || zu == 0 || w == 0)
 342     {
 343       GSL_ERROR("failed to allocate working space", GSL_ENOMEM);
 344     }
 345   else
 346     {
 347       double beta;
 348
 349       /* Bidiagonalization (eliminating belowdiag)
 350          & rhs update
 351          diag' = alpha
 352          rhs' = zb
 353          rhs' for Aq=u is zu
 354        */
 355       zb[0] = rhs[0];
 356       if (diag[0] != 0) beta = -diag[0]; else beta = 1;
 357       {
 358         const double q = 1 - abovediag[0]*belowdiag[0]/(diag[0]*diag[d_stride]);
 359         if (fabs(q/beta) > 0.5 && fabs(q/beta) < 2) {
 360           beta *= (fabs(q/beta) < 1) ? 0.5 : 2;
 361         }
 362       }
 363       zu[0] = beta;
 364       alpha[0] = diag[0] - beta;
 365
 366       if (alpha[0] == 0) {
 367         status = GSL_EZERODIV;
 368       }
 369
 370       {
 371         size_t i;
 372         for (i = 1; i+1 < N; i++)
 373         {
 374           const double t = belowdiag[b_stride*(i - 1)]/alpha[i-1];
 375           alpha[i] = diag[d_stride*i] - t*abovediag[a_stride*(i - 1)];
 376           zb[i] = rhs[r_stride*i] - t*zb[i-1];
 377           zu[i] = -t*zu[i-1];
 378           /* FIXME!!! */
 379           if (alpha[i] == 0) {
 380             status = GSL_EZERODIV;
 381           }
 382         }
 383       }
 384
 385       {
 386         const size_t i = N-1;
 387         const double t = belowdiag[b_stride*(i - 1)]/alpha[i-1];
 388         alpha[i] = diag[d_stride*i]
 389                    - abovediag[a_stride*i]*belowdiag[b_stride*i]/beta
 390                    - t*abovediag[a_stride*(i - 1)];
 391         zb[i] = rhs[r_stride*i] - t*zb[i-1];
 392         zu[i] = abovediag[a_stride*i] - t*zu[i-1];
 393         /* FIXME!!! */
 394         if (alpha[i] == 0) {
 395           status = GSL_EZERODIV;
 396         }
 397       }
 398
 399       /* backsubstitution */
 400       {
 401         size_t i, j;
 402         w[N-1] = zu[N-1]/alpha[N-1];
 403         x[N-1] = zb[N-1]/alpha[N-1];
 404         for (i = N - 2, j = 0; j <= N - 2; j++, i--)
 405           {
 406             w[i] = (zu[i] - abovediag[a_stride*i] * w[i+1])/alpha[i];
 407             x[i*x_stride] = (zb[i] - abovediag[a_stride*i] * x[x_stride*(i + 1)])/alpha[i];
 408           }
 409       }
 410
 411       /* Sherman-Morrison */
 412       {
 413         const double vw = w[0] + belowdiag[b_stride*(N - 1)]/beta * w[N-1];
 414         const double vx = x[0] + belowdiag[b_stride*(N - 1)]/beta * x[x_stride*(N - 1)];
 415         /* FIXME!!! */
 416         if (vw + 1 == 0) {
 417           status = GSL_EZERODIV;
 418         }
 419
 420         {
 421           size_t i;
 422           for (i = 0; i < N; i++)
 423             x[i] -= vx/(1 + vw)*w[i];
 424         }
 425       }
 426     }
 427
 428   if (zb != 0)
 429     free (zb);
 430   if (zu != 0)
 431     free (zu);
 432   if (w != 0)
 433     free (w);
 434   if (alpha != 0)
 435     free (alpha);
 436
 437   if (status == GSL_EZERODIV) {
 438     GSL_ERROR ("matrix must be positive definite", status);
 439   }
 440
 441   return status;
 442 }
 443
 444 int
 445 gsl_linalg_solve_symm_tridiag(
 446   const gsl_vector * diag,
 447   const gsl_vector * offdiag,
 448   const gsl_vector * rhs,
 449   gsl_vector * solution)
 450 {
 451   if(diag->size != rhs->size)
 452     {
 453       GSL_ERROR ("size of diag must match rhs", GSL_EBADLEN);
 454     }
 455   else if (offdiag->size != rhs->size-1)
 456     {
 457       GSL_ERROR ("size of offdiag must match rhs-1", GSL_EBADLEN);
 458     }
 459   else if (solution->size != rhs->size)
 460     {
 461       GSL_ERROR ("size of solution must match rhs", GSL_EBADLEN);
 462     }
 463   else
 464     {
 465       return solve_tridiag(diag->data, diag->stride,
 466                            offdiag->data, offdiag->stride,
 467                            rhs->data, rhs->stride,
 468                            solution->data, solution->stride,
 469                            diag->size);
 470
 471     }
 472 }
 473
 474 int
 475 gsl_linalg_solve_tridiag(
 476   const gsl_vector * diag,
 477   const gsl_vector * abovediag,
 478   const gsl_vector * belowdiag,
 479   const gsl_vector * rhs,
 480   gsl_vector * solution)
 481 {
 482   if(diag->size != rhs->size)
 483     {
 484       GSL_ERROR ("size of diag must match rhs", GSL_EBADLEN);
 485     }
 486   else if (abovediag->size != rhs->size-1)
 487     {
 488       GSL_ERROR ("size of abovediag must match rhs-1", GSL_EBADLEN);
 489     }
 490   else if (belowdiag->size != rhs->size-1)
 491     {
 492       GSL_ERROR ("size of belowdiag must match rhs-1", GSL_EBADLEN);
 493     }
 494   else if (solution->size != rhs->size)
 495     {
 496       GSL_ERROR ("size of solution must match rhs", GSL_EBADLEN);
 497     }
 498   else
 499     {
 500       return solve_tridiag_nonsym(diag->data, diag->stride,
 501                                   abovediag->data, abovediag->stride,
 502                                   belowdiag->data, belowdiag->stride,
 503                                   rhs->data, rhs->stride,
 504                                   solution->data, solution->stride,
 505                                   diag->size);
 506     }
 507 }
 508
 509
 510 int
 511 gsl_linalg_solve_symm_cyc_tridiag(
 512   const gsl_vector * diag,
 513   const gsl_vector * offdiag,
 514   const gsl_vector * rhs,
 515   gsl_vector * solution)
 516 {
 517   if(diag->size != rhs->size)
 518     {
 519       GSL_ERROR ("size of diag must match rhs", GSL_EBADLEN);
 520     }
 521   else if (offdiag->size != rhs->size)
 522     {
 523       GSL_ERROR ("size of offdiag must match rhs", GSL_EBADLEN);
 524     }
 525   else if (solution->size != rhs->size)
 526     {
 527       GSL_ERROR ("size of solution must match rhs", GSL_EBADLEN);
 528     }
 529   else if (diag->size < 3)
 530     {
 531       GSL_ERROR ("size of cyclic system must be 3 or more", GSL_EBADLEN);
 532     }
 533   else
 534     {
 535       return solve_cyc_tridiag(diag->data, diag->stride,
 536                                offdiag->data, offdiag->stride,
 537                                rhs->data, rhs->stride,
 538                                solution->data, solution->stride,
 539                                diag->size);
 540     }
 541 }
 542
 543 int
 544 gsl_linalg_solve_cyc_tridiag(
 545   const gsl_vector * diag,
 546   const gsl_vector * abovediag,
 547   const gsl_vector * belowdiag,
 548   const gsl_vector * rhs,
 549   gsl_vector * solution)
 550 {
 551   if(diag->size != rhs->size)
 552     {
 553       GSL_ERROR ("size of diag must match rhs", GSL_EBADLEN);
 554     }
 555   else if (abovediag->size != rhs->size)
 556     {
 557       GSL_ERROR ("size of abovediag must match rhs", GSL_EBADLEN);
 558     }
 559   else if (belowdiag->size != rhs->size)
 560     {
 561       GSL_ERROR ("size of belowdiag must match rhs", GSL_EBADLEN);
 562     }
 563   else if (solution->size != rhs->size)
 564     {
 565       GSL_ERROR ("size of solution must match rhs", GSL_EBADLEN);
 566     }
 567   else if (diag->size < 3)
 568     {
 569       GSL_ERROR ("size of cyclic system must be 3 or more", GSL_EBADLEN);
 570     }
 571   else
 572     {
 573       return solve_cyc_tridiag_nonsym(diag->data, diag->stride,
 574                                       abovediag->data, abovediag->stride,
 575                                       belowdiag->data, belowdiag->stride,
 576                                       rhs->data, rhs->stride,
 577                                       solution->data, solution->stride,
 578                                       diag->size);
 579     }
 580 }