123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581 |
- /* linalg/tridiag.c
- *
- * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2002, 2004, 2007 Gerard Jungman, Brian Gough, David Necas
- *
- * 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 3 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 "gsl__config.h"
- #include <stdlib.h>
- #include <math.h>
- #include "gsl_errno.h"
- #include "gsl_linalg__tridiag.h"
- #include "gsl_linalg.h"
- /* for description of method see [Engeln-Mullges + Uhlig, p. 92]
- *
- * diag[0] offdiag[0] 0 .....
- * offdiag[0] diag[1] offdiag[1] .....
- * 0 offdiag[1] diag[2]
- * 0 0 offdiag[2] .....
- */
- static
- int
- solve_tridiag(
- const double diag[], size_t d_stride,
- const double offdiag[], size_t o_stride,
- const double b[], size_t b_stride,
- double x[], size_t x_stride,
- size_t N)
- {
- int status = GSL_SUCCESS;
- double *gamma = (double *) malloc (N * sizeof (double));
- double *alpha = (double *) malloc (N * sizeof (double));
- double *c = (double *) malloc (N * sizeof (double));
- double *z = (double *) malloc (N * sizeof (double));
- if (gamma == 0 || alpha == 0 || c == 0 || z == 0)
- {
- GSL_ERROR("failed to allocate working space", GSL_ENOMEM);
- }
- else
- {
- size_t i, j;
- /* Cholesky decomposition
- A = L.D.L^t
- lower_diag(L) = gamma
- diag(D) = alpha
- */
- alpha[0] = diag[0];
- gamma[0] = offdiag[0] / alpha[0];
- if (alpha[0] == 0) {
- status = GSL_EZERODIV;
- }
- for (i = 1; i < N - 1; i++)
- {
- alpha[i] = diag[d_stride * i] - offdiag[o_stride*(i - 1)] * gamma[i - 1];
- gamma[i] = offdiag[o_stride * i] / alpha[i];
- if (alpha[i] == 0) {
- status = GSL_EZERODIV;
- }
- }
- if (N > 1)
- {
- alpha[N - 1] = diag[d_stride * (N - 1)] - offdiag[o_stride*(N - 2)] * gamma[N - 2];
- }
- /* update RHS */
- z[0] = b[0];
- for (i = 1; i < N; i++)
- {
- z[i] = b[b_stride * i] - gamma[i - 1] * z[i - 1];
- }
- for (i = 0; i < N; i++)
- {
- c[i] = z[i] / alpha[i];
- }
- /* backsubstitution */
- x[x_stride * (N - 1)] = c[N - 1];
- if (N >= 2)
- {
- for (i = N - 2, j = 0; j <= N - 2; j++, i--)
- {
- x[x_stride * i] = c[i] - gamma[i] * x[x_stride * (i + 1)];
- }
- }
- }
- if (z != 0)
- free (z);
- if (c != 0)
- free (c);
- if (alpha != 0)
- free (alpha);
- if (gamma != 0)
- free (gamma);
- if (status == GSL_EZERODIV) {
- GSL_ERROR ("matrix must be positive definite", status);
- }
- return status;
- }
- /* plain gauss elimination, only not bothering with the zeroes
- *
- * diag[0] abovediag[0] 0 .....
- * belowdiag[0] diag[1] abovediag[1] .....
- * 0 belowdiag[1] diag[2]
- * 0 0 belowdiag[2] .....
- */
- static
- int
- solve_tridiag_nonsym(
- const double diag[], size_t d_stride,
- const double abovediag[], size_t a_stride,
- const double belowdiag[], size_t b_stride,
- const double rhs[], size_t r_stride,
- double x[], size_t x_stride,
- size_t N)
- {
- int status = GSL_SUCCESS;
- double *alpha = (double *) malloc (N * sizeof (double));
- double *z = (double *) malloc (N * sizeof (double));
- if (alpha == 0 || z == 0)
- {
- GSL_ERROR("failed to allocate working space", GSL_ENOMEM);
- }
- else
- {
- size_t i, j;
- /* Bidiagonalization (eliminating belowdiag)
- & rhs update
- diag' = alpha
- rhs' = z
- */
- alpha[0] = diag[0];
- z[0] = rhs[0];
-
- if (alpha[0] == 0) {
- status = GSL_EZERODIV;
- }
- for (i = 1; i < N; i++)
- {
- const double t = belowdiag[b_stride*(i - 1)]/alpha[i-1];
- alpha[i] = diag[d_stride*i] - t*abovediag[a_stride*(i - 1)];
- z[i] = rhs[r_stride*i] - t*z[i-1];
- if (alpha[i] == 0) {
- status = GSL_EZERODIV;
- }
- }
- /* backsubstitution */
- x[x_stride * (N - 1)] = z[N - 1]/alpha[N - 1];
- if (N >= 2)
- {
- for (i = N - 2, j = 0; j <= N - 2; j++, i--)
- {
- x[x_stride * i] = (z[i] - abovediag[a_stride*i] * x[x_stride * (i + 1)])/alpha[i];
- }
- }
- }
- if (z != 0)
- free (z);
- if (alpha != 0)
- free (alpha);
- if (status == GSL_EZERODIV) {
- GSL_ERROR ("matrix must be positive definite", status);
- }
- return status;
- }
- /* for description of method see [Engeln-Mullges + Uhlig, p. 96]
- *
- * diag[0] offdiag[0] 0 ..... offdiag[N-1]
- * offdiag[0] diag[1] offdiag[1] .....
- * 0 offdiag[1] diag[2]
- * 0 0 offdiag[2] .....
- * ... ...
- * offdiag[N-1] ...
- *
- */
- static
- int
- solve_cyc_tridiag(
- const double diag[], size_t d_stride,
- const double offdiag[], size_t o_stride,
- const double b[], size_t b_stride,
- double x[], size_t x_stride,
- size_t N)
- {
- int status = GSL_SUCCESS;
- double * delta = (double *) malloc (N * sizeof (double));
- double * gamma = (double *) malloc (N * sizeof (double));
- double * alpha = (double *) malloc (N * sizeof (double));
- double * c = (double *) malloc (N * sizeof (double));
- double * z = (double *) malloc (N * sizeof (double));
- if (delta == 0 || gamma == 0 || alpha == 0 || c == 0 || z == 0)
- {
- GSL_ERROR("failed to allocate working space", GSL_ENOMEM);
- }
- else
- {
- size_t i, j;
- double sum = 0.0;
- /* factor */
- if (N == 1)
- {
- x[0] = b[0] / diag[0];
- return GSL_SUCCESS;
- }
- alpha[0] = diag[0];
- gamma[0] = offdiag[0] / alpha[0];
- delta[0] = offdiag[o_stride * (N-1)] / alpha[0];
- if (alpha[0] == 0) {
- status = GSL_EZERODIV;
- }
- for (i = 1; i < N - 2; i++)
- {
- alpha[i] = diag[d_stride * i] - offdiag[o_stride * (i-1)] * gamma[i - 1];
- gamma[i] = offdiag[o_stride * i] / alpha[i];
- delta[i] = -delta[i - 1] * offdiag[o_stride * (i-1)] / alpha[i];
- if (alpha[i] == 0) {
- status = GSL_EZERODIV;
- }
- }
- for (i = 0; i < N - 2; i++)
- {
- sum += alpha[i] * delta[i] * delta[i];
- }
- alpha[N - 2] = diag[d_stride * (N - 2)] - offdiag[o_stride * (N - 3)] * gamma[N - 3];
- gamma[N - 2] = (offdiag[o_stride * (N - 2)] - offdiag[o_stride * (N - 3)] * delta[N - 3]) / alpha[N - 2];
- alpha[N - 1] = diag[d_stride * (N - 1)] - sum - alpha[(N - 2)] * gamma[N - 2] * gamma[N - 2];
- /* update */
- z[0] = b[0];
- for (i = 1; i < N - 1; i++)
- {
- z[i] = b[b_stride * i] - z[i - 1] * gamma[i - 1];
- }
- sum = 0.0;
- for (i = 0; i < N - 2; i++)
- {
- sum += delta[i] * z[i];
- }
- z[N - 1] = b[b_stride * (N - 1)] - sum - gamma[N - 2] * z[N - 2];
- for (i = 0; i < N; i++)
- {
- c[i] = z[i] / alpha[i];
- }
- /* backsubstitution */
- x[x_stride * (N - 1)] = c[N - 1];
- x[x_stride * (N - 2)] = c[N - 2] - gamma[N - 2] * x[x_stride * (N - 1)];
- if (N >= 3)
- {
- for (i = N - 3, j = 0; j <= N - 3; j++, i--)
- {
- x[x_stride * i] = c[i] - gamma[i] * x[x_stride * (i + 1)] - delta[i] * x[x_stride * (N - 1)];
- }
- }
- }
- if (z != 0)
- free (z);
- if (c != 0)
- free (c);
- if (alpha != 0)
- free (alpha);
- if (gamma != 0)
- free (gamma);
- if (delta != 0)
- free (delta);
- if (status == GSL_EZERODIV) {
- GSL_ERROR ("matrix must be positive definite", status);
- }
- return status;
- }
- /* solve following system w/o the corner elements and then use
- * Sherman-Morrison formula to compensate for them
- *
- * diag[0] abovediag[0] 0 ..... belowdiag[N-1]
- * belowdiag[0] diag[1] abovediag[1] .....
- * 0 belowdiag[1] diag[2]
- * 0 0 belowdiag[2] .....
- * ... ...
- * abovediag[N-1] ...
- */
- static
- int solve_cyc_tridiag_nonsym(
- const double diag[], size_t d_stride,
- const double abovediag[], size_t a_stride,
- const double belowdiag[], size_t b_stride,
- const double rhs[], size_t r_stride,
- double x[], size_t x_stride,
- size_t N)
- {
- int status = GSL_SUCCESS;
- double *alpha = (double *) malloc (N * sizeof (double));
- double *zb = (double *) malloc (N * sizeof (double));
- double *zu = (double *) malloc (N * sizeof (double));
- double *w = (double *) malloc (N * sizeof (double));
- if (alpha == 0 || zb == 0 || zu == 0 || w == 0)
- {
- GSL_ERROR("failed to allocate working space", GSL_ENOMEM);
- }
- else
- {
- double beta;
- /* Bidiagonalization (eliminating belowdiag)
- & rhs update
- diag' = alpha
- rhs' = zb
- rhs' for Aq=u is zu
- */
- zb[0] = rhs[0];
- if (diag[0] != 0) beta = -diag[0]; else beta = 1;
- {
- const double q = 1 - abovediag[0]*belowdiag[0]/(diag[0]*diag[d_stride]);
- if (fabs(q/beta) > 0.5 && fabs(q/beta) < 2) {
- beta *= (fabs(q/beta) < 1) ? 0.5 : 2;
- }
- }
- zu[0] = beta;
- alpha[0] = diag[0] - beta;
- if (alpha[0] == 0) {
- status = GSL_EZERODIV;
- }
- {
- size_t i;
- for (i = 1; i+1 < N; i++)
- {
- const double t = belowdiag[b_stride*(i - 1)]/alpha[i-1];
- alpha[i] = diag[d_stride*i] - t*abovediag[a_stride*(i - 1)];
- zb[i] = rhs[r_stride*i] - t*zb[i-1];
- zu[i] = -t*zu[i-1];
- /* FIXME!!! */
- if (alpha[i] == 0) {
- status = GSL_EZERODIV;
- }
- }
- }
- {
- const size_t i = N-1;
- const double t = belowdiag[b_stride*(i - 1)]/alpha[i-1];
- alpha[i] = diag[d_stride*i]
- - abovediag[a_stride*i]*belowdiag[b_stride*i]/beta
- - t*abovediag[a_stride*(i - 1)];
- zb[i] = rhs[r_stride*i] - t*zb[i-1];
- zu[i] = abovediag[a_stride*i] - t*zu[i-1];
- /* FIXME!!! */
- if (alpha[i] == 0) {
- status = GSL_EZERODIV;
- }
- }
- /* backsubstitution */
- {
- size_t i, j;
- w[N-1] = zu[N-1]/alpha[N-1];
- x[N-1] = zb[N-1]/alpha[N-1];
- for (i = N - 2, j = 0; j <= N - 2; j++, i--)
- {
- w[i] = (zu[i] - abovediag[a_stride*i] * w[i+1])/alpha[i];
- x[i*x_stride] = (zb[i] - abovediag[a_stride*i] * x[x_stride*(i + 1)])/alpha[i];
- }
- }
-
- /* Sherman-Morrison */
- {
- const double vw = w[0] + belowdiag[b_stride*(N - 1)]/beta * w[N-1];
- const double vx = x[0] + belowdiag[b_stride*(N - 1)]/beta * x[x_stride*(N - 1)];
- /* FIXME!!! */
- if (vw + 1 == 0) {
- status = GSL_EZERODIV;
- }
- {
- size_t i;
- for (i = 0; i < N; i++)
- x[i] -= vx/(1 + vw)*w[i];
- }
- }
- }
- if (zb != 0)
- free (zb);
- if (zu != 0)
- free (zu);
- if (w != 0)
- free (w);
- if (alpha != 0)
- free (alpha);
- if (status == GSL_EZERODIV) {
- GSL_ERROR ("matrix must be positive definite", status);
- }
- return status;
- }
- int
- gsl_linalg_solve_symm_tridiag(
- const gsl_vector * diag,
- const gsl_vector * offdiag,
- const gsl_vector * rhs,
- gsl_vector * solution)
- {
- if(diag->size != rhs->size)
- {
- GSL_ERROR ("size of diag must match rhs", GSL_EBADLEN);
- }
- else if (offdiag->size != rhs->size-1)
- {
- GSL_ERROR ("size of offdiag must match rhs-1", GSL_EBADLEN);
- }
- else if (solution->size != rhs->size)
- {
- GSL_ERROR ("size of solution must match rhs", GSL_EBADLEN);
- }
- else
- {
- return solve_tridiag(diag->data, diag->stride,
- offdiag->data, offdiag->stride,
- rhs->data, rhs->stride,
- solution->data, solution->stride,
- diag->size);
- }
- }
- int
- gsl_linalg_solve_tridiag(
- const gsl_vector * diag,
- const gsl_vector * abovediag,
- const gsl_vector * belowdiag,
- const gsl_vector * rhs,
- gsl_vector * solution)
- {
- if(diag->size != rhs->size)
- {
- GSL_ERROR ("size of diag must match rhs", GSL_EBADLEN);
- }
- else if (abovediag->size != rhs->size-1)
- {
- GSL_ERROR ("size of abovediag must match rhs-1", GSL_EBADLEN);
- }
- else if (belowdiag->size != rhs->size-1)
- {
- GSL_ERROR ("size of belowdiag must match rhs-1", GSL_EBADLEN);
- }
- else if (solution->size != rhs->size)
- {
- GSL_ERROR ("size of solution must match rhs", GSL_EBADLEN);
- }
- else
- {
- return solve_tridiag_nonsym(diag->data, diag->stride,
- abovediag->data, abovediag->stride,
- belowdiag->data, belowdiag->stride,
- rhs->data, rhs->stride,
- solution->data, solution->stride,
- diag->size);
- }
- }
- int
- gsl_linalg_solve_symm_cyc_tridiag(
- const gsl_vector * diag,
- const gsl_vector * offdiag,
- const gsl_vector * rhs,
- gsl_vector * solution)
- {
- if(diag->size != rhs->size)
- {
- GSL_ERROR ("size of diag must match rhs", GSL_EBADLEN);
- }
- else if (offdiag->size != rhs->size)
- {
- GSL_ERROR ("size of offdiag must match rhs", GSL_EBADLEN);
- }
- else if (solution->size != rhs->size)
- {
- GSL_ERROR ("size of solution must match rhs", GSL_EBADLEN);
- }
- else if (diag->size < 3)
- {
- GSL_ERROR ("size of cyclic system must be 3 or more", GSL_EBADLEN);
- }
- else
- {
- return solve_cyc_tridiag(diag->data, diag->stride,
- offdiag->data, offdiag->stride,
- rhs->data, rhs->stride,
- solution->data, solution->stride,
- diag->size);
- }
- }
- int
- gsl_linalg_solve_cyc_tridiag(
- const gsl_vector * diag,
- const gsl_vector * abovediag,
- const gsl_vector * belowdiag,
- const gsl_vector * rhs,
- gsl_vector * solution)
- {
- if(diag->size != rhs->size)
- {
- GSL_ERROR ("size of diag must match rhs", GSL_EBADLEN);
- }
- else if (abovediag->size != rhs->size)
- {
- GSL_ERROR ("size of abovediag must match rhs", GSL_EBADLEN);
- }
- else if (belowdiag->size != rhs->size)
- {
- GSL_ERROR ("size of belowdiag must match rhs", GSL_EBADLEN);
- }
- else if (solution->size != rhs->size)
- {
- GSL_ERROR ("size of solution must match rhs", GSL_EBADLEN);
- }
- else if (diag->size < 3)
- {
- GSL_ERROR ("size of cyclic system must be 3 or more", GSL_EBADLEN);
- }
- else
- {
- return solve_cyc_tridiag_nonsym(diag->data, diag->stride,
- abovediag->data, abovediag->stride,
- belowdiag->data, belowdiag->stride,
- rhs->data, rhs->stride,
- solution->data, solution->stride,
- diag->size);
- }
- }
|