diff options
author | Jörg Frings-Fürst <debian@jff-webhosting.net> | 2014-09-01 13:56:46 +0200 |
---|---|---|
committer | Jörg Frings-Fürst <debian@jff-webhosting.net> | 2014-09-01 13:56:46 +0200 |
commit | 22f703cab05b7cd368f4de9e03991b7664dc5022 (patch) | |
tree | 6f4d50beaa42328e24b1c6b56b6ec059e4ef21a5 /numlib/ludecomp.c |
Initial import of argyll version 1.5.1-8debian/1.5.1-8
Diffstat (limited to 'numlib/ludecomp.c')
-rw-r--r-- | numlib/ludecomp.c | 500 |
1 files changed, 500 insertions, 0 deletions
diff --git a/numlib/ludecomp.c b/numlib/ludecomp.c new file mode 100644 index 0000000..72e014a --- /dev/null +++ b/numlib/ludecomp.c @@ -0,0 +1,500 @@ +/***************************************************/ +/* Linear Simultaeous equation solver */ +/***************************************************/ + +/* General simultaneous equation solver. */ +/* Code was inspired by the algorithm decsribed in section 2.3 */ +/* of "Numerical Recipes in C", by W.H.Press, B.P.Flannery, */ +/* S.A.Teukolsky & W.T.Vetterling. */ + +/* + * Copyright 2000 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#include "numsup.h" +#include "ludecomp.h" + +#undef DO_POLISH +#undef DO_CHECK + +/* Solve the simultaneous linear equations A.X = B */ +/* Return 1 if the matrix is singular, 0 if OK */ +int +solve_se( +double **a, /* A[][] input matrix, returns LU decomposition of A */ +double *b, /* B[] input array, returns solution X[] */ +int n /* Dimensionality */ +) { + double rip; /* Row interchange parity */ + int *pivx, PIVX[10]; +#if defined(DO_POLISH) || defined(DO_CHECK) + double **sa; /* save input matrix values */ + double *sb; /* save input vector values */ + int i, j; +#endif + + if (n <= 10) + pivx = PIVX; + else + pivx = ivector(0, n-1); + +#if defined(DO_POLISH) || defined(DO_CHECK) + sa = dmatrix(0, n-1, 0, n-1); + sb = dvector(0, n-1); + + /* Copy input matrix and vector values */ + for (i = 0; i < n; i++) { + sb[i] = b[i]; + for (j = 0; j < n; j++) + sa[i][j] = a[i][j]; + } +#endif + + if (lu_decomp(a, n, pivx, &rip)) { +#if defined(DO_POLISH) || defined(DO_CHECK) + free_dvector(sb, 0, n-1); + free_dmatrix(sa, 0, n-1, 0, n-1); +#endif + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + return 1; + } + + lu_backsub(a, n, pivx, b); + +#ifdef DO_POLISH + lu_polish(sa, a, n, sb, b, pivx); /* Improve the solution */ +#endif + +#ifdef DO_CHECK + /* Check that the solution is correct */ + for (i = 0; i < n; i++) { + double sum, temp; + sum = 0.0; + for (j = 0; j < n; j++) + sum += sa[i][j] * b[j]; + temp = fabs(sum - sb[i]); + if (temp > 1e-6) { + free_dvector(sb, 0, n-1); + free_dmatrix(sa, 0, n-1, 0, n-1); + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + return 2; + } + } +#endif +#if defined(DO_POLISH) || defined(DO_CHECK) + free_dvector(sb, 0, n-1); + free_dmatrix(sa, 0, n-1, 0, n-1); +#endif + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + return 0; +} + +/* Solve the simultaneous linear equations A.X = B, with polishing */ +/* Return 1 if the matrix is singular, 0 if OK */ +int +polished_solve_se( +double **a, /* A[][] input matrix, returns LU decomposition of A */ +double *b, /* B[] input array, returns solution X[] */ +int n /* Dimensionality */ +) { + double rip; /* Row interchange parity */ + int *pivx, PIVX[10]; + double **sa; /* save input matrix values */ + double *sb; /* save input vector values */ + int i, j; + + if (n <= 10) + pivx = PIVX; + else + pivx = ivector(0, n-1); + + sa = dmatrix(0, n-1, 0, n-1); + sb = dvector(0, n-1); + + /* Copy source input matrix and vector values */ + for (i = 0; i < n; i++) { + sb[i] = b[i]; + for (j = 0; j < n; j++) + sa[i][j] = a[i][j]; + } + + if (lu_decomp(a, n, pivx, &rip)) { + free_dvector(sb, 0, n-1); + free_dmatrix(sa, 0, n-1, 0, n-1); + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + return 1; + } + + lu_backsub(a, n, pivx, b); + + lu_polish(sa, a, n, sb, b, pivx); /* Improve the solution */ + +#ifdef DO_CHECK + /* Check that the solution is correct */ + for (i = 0; i < n; i++) { + double sum, temp; + sum = 0.0; + for (j = 0; j < n; j++) + sum += sa[i][j] * b[j]; + temp = fabs(sum - sb[i]); + if (temp > 1e-6) { + free_dvector(sb, 0, n-1); + free_dmatrix(sa, 0, n-1, 0, n-1); + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + return 2; + } + } +#endif + free_dvector(sb, 0, n-1); + free_dmatrix(sa, 0, n-1, 0, n-1); + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + return 0; +} + +/* Decompose the square matrix A[][] into lower and upper triangles */ +/* Return 1 if the matrix is singular. */ +int +lu_decomp( +double **a, /* A input array, output upper and lower triangles. */ +int n, /* Dimensionality */ +int *pivx, /* Return pivoting row permutations record */ +double *rip /* Row interchange parity, +/- 1.0, used for determinant */ +) { + int i, j; + double *rscale, RSCALE[10]; /* Implicit scaling of each row */ + + if (n <= 10) + rscale = RSCALE; + else + rscale = dvector(0, n-1); + + /* For each row */ + for (i = 0; i < n; i++) { + double big; + /* For each column in row */ + for (big = 0.0, j=0; j < n; j++) { + double temp; + temp = fabs(a[i][j]); + if (temp > big) + big = temp; + } + if (fabs(big) <= DBL_MIN) { + if (rscale != RSCALE) + free_dvector(rscale, 0, n-1); + return 1; /* singular matrix */ + } + rscale[i] = 1.0/big; /* Save the scaling */ + } + + /* For each column (Crout's method) */ + for (*rip = 1.0, j = 0; j < n; j++) { + double big; + int k, bigi = 0; + + /* For each row */ + for (i = 0; i < j; i++) { + double sum; + sum = a[i][j]; + for (k = 0; k < i; k++) + sum -= a[i][k] * a[k][j]; + a[i][j] = sum; + } + + /* Find largest pivot element */ + for (big = 0.0, i = j; i < n; i++) { + double sum, temp; + + sum = a[i][j]; + for (k = 0; k < j; k++) + sum -= a[i][k] * a[k][j]; + a[i][j] = sum; + + temp = rscale[i] * fabs(sum); /* Figure of merit */ + if (temp >= big) { + big = temp; /* Best so far */ + bigi = i; /* Remember index */ + } + } + + /* If we need to interchange rows */ + if (j != bigi) { + { /* Take advantage of matrix storage to swap pointers to rows */ + double *temp; + temp = a[bigi]; + a[bigi] = a[j]; + a[j] = temp; + } + *rip = -(*rip); /* Another row interchange */ + rscale[bigi] = rscale[j]; /* Interchange scale factor */ + } + + pivx[j] = bigi; /* Record pivot */ + if (fabs(a[j][j]) <= DBL_MIN) { + if (rscale != RSCALE) + free_dvector(rscale, 0, n-1); + return 1; /* Pivot element is zero, so matrix is singular */ + } + + /* Divide by the pivot element */ + if (j != (n-1)) { + double temp; + temp = 1.0/a[j][j]; + for (i = j+1; i < n; i++) + a[i][j] *= temp; + } + } + if (rscale != RSCALE) + free_dvector(rscale, 0, n-1); + return 0; +} + +/* Solve a set of simultaneous equations from the */ +/* LU decomposition, by back substitution. */ +void +lu_backsub( +double **a, /* A[][] LU decomposed matrix */ +int n, /* Dimensionality */ +int *pivx, /* Pivoting row permutations record */ +double *b /* Input B[] vecor, return X[] */ +) { + int i, j; + int nvi; /* When >= 0, indicates non-vanishing B[] index */ + + /* Forward substitution, undo pivoting on the fly */ + for (nvi = -1, i = 0; i < n; i++) { + int px; + double sum; + + px = pivx[i]; + sum = b[px]; + b[px] = b[i]; + if (nvi >= 0) { + for (j = nvi; j < i; j++) + sum -= a[i][j] * b[j]; + } else { + if (sum != 0.0) + nvi = i; /* Found non-vanishing element */ + } + b[i] = sum; + } + + /* Back substitution */ + for (i = (n-1); i >= 0; i--) { + double sum; + sum = b[i]; + for (j = i+1; j < n; j++) + sum -= a[i][j] * b[j]; + b[i] = sum/a[i][i]; + } +} + + +/* Improve a solution of equations */ +void +lu_polish( +double **a, /* Original A[][] matrix */ +double **lua, /* LU decomposition of A[][] */ +int n, /* Dimensionality */ +double *b, /* B[] vector of equation */ +double *x, /* X[] solution to be polished */ +int *pivx /* Pivoting row permutations record */ +) { + int i, j; + double *r, R[10]; /* Residuals */ + + if (n <= 10) + r = R; + else + r = dvector(0, n-1); + + /* Accumulate the residual error */ + for (i = 0; i < n; i++) { + double sum; + sum = -b[i]; + for (j = 0; j < n; j++) + sum += a[i][j] * x[j]; + r[i] = sum; + } + + /* Solve for the error */ + lu_backsub(lua, n, pivx, r); + + /* Subtract error from the old solution */ + for (i = 0; i < n; i++) + x[i] -= r[i]; + + if (r != R) + free_dvector(r, 0, n-1); +} + + +/* Invert a matrix A using lu decomposition */ +/* Return 1 if the matrix is singular, 0 if OK */ +int +lu_invert( +double **a, /* A[][] input matrix, returns inversion of A */ +int n /* Dimensionality */ +) { + int i, j; + double rip; /* Row interchange parity */ + int *pivx, PIVX[10]; + double **y; + + if (n <= 10) + pivx = PIVX; + else + pivx = ivector(0, n-1); + + if (lu_decomp(a, n, pivx, &rip)) { + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + return 1; + } + + /* Copy lu decomp. to y[][] */ + y = dmatrix(0, n-1, 0, n-1); + for (i = 0; i < n; i++) { + for (j = 0; j < n; j++) { + y[i][j] = a[i][j]; + } + } + + /* Find inverse by columns */ + for (i = 0; i < n; i++) { + for (j = 0; j < n; j++) + a[i][j] = 0.0; + a[i][i] = 1.0; + lu_backsub(y, n, pivx, a[i]); + } + + /* Clean up */ + free_dmatrix(y, 0, n-1, 0, n-1); + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + + return 0; +} + +#ifdef NEVER /* It's not clear that this is correct */ +int +lu_polished_invert( +double **a, /* A[][] input matrix, returns inversion of A */ +int n /* Dimensionality */ +) { + int i, j, k; + double **aa; /* saved a */ + double **t1, **t2; + + aa = dmatrix(0, n-1, 0, n-1); + t1 = dmatrix(0, n-1, 0, n-1); + t2 = dmatrix(0, n-1, 0, n-1); + + /* Copy a to aa */ + for (i = 0; i < n; i++) { + for (j = 0; j < n; j++) + aa[i][j] = a[i][j]; + } + + /* Invert a */ + if ((i = lu_invert(a, n)) != 0) { + free_dmatrix(aa, 0, n-1, 0, n-1); + free_dmatrix(t1, 0, n-1, 0, n-1); + free_dmatrix(t2, 0, n-1, 0, n-1); + return i; + } + + for (k = 0; k < 10; k++) { + matrix_mult(t1, n, n, aa, n, n, a, n, n); + for (i = 0; i < n; i++) { + for (j = 0; j < n; j++) { + t2[i][j] = a[i][j]; + if (i == j) + t1[i][j] = 2.0 - t1[i][j]; + else + t1[i][j] = 0.0 - t1[i][j]; + } + } + matrix_mult(a, n, n, t2, n, n, t1, n, n); + } + + free_dmatrix(aa, 0, n-1, 0, n-1); + free_dmatrix(t1, 0, n-1, 0, n-1); + free_dmatrix(t2, 0, n-1, 0, n-1); + return 0; +} +#endif + +/* Pseudo-Invert matrix A using lu decomposition */ +/* Return 1 if the matrix is singular, 0 if OK */ +int +lu_psinvert( +double **out, /* Output[0..N-1][0..M-1] */ +double **in, /* Input[0..M-1][0..N-1] input matrix */ +int m, /* Rows */ +int n /* Columns */ +) { + int rv = 0; + double **tr; /* Transpose */ + double **sq; /* Square matrix */ + + tr = dmatrix(0, n-1, 0, m-1); + matrix_trans(tr, in, m, n); + + /* Use left inverse */ + if (m > n) { + sq = dmatrix(0, n-1, 0, n-1); + + /* Multiply transpose by input */ + if ((rv = matrix_mult(sq, n, n, tr, n, m, in, m, n)) == 0) { + + /* Invert the square matrix */ + if ((rv = lu_invert(sq, n)) == 0) { + + /* Multiply inverted square by transpose */ + rv = matrix_mult(out, n, m, sq, n, n, tr, n, m); + } + } + free_dmatrix(sq, 0, n-1, 0, n-1); + + /* Use right inverse */ + } else { + sq = dmatrix(0, m-1, 0, m-1); + + /* Multiply input by transpose */ + if ((rv = matrix_mult(sq, m, m, in, m, n, tr, n, m)) == 0) { + + /* Invert the square matrix */ + if ((rv = lu_invert(sq, m)) == 0) { + + /* Multiply transpose by inverted square */ + rv = matrix_mult(out, n, m, tr, n, m, sq, m, m); + } + } + free_dmatrix(sq, 0, m-1, 0, m-1); + } + + free_dmatrix(tr, 0, n-1, 0, m-1); + + return rv; +} + + + + + + + + + + + |