750 lines
17 KiB
C
750 lines
17 KiB
C
|
|
/*
|
|
* 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[100];
|
|
double anm;
|
|
int i, j, k;
|
|
int its;
|
|
|
|
if (n <= 100)
|
|
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;
|
|
}
|
|
}
|
|
|
|
/* --------------------------- */
|
|
#ifndef NEVER /* [und] Use debug version */
|
|
|
|
/* 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[100]; /* Intermediate value of B . U-1 . W-1 */
|
|
|
|
if (n <= 100)
|
|
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 = 0.0;
|
|
i = 0;
|
|
for (; i < (m-7); i += 8) {
|
|
s += b[i+0] * u[i+0][j];
|
|
s += b[i+1] * u[i+1][j];
|
|
s += b[i+2] * u[i+2][j];
|
|
s += b[i+3] * u[i+3][j];
|
|
s += b[i+4] * u[i+4][j];
|
|
s += b[i+5] * u[i+5][j];
|
|
s += b[i+6] * u[i+6][j];
|
|
s += b[i+7] * u[i+7][j];
|
|
}
|
|
for (; 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 = 0.0;
|
|
i = 0;
|
|
for (; i < (n-7); i += 8) {
|
|
s += v[j][i+0] * tmp[i+0];
|
|
s += v[j][i+1] * tmp[i+1];
|
|
s += v[j][i+2] * tmp[i+2];
|
|
s += v[j][i+3] * tmp[i+3];
|
|
s += v[j][i+4] * tmp[i+4];
|
|
s += v[j][i+5] * tmp[i+5];
|
|
s += v[j][i+6] * tmp[i+6];
|
|
s += v[j][i+7] * tmp[i+7];
|
|
}
|
|
for (; i < n; i++)
|
|
s += v[j][i] * tmp[i];
|
|
x[j] = s;
|
|
}
|
|
if (tmp != TMP)
|
|
free_dvector(tmp, 0, n-1);
|
|
return 0;
|
|
}
|
|
|
|
#else /* Diagnostic version */
|
|
|
|
/* 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[100]; /* Intermediate value of B . U-1 . W-1 */
|
|
|
|
printf("svdbacksub diag:\n");
|
|
|
|
if (n <= 100)
|
|
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 = 0.0;
|
|
i = 0;
|
|
for (; i < m; i++) {
|
|
s += b[i] * u[i][j];
|
|
printf("s += b[%d] %f * u[%d][%d] %f => %f\n", i, b[i], i, j, u[i][j], s);
|
|
}
|
|
s /= w[j];
|
|
printf("s /= w[%d] %f => %f\n", j, w[j], s);
|
|
tmp[j] = s;
|
|
} else {
|
|
tmp[j] = 0.0;
|
|
}
|
|
}
|
|
/* Compute T. V-1 */
|
|
for (j = 0; j < n; j++) {
|
|
double s = 0.0;
|
|
i = 0;
|
|
for (; i < n; i++) {
|
|
s += v[j][i] * tmp[i];
|
|
printf("s += v[%d][%d] %f * tmp[%d] %f => %f\n", j, i, v[j][i], i, tmp[i], s);
|
|
}
|
|
x[j] = s;
|
|
printf("x[%d] = %f\n", j, x[j]);
|
|
}
|
|
if (tmp != TMP)
|
|
free_dvector(tmp, 0, n-1);
|
|
|
|
printf("svdbacksub done:\n");
|
|
|
|
return 0;
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
/* --------------------------- */
|
|
/* 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);
|
|
}
|
|
}
|
|
|
|
|
|
/* --------------------------- */
|
|
|
|
/* Compute the inverse matrix Ai[[0..n-1][0..m-1] from SVD components */
|
|
void svdinverse(
|
|
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 **ia, /* iA[0..n-1][0..m-1] return inverse of A */
|
|
int m, /* Number of equations */
|
|
int n /* Number of unknowns */
|
|
) {
|
|
int i, j, k;
|
|
|
|
for (j = 0; j < n; j++) { /* Outer of result */
|
|
for (i = 0; i < m; i++) { /* Inner of result */
|
|
ia[j][i] = 0.0;
|
|
for (k = 0; k < n; k++) {
|
|
if (w[k] != 0.0)
|
|
ia[j][i] += v[j][k] * u[i][k] / w[k];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Compute x from b using inverse A matrix */
|
|
void svdmulia(
|
|
double **ia, /* iA[0..n-1][0..m-1] inverse of A */
|
|
double b[], /* B[0..m-1] Right hand side of equation */
|
|
double x[], /* X[0..n-1] Return solution. (Must be different to b[]) */
|
|
int m, /* Number of equations */
|
|
int n /* Number of unknowns */
|
|
) {
|
|
int i, j;
|
|
|
|
for (j = 0; j < n; j++) {
|
|
double s = 0.0;
|
|
i = 0.0;
|
|
for (; i < (m-7); i += 8) {
|
|
s += b[i+0] * ia[j][i+0];
|
|
s += b[i+1] * ia[j][i+1];
|
|
s += b[i+2] * ia[j][i+2];
|
|
s += b[i+3] * ia[j][i+3];
|
|
s += b[i+4] * ia[j][i+4];
|
|
s += b[i+5] * ia[j][i+5];
|
|
s += b[i+6] * ia[j][i+6];
|
|
s += b[i+7] * ia[j][i+7];
|
|
}
|
|
for (; i < m; i++)
|
|
s += b[i] * ia[j][i];
|
|
x[j] = s;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|