From 22f703cab05b7cd368f4de9e03991b7664dc5022 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=B6rg=20Frings-F=C3=BCrst?= Date: Mon, 1 Sep 2014 13:56:46 +0200 Subject: Initial import of argyll version 1.5.1-8 --- numlib/svd.c | 611 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 611 insertions(+) create mode 100644 numlib/svd.c (limited to 'numlib/svd.c') diff --git a/numlib/svd.c b/numlib/svd.c new file mode 100644 index 0000000..fdb8edd --- /dev/null +++ b/numlib/svd.c @@ -0,0 +1,611 @@ + +/* + * Singular Value Decomposition, + * from the Burton S. Garbow's EISPACK FORTRAN code, + * based on the algorithm of Golub and Reinsch in + * "Handbook of Automatic Computation", + * with some guidance from R. B Davie's newmat09. + * + * 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 "svd.h" + +#ifndef NEVER +/* Compute the pythagorian distance sqrt(a^2 + b^2) */ +/* taking care of under or overflow. */ +double pythag( +double a, +double b) { + double aba, abb; + + aba = fabs(a); + abb = fabs(b); + + if (aba > abb) { + double boa; + boa = abb/aba; + return aba * sqrt(1.0 + boa * boa); + } else { + double aob; + if (abb == 0.0) + return 0.0; + aob = aba/abb; + return abb * sqrt(1.0 + aob * aob); + } +} +#else /* Quicker, but less robust */ +#define pythag(a, b) sqrt((a) * (a) + (b) * (b)) +#endif + +#define MAXITS 30 + +/* Compute Singular Value Decomposition of A = U.W.Vt */ +/* Return status value: */ +/* 0 - no error */ +/* 1 - Too many itterations */ +int svdecomp( +double **a, /* A[0..m-1][0..n-1], return U[][] */ +double *w, /* return W[0..n-1] */ +double **v, /* return V[0..n-1][0..n-1] (not transpose!) */ +int m, /* Number of equations */ +int n /* Number of unknowns */ +) { + double eps = DBL_EPSILON; /* 1.0 + eps = 1.0 */ + double tol = DBL_MIN/eps; /* Minumum +ve value/eps */ + double *rv1, RV1[10]; + double anm; + int i, j, k; + int its; + + if (n <= 10) + rv1 = RV1; /* Allocate fast off stack */ + else + rv1 = dvector(0, n-1); + + /* Housholder reduction of A to bidiagonal form */ + anm = 0.0; + rv1[0] = 0.0; /* Will always == 0.0 */ + for (i = 0; i < n; i++) { /* For each element in the diagonal of A */ + int ip1 = i + 1; + + /* Deal with lower column at i */ + w[i] = 0.0; + if (i < m) { /* If it makes sense to go from row i .. m-1 */ + double ss, ff = 0.0; + + for (ss = 0.0, k = m-1; k >= i; k--) { /* Sum of squares of column */ + ff = a[k][i]; + ss += ff * ff; + } /* Note ff = A[i][i] */ + if (ss >= tol) { + double gg, hh; + gg = sqrt(ss); + w[i] = gg = ff < 0.0 ? gg : -gg; /* gg has -sign of ff */ + hh = ff * gg - ss; + a[i][i] = ff - gg; + + /* For all lower columns to the right of this one */ + for (j = ip1; j < n; j++) { /* Column j */ + double tt; + for (ss = 0.0, k = i; k < m; k++) + ss += a[k][j] * a[k][i]; /* Sum of products of i and j columns */ + tt = ss / hh; + for (k = i; k < m; k++) + a[k][j] += tt * a[k][i]; /* Add sumprod/hh to column j */ + } + } + } + + /* Deal with upper super row at i */ + if (ip1 < n) { /* If it makes sense to go from column i+1 .. n-1 */ + rv1[ip1] = 0.0; + if (i < m) { /* If it makes sense to process row i */ + double ss, ff = 0.0; + for (ss = 0.0, k = n-1; k >= ip1; k--) { /* Sum of squares of row */ + ff = a[i][k]; + ss += ff * ff; + } /* Note ff = A[i][ip1] */ + if (ss >= tol) { + double gg, hh; + gg = sqrt(ss); + rv1[ip1] = gg = ff < 0.0 ? gg : -gg; /* gg has -sign of ff */ + hh = ff * gg - ss; + a[i][ip1] = (ff - gg); + + /* For all upper rows below this one */ + for (j = ip1; j < m; j++) { + double tt; + for (ss = 0.0, k = ip1; k < n; k++) /* Sum of products of i and j rows */ + ss += a[j][k] * a[i][k]; + tt = ss / hh; + for (k = ip1; k < n; k++) + a[j][k] += tt * a[i][k]; /* Add sumprod/hh to row j */ + } + } + } + } + { + double tt; + tt = fabs(w[i]) + fabs(rv1[i]); + if (tt > anm) + anm = tt; + } + } + + /* Accumulation of right hand transformations */ + for (i = n-1; i >= 0; i--) { + int ip1 = i + 1; + if (ip1 < n) { + double gg; + gg = rv1[ip1]; + if (gg != 0.0) { + gg = 1.0 / gg; + for (j = ip1; j < n; j++) + v[j][i] = (a[i][j] / a[i][ip1]) * gg; /* Double division to avoid underflow */ + for (j = ip1; j < n; j++) { + double ss; + for (ss = 0.0, k = ip1; k < n; k++) + ss += a[i][k] * v[k][j]; + for (k = ip1; k < n; k++) + v[k][j] += ss * v[k][i]; + } + } + for (j = ip1; j < n; j++) + v[i][j] = v[j][i] = 0.0; + } + v[i][i] = 1.0; + } + /* Accumulation of left hand transformations */ + for (i = n < m ? n-1 : m-1; i >= 0; i--) { + int ip1 = i + 1; + double gg = w[i]; + if (ip1 < n) + for (j = ip1; j < n; j++) + a[i][j] = 0.0; + if (gg == 0.0) { + for (j = i; j < m; j++) + a[j][i] = 0.0; + } else { + gg = 1.0 / gg; + if (ip1 < n) { + for (j = ip1; j < n; j++) { + double ss, ff; + for (ss = 0.0, k = ip1; k < m; k++) + ss += a[k][i] * a[k][j]; + ff = (ss / a[i][i]) * gg; /* Double division to avoid underflow */ + for (k = i; k < m; k++) + a[k][j] += ff * a[k][i]; + } + } + for (j = i; j < m; j++) + a[j][i] *= gg; + } + a[i][i] += 1.0; + } + + eps *= anm; + + /* Fully diagonalize bidiagonal result, by */ + /* successive QR rotations. */ + for (k = (n-1); k >= 0; k--) { /* For all the singular values */ + for (its = 0;; its++) { + int flag; + int lm1 = 0; + int ll; + double zz; + + /* Test for splitting */ + for (flag = 1, ll = k; ll >= 0; ll--) { + lm1 = ll - 1; + if (fabs(rv1[ll]) <= eps) { /* Note always stops at 0 because rv1[0] = 0.0 */ + flag = 0; + break; + } + if (fabs(w[lm1]) <= eps) + break; + } + if (flag != 0) { + double cc = 0.0; + double ss = 1.0; + for (i = ll; i <= k; i++) { + double ff, gg, hh; + gg = rv1[i]; + rv1[i] = cc * gg; + ff = ss * gg; + if (fabs(ff) <= eps) + break; /* Found acceptable solution */ + gg = w[i]; + w[i] = hh = pythag(ff, gg); + hh = 1.0 / hh; + cc = gg * hh; + ss = -ff * hh; + + /* Apply rotation */ + for (j = 0; j < m; j++) { + double y1, z1; + y1 = a[j][lm1]; + z1 = a[j][i]; + a[j][lm1] = y1 * cc + z1 * ss; + a[j][i] = z1 * cc - y1 * ss; + } + } + } + zz = w[k]; + if (k == ll) { /* Convergence */ + if (zz < 0.0) { + w[k] = -zz; /* Make singular value non-negative */ + for (j = 0; j < n; j++) + v[j][k] = (-v[j][k]); + } + break; + } + if (its == MAXITS) { +/* fprintf(stderr,"No convergence in %d SVDCMP iterations",MAXITS); */ + if (rv1 != RV1) + free_dvector(rv1, 0, n-1); + return 1; + } + { + double ff, gg, hh, cc, ss, xx, yy; + int km1; + + km1 = k - 1; + xx = w[ll]; + yy = w[km1]; + gg = rv1[km1]; + hh = rv1[k]; + ff = ((yy - zz) * (yy + zz) + (gg - hh) * (gg + hh)) / (2.0 * hh * yy); + gg = pythag(ff, 1.0); + gg = ff < 0.0 ? -gg : gg; + ff = ((xx - zz) * (xx + zz) + hh * ((yy / (ff + gg)) - hh)) / xx; + cc = ss = 1.0; + + for (j = ll; j <= km1; j++) { + double f2, g2, y2, h2, z2; + int jp1 = j + 1; + g2 = rv1[jp1]; + y2 = w[jp1]; + h2 = ss * g2; + g2 = cc * g2; + rv1[j] = z2 = pythag(ff, h2); + cc = ff / z2; + ss = h2 / z2; + f2 = xx * cc + g2 * ss; + g2 = g2 * cc - xx * ss; + h2 = y2 * ss; + y2 = y2 * cc; + + /* Apply rotation */ + for (i = 0; i < n; i++) { + double x1, z1; + x1 = v[i][j]; + z1 = v[i][jp1]; + v[i][j] = x1 * cc + z1 * ss; + v[i][jp1] = z1 * cc - x1 * ss; + } + w[j] = z2 = pythag(f2, h2); + if (z2 != 0.0) { /* Rotation can be arbitrary */ + z2 = 1.0 / z2; + cc = f2 * z2; + ss = h2 * z2; + } + ff = (cc * g2) + (ss * y2); + xx = (cc * y2) - (ss * g2); + + /* Apply rotation */ + for (i = 0; i < m; i++) { + double y1, z1; + y1 = a[i][j]; + z1 = a[i][jp1]; + a[i][j] = y1 * cc + z1 * ss; + a[i][jp1] = z1 * cc - y1 * ss; + } + } + rv1[ll] = 0.0; + rv1[k] = ff; + w[k] = xx; + } + } + } + if (rv1 != RV1) + free_dvector(rv1, 0, n-1); + + return 0; +} + +/* --------------------------- */ +/* Threshold the singular values W[] */ +void svdthresh( +double w[], /* Singular values */ +int n /* Number of unknowns */ +) { + int i; + double maxw; + + /* Threshold the w[] values */ + for (maxw = 0.0, i = 0; i < n; i++) { + if (w[i] > maxw) + maxw = w[i]; + } + maxw *= 1.0e-12; + for (i = 0; i < n; i++) { + if (w[i] < maxw) + w[i] = 0.0; + } +} + +/* --------------------------- */ +/* Threshold the singular values W[] to give */ +/* a specific degree of freedom. */ +void svdsetthresh( +double w[], /* Singular values */ +int n, /* Number of unknowns */ +int dof /* Expected degree of freedom */ +) { + int i, j; + + /* Set the dof smallest elements to zero */ + /* (This algorithm is simple but not quick) */ + for (j = 0; j < dof;) { + int k; + double minv = 1e38; + for (k = j = i = 0; i < n; i++) { + if (w[i] == 0.0) { + j++; + continue; + } + if (w[i] < minv) { + minv = w[i]; + k = i; + } + } + if (j < dof) /* Zero next smallest */ + w[k] = 0.0; + } +} + +/* --------------------------- */ +/* Use output of svdcmp() to solve overspecified and/or */ +/* singular equation A.x = b */ +int svdbacksub( +double **u, /* U[0..m-1][0..n-1] U, W, V SVD decomposition of A[][] */ +double *w, /* W[0..n-1] */ +double **v, /* V[0..n-1][0..n-1] (not transpose!) */ +double b[], /* B[0..m-1] Right hand side of equation */ +double x[], /* X[0..n-1] Return solution. (May be the same as b[]) */ +int m, /* Number of equations */ +int n /* Number of unknowns */ +) { + int i, j; + double *tmp, TMP[10]; /* Intermediate value of B . U-1 . W-1 */ + + if (n <= 10) + tmp = TMP; + else + tmp = dvector(0, n-1); + + /* A . X = B == U . W . Vt . X = B */ + /* and U, W, and Vt are trivialy invertable */ + + /* Compute B . U-1 . W-1 */ + for (j = 0; j < n; j++) { + if (w[j]) { + double s; + for (s = 0.0, i = 0; i < m; i++) + s += b[i] * u[i][j]; + s /= w[j]; + tmp[j] = s; + } else { + tmp[j] = 0.0; + } + } + /* Compute T. V-1 */ + for (j = 0; j < n; j++) { + double s; + for (s = 0.0, i = 0; i < n; i++) + s += v[j][i] * tmp[i]; + x[j] = s; + } + if (tmp != TMP) + free_dvector(tmp, 0, n-1); + return 0; +} + + +/* --------------------------- */ +/* Solve the equation A.x = b using SVD */ +/* (The w[] values are thresholded for best accuracy) */ +/* Return non-zero if no solution found */ +int svdsolve( +double **a, /* A[0..m-1][0..n-1] input A[][], will return U[][] */ +double b[], /* B[0..m-1] Right hand side of equation, return solution */ +int m, /* Number of equations */ +int n /* Number of unknowns */ +) { + int i; + double *w, W[8]; + double **v, *VP[8], V[8][8]; + double maxw; + + if (n <= 8) { + w = W; + VP[0] = V[0]; VP[1] = V[1]; VP[2] = V[2]; VP[3] = V[3]; + VP[4] = V[4]; VP[5] = V[5]; VP[6] = V[6]; VP[7] = V[7]; + v = VP; + } else { + w = dvector(0, n-1); + v = dmatrix(0, n-1, 0, n-1); + } + + /* Singular value decompose */ + if (svdecomp(a, w, v, m, n)) { + if (w != W) { + free_dvector(w, 0, n-1); + free_dmatrix(v, 0, n-1, 0, n-1); + } + return 1; + } + + /* Threshold the w[] values */ + for (maxw = 0.0, i = 0; i < n; i++) { + if (w[i] > maxw) + maxw = w[i]; + } + maxw *= 1.0e-12; + for (i = 0; i < n; i++) { + if (w[i] < maxw) + w[i] = 0.0; + } + + /* Back substitute to solve the equation */ + svdbacksub(a, w, v, b, b, m, n); + + if (w != W) { + free_dvector(w, 0, n-1); + free_dmatrix(v, 0, n-1, 0, n-1); + } + return 0; +} + + +/* --------------------------- */ +/* Solve the equation A.x = b using SVD */ +/* The top s out of n singular values will be used */ +/* Return non-zero if no solution found */ +int svdsolve_s( +double **a, /* A[0..m-1][0..n-1] input A[][], will return U[][] */ +double b[], /* B[0..m-1] Right hand side of equation, return solution */ +int m, /* Number of equations */ +int n, /* Number of unknowns */ +int s /* Number of singular values */ +) { + int i, j; + double *w, W[8]; + int *sw, SW[8]; + double **v, *VP[8], V[8][8]; + double maxw; + + if (n <= 8) { + w = W; + sw = SW; + VP[0] = V[0]; VP[1] = V[1]; VP[2] = V[2]; VP[3] = V[3]; + VP[4] = V[4]; VP[5] = V[5]; VP[6] = V[6]; VP[7] = V[7]; + v = VP; + } else { + w = dvector(0, n-1); + sw = ivector(0, n-1); + v = dmatrix(0, n-1, 0, n-1); + } + + /* Singular value decompose */ + if (svdecomp(a, w, v, m, n)) { + if (w != W) { + free_dvector(w, 0, n-1); + free_dmatrix(v, 0, n-1, 0, n-1); + } + return 1; + } + + /* Create sorted index of w[] */ + for (maxw = 0.0, i = 0; i < n; i++) { + sw[i] = i; + if (w[i] > maxw) + maxw = w[i]; + } + maxw *= 1.0e-12; + + /* Really dumb exchange sort.. */ + for (i = 0; i < (n-1); i++) { + for (j = i+1; j < n; j++) { + if (w[sw[i]] > w[sw[j]]) { + int tt = sw[i]; + sw[i] = sw[j]; + sw[j] = tt; + } + } + } + + /* Set the (n - s) smallest values to zero */ + s = n - s; + if (s < 0) + s = 0; + if (s > n) + s = n; + for (i = 0; i < s; i++) + w[sw[i]] = 0.0; + + /* And threshold them too */ + for (maxw = 0.0, i = 0; i < n; i++) { + if (w[i] < maxw) + w[i] = 0.0; + } + + /* Back substitute to solve the equation */ + svdbacksub(a, w, v, b, b, m, n); + + if (w != W) { + free_dvector(w, 0, n-1); + free_ivector(sw, 0, n-1); + free_dmatrix(v, 0, n-1, 0, n-1); + } + return 0; +} + + +/* --------------------------- */ +/* Solve the equation A.x = b using Direct calculation, LU or SVD as appropriate */ +/* Return non-zero if no solution found */ + +#include "ludecomp.h" + +int gen_solve_se( +double **a, /* A[0..m-1][0..n-1] input A[][], will return U[][] */ +double b[], /* B[0..m-1] Right hand side of equation, return solution */ +int m, /* Number of equations */ +int n /* Number of unknowns */ +) { + if (n == m) { + if (n == 1) { /* So simple, solve it directly */ + double tt = a[0][0]; + if (fabs(tt) <= DBL_MIN) + return 1; + b[0] = b[0]/tt; + return 0; + } else { + return solve_se(a, b, n); + } + } else { + return svdsolve(a, b, m, n); + } +} + + + + + + + + + + + + + + + + + + + + + + + + + -- cgit v1.2.3