GRASS Programmer's Manual
6.4.2(2012)
|
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