GRASS Programmer's Manual  6.4.2(2012)
svd.c
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <grass/gis.h>
00003 #include <grass/gmath.h>
00004 
00005 static double at, bt, ct;
00006 
00007 #define PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ? \
00008     (ct=bt/at,at*sqrt(1.0+ct*ct)) : (bt ? (ct=at/bt,bt*sqrt(1.0+ct*ct)): 0.0))
00009 
00010 static double maxarg1, maxarg2;
00011 
00012 #define MAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ?\
00013         (maxarg1) : (maxarg2))
00014 #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
00015 
00016 int G_svdcmp(double **a, int m, int n, double *w, double **v)
00017 {
00018     int flag, i, its, j, jj, k, ii = 0, nm = 0;
00019     double c, f, h, s, x, y, z;
00020     double anorm = 0.0, g = 0.0, scale = 0.0;
00021     double *rv1, *G_alloc_vector();
00022 
00023 
00024     if (m < n)
00025         return -1;              /* must augment A with extra zero rows */
00026     rv1 = G_alloc_vector(n);
00027 
00028     n--;
00029     m--;
00030 
00031     for (i = 0; i <= n; i++) {
00032         ii = i + 1;
00033         rv1[i] = scale * g;
00034         g = s = scale = 0.0;
00035         if (i <= m) {
00036             for (k = i; k <= m; k++)
00037                 scale += fabs(a[k][i]);
00038             if (scale) {
00039                 for (k = i; k <= m; k++) {
00040                     a[k][i] /= scale;
00041                     s += a[k][i] * a[k][i];
00042                 }
00043                 f = a[i][i];
00044                 g = -SIGN(sqrt(s), f);
00045                 h = f * g - s;
00046                 a[i][i] = f - g;
00047                 if (i != n) {
00048                     for (j = ii; j <= n; j++) {
00049                         for (s = 0.0, k = i; k <= m; k++)
00050                             s += a[k][i] * a[k][j];
00051                         f = s / h;
00052                         for (k = i; k <= m; k++)
00053                             a[k][j] += f * a[k][i];
00054                     }
00055                 }
00056                 for (k = i; k <= m; k++)
00057                     a[k][i] *= scale;
00058             }
00059         }
00060         w[i] = scale * g;
00061         g = s = scale = 0.0;
00062         if (i <= m && i != n) {
00063             for (k = ii; k <= n; k++)
00064                 scale += fabs(a[i][k]);
00065             if (scale) {
00066                 for (k = ii; k <= n; k++) {
00067                     a[i][k] /= scale;
00068                     s += a[i][k] * a[i][k];
00069                 }
00070                 f = a[i][ii];
00071                 g = -SIGN(sqrt(s), f);
00072                 h = f * g - s;
00073                 a[i][ii] = f - g;
00074                 for (k = ii; k <= n; k++)
00075                     rv1[k] = a[i][k] / h;
00076                 if (i != m) {
00077                     for (j = ii; j <= m; j++) {
00078                         for (s = 0.0, k = ii; k <= n; k++)
00079                             s += a[j][k] * a[i][k];
00080                         for (k = ii; k <= n; k++)
00081                             a[j][k] += s * rv1[k];
00082                     }
00083                 }
00084                 for (k = ii; k <= n; k++)
00085                     a[i][k] *= scale;
00086             }
00087         }
00088         anorm = MAX(anorm, (fabs(w[i]) + fabs(rv1[i])));
00089     }
00090     for (i = n; i >= 0; i--) {
00091         if (i < n) {
00092             if (g) {
00093                 for (j = ii; j <= n; j++)
00094                     v[j][i] = (a[i][j] / a[i][ii]) / g;
00095                 for (j = ii; j <= n; j++) {
00096                     for (s = 0.0, k = ii; k <= n; k++)
00097                         s += a[i][k] * v[k][j];
00098                     for (k = ii; k <= n; k++)
00099                         v[k][j] += s * v[k][i];
00100                 }
00101             }
00102             for (j = ii; j <= n; j++)
00103                 v[i][j] = v[j][i] = 0.0;
00104         }
00105         v[i][i] = 1.0;
00106         g = rv1[i];
00107         ii = i;
00108     }
00109     for (i = n; i >= 0; i--) {
00110         ii = i + 1;
00111         g = w[i];
00112         if (i < n)
00113             for (j = ii; j <= n; j++)
00114                 a[i][j] = 0.0;
00115         if (g) {
00116             g = 1.0 / g;
00117             if (i != n) {
00118                 for (j = ii; j <= n; j++) {
00119                     for (s = 0.0, k = ii; k <= m; k++)
00120                         s += a[k][i] * a[k][j];
00121                     f = (s / a[i][i]) * g;
00122                     for (k = i; k <= m; k++)
00123                         a[k][j] += f * a[k][i];
00124                 }
00125             }
00126             for (j = i; j <= m; j++)
00127                 a[j][i] *= g;
00128         }
00129         else {
00130             for (j = i; j <= m; j++)
00131                 a[j][i] = 0.0;
00132         }
00133         ++a[i][i];
00134     }
00135     for (k = n; k >= 0; k--) {
00136         for (its = 1; its <= 30; its++) {
00137             flag = 1;
00138             for (ii = k; ii >= 0; ii--) {
00139                 nm = ii - 1;
00140                 if (fabs(rv1[ii]) + anorm == anorm) {
00141                     flag = 0;
00142                     break;
00143                 }
00144                 if (fabs(w[nm]) + anorm == anorm)
00145                     break;
00146             }
00147             if (flag) {
00148                 c = 0.0;
00149                 s = 1.0;
00150                 for (i = ii; i <= k; i++) {
00151                     f = s * rv1[i];
00152                     if (fabs(f) + anorm != anorm) {
00153                         g = w[i];
00154                         h = PYTHAG(f, g);
00155                         w[i] = h;
00156                         h = 1.0 / h;
00157                         c = g * h;
00158                         s = (-f * h);
00159                         for (j = 0; j <= m; j++) {
00160                             y = a[j][nm];
00161                             z = a[j][i];
00162                             a[j][nm] = y * c + z * s;
00163                             a[j][i] = z * c - y * s;
00164                         }
00165                     }
00166                 }
00167             }
00168             z = w[k];
00169             if (ii == k) {
00170                 if (z < 0.0) {
00171                     w[k] = -z;
00172                     for (j = 0; j <= n; j++)
00173                         v[j][k] = (-v[j][k]);
00174                 }
00175                 break;
00176             }
00177             if (its == 30)
00178                 return -2;      /*No convergence in 30 SVDCMP iterations */
00179             x = w[ii];
00180             nm = k - 1;
00181             y = w[nm];
00182             g = rv1[nm];
00183             h = rv1[k];
00184             f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
00185             g = PYTHAG(f, 1.0);
00186             f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
00187             c = s = 1.0;
00188             for (j = ii; j <= nm; j++) {
00189                 i = j + 1;
00190                 g = rv1[i];
00191                 y = w[i];
00192                 h = s * g;
00193                 g = c * g;
00194                 z = PYTHAG(f, h);
00195                 rv1[j] = z;
00196                 c = f / z;
00197                 s = h / z;
00198                 f = x * c + g * s;
00199                 g = g * c - x * s;
00200                 h = y * s;
00201                 y = y * c;
00202                 for (jj = 0; jj <= n; jj++) {
00203                     x = v[jj][j];
00204                     z = v[jj][i];
00205                     v[jj][j] = x * c + z * s;
00206                     v[jj][i] = z * c - x * s;
00207                 }
00208                 z = PYTHAG(f, h);
00209                 w[j] = z;
00210                 if (z) {
00211                     z = 1.0 / z;
00212                     c = f * z;
00213                     s = h * z;
00214                 }
00215                 f = (c * g) + (s * y);
00216                 x = (c * y) - (s * g);
00217                 for (jj = 0; jj <= m; jj++) {
00218                     y = a[jj][j];
00219                     z = a[jj][i];
00220                     a[jj][j] = y * c + z * s;
00221                     a[jj][i] = z * c - y * s;
00222                 }
00223             }
00224             rv1[ii] = 0.0;
00225             rv1[k] = f;
00226             w[k] = x;
00227         }
00228     }
00229     G_free_vector(rv1);
00230     return 0;
00231 }
00232 
00233 #undef SIGN
00234 #undef MAX
00235 #undef PYTHAG
00236 
00237 int G_svbksb(double **u, double w[], double **v,
00238              int m, int n, double b[], double x[])
00239 {
00240     int j, i;
00241     double s, *tmp, *G_alloc_vector();
00242 
00243     tmp = G_alloc_vector(n);
00244     for (j = 0; j < n; j++) {
00245         s = 0.0;
00246         if (w[j]) {
00247             for (i = 0; i < m; i++)
00248                 s += u[i][j] * b[i];
00249             s /= w[j];
00250         }
00251         tmp[j] = s;
00252     }
00253     for (j = 0; j < n; j++) {
00254         s = 0.0;
00255         for (i = 0; i < n; i++)
00256             s += v[j][i] * tmp[i];
00257         x[j] = s;
00258     }
00259     G_free_vector(tmp);
00260 
00261     return 0;
00262 }
00263 
00264 #define TOL 1e-8
00265 
00266 int G_svelim(double *w, int n)
00267 {
00268     int i;
00269     double thresh;
00270 
00271     thresh = 0.0;               /* remove singularity */
00272     for (i = 0; i < n; i++)
00273         if (w[i] > thresh)
00274             thresh = w[i];
00275     thresh *= TOL;
00276     for (i = 0; i < n; i++)
00277         if (w[i] < thresh)
00278             w[i] = 0.0;
00279 
00280     return 0;
00281 }
00282 
00283 #undef TOL
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines