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

   1 /* linalg/hh.c
   2  *
   3  * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Gerard Jungman, 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 /* Author:  G. Jungman */
  21
  22 #include <config.h>
  23 #include <stdlib.h>
  24 #include <gsl/gsl_math.h>
  25 #include <gsl/gsl_vector.h>
  26 #include <gsl/gsl_matrix.h>
  27 #include <gsl/gsl_linalg.h>
  28
  29 #define REAL double
  30
  31 /* [Engeln-Mullges + Uhlig, Alg. 4.42]
  32  */
  33
  34 int
  35 gsl_linalg_HH_solve (gsl_matrix * A, const gsl_vector * b, gsl_vector * x)
  36 {
  37   if (A->size1 > A->size2)
  38     {
  39       /* System is underdetermined. */
  40
  41       GSL_ERROR ("System is underdetermined", GSL_EINVAL);
  42     }
  43   else if (A->size2 != x->size)
  44     {
  45       GSL_ERROR ("matrix and vector sizes must be equal", GSL_EBADLEN);
  46     }
  47   else
  48     {
  49       int status ;
  50
  51       gsl_vector_memcpy (x, b);
  52
  53       status = gsl_linalg_HH_svx (A, x);
  54
  55       return status ;
  56     }
  57 }
  58
  59 int
  60 gsl_linalg_HH_svx (gsl_matrix * A, gsl_vector * x)
  61 {
  62   if (A->size1 > A->size2)
  63     {
  64       /* System is underdetermined. */
  65
  66       GSL_ERROR ("System is underdetermined", GSL_EINVAL);
  67     }
  68   else if (A->size2 != x->size)
  69     {
  70       GSL_ERROR ("matrix and vector sizes must be equal", GSL_EBADLEN);
  71     }
  72   else
  73     {
  74       const size_t N = A->size1;
  75       const size_t M = A->size2;
  76       size_t i, j, k;
  77       REAL *d = (REAL *) malloc (N * sizeof (REAL));
  78
  79       if (d == 0)
  80         {
  81           GSL_ERROR ("could not allocate memory for workspace", GSL_ENOMEM);
  82         }
  83
  84       /* Perform Householder transformation. */
  85
  86       for (i = 0; i < N; i++)
  87         {
  88           const REAL aii = gsl_matrix_get (A, i, i);
  89           REAL alpha;
  90           REAL f;
  91           REAL ak;
  92           REAL max_norm = 0.0;
  93           REAL r = 0.0;
  94
  95           for (k = i; k < M; k++)
  96             {
  97               REAL aki = gsl_matrix_get (A, k, i);
  98               r += aki * aki;
  99             }
 100
 101           if (r == 0.0)
 102             {
 103               /* Rank of matrix is less than size1. */
 104               free (d);
 105               GSL_ERROR ("matrix is rank deficient", GSL_ESING);
 106             }
 107
 108           alpha = sqrt (r) * GSL_SIGN (aii);
 109
 110           ak = 1.0 / (r + alpha * aii);
 111           gsl_matrix_set (A, i, i, aii + alpha);
 112
 113           d[i] = -alpha;
 114
 115           for (k = i + 1; k < N; k++)
 116             {
 117               REAL norm = 0.0;
 118               f = 0.0;
 119               for (j = i; j < M; j++)
 120                 {
 121                   REAL ajk = gsl_matrix_get (A, j, k);
 122                   REAL aji = gsl_matrix_get (A, j, i);
 123                   norm += ajk * ajk;
 124                   f += ajk * aji;
 125                 }
 126               max_norm = GSL_MAX (max_norm, norm);
 127
 128               f *= ak;
 129
 130               for (j = i; j < M; j++)
 131                 {
 132                   REAL ajk = gsl_matrix_get (A, j, k);
 133                   REAL aji = gsl_matrix_get (A, j, i);
 134                   gsl_matrix_set (A, j, k, ajk - f * aji);
 135                 }
 136             }
 137
 138           if (fabs (alpha) < 2.0 * GSL_DBL_EPSILON * sqrt (max_norm))
 139             {
 140               /* Apparent singularity. */
 141               free (d);
 142               GSL_ERROR("apparent singularity detected", GSL_ESING);
 143             }
 144
 145           /* Perform update of RHS. */
 146
 147           f = 0.0;
 148           for (j = i; j < M; j++)
 149             {
 150               f += gsl_vector_get (x, j) * gsl_matrix_get (A, j, i);
 151             }
 152           f *= ak;
 153           for (j = i; j < M; j++)
 154             {
 155               REAL xj = gsl_vector_get (x, j);
 156               REAL aji = gsl_matrix_get (A, j, i);
 157               gsl_vector_set (x, j, xj - f * aji);
 158             }
 159         }
 160
 161       /* Perform back-substitution. */
 162
 163       for (i = N; i > 0 && i--;)
 164         {
 165           REAL xi = gsl_vector_get (x, i);
 166           REAL sum = 0.0;
 167           for (k = i + 1; k < N; k++)
 168             {
 169               sum += gsl_matrix_get (A, i, k) * gsl_vector_get (x, k);
 170             }
 171
 172           gsl_vector_set (x, i, (xi - sum) / d[i]);
 173         }
 174
 175       free (d);
 176       return GSL_SUCCESS;
 177     }
 178 }
 179