summaryrefslogtreecommitdiff
path: root/numlib/svd.c
diff options
context:
space:
mode:
Diffstat (limited to 'numlib/svd.c')
-rw-r--r--numlib/svd.c611
1 files changed, 611 insertions, 0 deletions
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);
+ }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
generated by cgit v1.2.3 (git 2.25.1) at 2024-04-18 23:53:37 +0000