| /* linalg/hh.c |
| * |
| * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, Brian Gough |
| * |
| * This program is free software; you can redistribute it and/or modify |
| * it under the terms of the GNU General Public License as published by |
| * the Free Software Foundation; either version 2 of the License, or (at |
| * your option) any later version. |
| * |
| * This program is distributed in the hope that it will be useful, but |
| * WITHOUT ANY WARRANTY; without even the implied warranty of |
| * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
| * General Public License for more details. |
| * |
| * You should have received a copy of the GNU General Public License |
| * along with this program; if not, write to the Free Software |
| * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. |
| */ |
| |
| /* Author: G. Jungman */ |
| |
| #include <config.h> |
| #include <stdlib.h> |
| #include <gsl/gsl_math.h> |
| #include <gsl/gsl_vector.h> |
| #include <gsl/gsl_matrix.h> |
| #include <gsl/gsl_linalg.h> |
| |
| #define REAL double |
| |
| /* [Engeln-Mullges + Uhlig, Alg. 4.42] |
| */ |
| |
| int |
| gsl_linalg_HH_solve (gsl_matrix * A, const gsl_vector * b, gsl_vector * x) |
| { |
| if (A->size1 > A->size2) |
| { |
| /* System is underdetermined. */ |
| |
| GSL_ERROR ("System is underdetermined", GSL_EINVAL); |
| } |
| else if (A->size2 != x->size) |
| { |
| GSL_ERROR ("matrix and vector sizes must be equal", GSL_EBADLEN); |
| } |
| else |
| { |
| int status ; |
| |
| gsl_vector_memcpy (x, b); |
| |
| status = gsl_linalg_HH_svx (A, x); |
| |
| return status ; |
| } |
| } |
| |
| int |
| gsl_linalg_HH_svx (gsl_matrix * A, gsl_vector * x) |
| { |
| if (A->size1 > A->size2) |
| { |
| /* System is underdetermined. */ |
| |
| GSL_ERROR ("System is underdetermined", GSL_EINVAL); |
| } |
| else if (A->size2 != x->size) |
| { |
| GSL_ERROR ("matrix and vector sizes must be equal", GSL_EBADLEN); |
| } |
| else |
| { |
| const size_t N = A->size1; |
| const size_t M = A->size2; |
| size_t i, j, k; |
| REAL *d = (REAL *) malloc (N * sizeof (REAL)); |
| |
| if (d == 0) |
| { |
| GSL_ERROR ("could not allocate memory for workspace", GSL_ENOMEM); |
| } |
| |
| /* Perform Householder transformation. */ |
| |
| for (i = 0; i < N; i++) |
| { |
| const REAL aii = gsl_matrix_get (A, i, i); |
| REAL alpha; |
| REAL f; |
| REAL ak; |
| REAL max_norm = 0.0; |
| REAL r = 0.0; |
| |
| for (k = i; k < M; k++) |
| { |
| REAL aki = gsl_matrix_get (A, k, i); |
| r += aki * aki; |
| } |
| |
| if (r == 0.0) |
| { |
| /* Rank of matrix is less than size1. */ |
| free (d); |
| GSL_ERROR ("matrix is rank deficient", GSL_ESING); |
| } |
| |
| alpha = sqrt (r) * GSL_SIGN (aii); |
| |
| ak = 1.0 / (r + alpha * aii); |
| gsl_matrix_set (A, i, i, aii + alpha); |
| |
| d[i] = -alpha; |
| |
| for (k = i + 1; k < N; k++) |
| { |
| REAL norm = 0.0; |
| f = 0.0; |
| for (j = i; j < M; j++) |
| { |
| REAL ajk = gsl_matrix_get (A, j, k); |
| REAL aji = gsl_matrix_get (A, j, i); |
| norm += ajk * ajk; |
| f += ajk * aji; |
| } |
| max_norm = GSL_MAX (max_norm, norm); |
| |
| f *= ak; |
| |
| for (j = i; j < M; j++) |
| { |
| REAL ajk = gsl_matrix_get (A, j, k); |
| REAL aji = gsl_matrix_get (A, j, i); |
| gsl_matrix_set (A, j, k, ajk - f * aji); |
| } |
| } |
| |
| if (fabs (alpha) < 2.0 * GSL_DBL_EPSILON * sqrt (max_norm)) |
| { |
| /* Apparent singularity. */ |
| free (d); |
| GSL_ERROR("apparent singularity detected", GSL_ESING); |
| } |
| |
| /* Perform update of RHS. */ |
| |
| f = 0.0; |
| for (j = i; j < M; j++) |
| { |
| f += gsl_vector_get (x, j) * gsl_matrix_get (A, j, i); |
| } |
| f *= ak; |
| for (j = i; j < M; j++) |
| { |
| REAL xj = gsl_vector_get (x, j); |
| REAL aji = gsl_matrix_get (A, j, i); |
| gsl_vector_set (x, j, xj - f * aji); |
| } |
| } |
| |
| /* Perform back-substitution. */ |
| |
| for (i = N; i > 0 && i--;) |
| { |
| REAL xi = gsl_vector_get (x, i); |
| REAL sum = 0.0; |
| for (k = i + 1; k < N; k++) |
| { |
| sum += gsl_matrix_get (A, i, k) * gsl_vector_get (x, k); |
| } |
| |
| gsl_vector_set (x, i, (xi - sum) / d[i]); |
| } |
| |
| free (d); |
| return GSL_SUCCESS; |
| } |
| } |
| |