1
2 /*
3 * svdcmp.c
4 *
5 * svdcmp and such from Numerical Recipes
6 *
7 */
8
9 #include "nr.h"
10 #include "math.h"
11 #include <signal.h>
12 #include <stdio.h>
13
14 #define SIG_MATH
15
16 void nr_sig(int reason)
17 {
18 nrerror("Caught FPE");
19 }
20
21 #define C(x) do { x; \
22 if (nrerror_set) { char str[256]; sprintf(str, "SIGFPE at %s:%d", __FILE__, __LINE__); nrerror(str); goto done; } \
23 } while(0)
24
25 #define sgn(x) ((x<0) ? -1 : 1)
26
27 #define SMALL_F (10e-38)
28
29 #if 0
30 #define RMULT(x,y) \
31 (((x==0)||(y==0)) ? 0 : \
32 (((ilogbf(x)+ilogbf(y)) < -125) ? (printf("***\n"),(sgn(x)*sgn(y)*SMALL_F)) : ((x)*(y))))
33 #endif
34 #define RMULT(x,y) ((x)*(y))
35
36
37 /* compute h hat matrix */
38 void compute_h(float **a, float **u, float w[], float **v, int m, int n,
39 float **h, float **tmp_nm)
40 {
41 int j, i, k;
42 float s;
43
44 nrerror_set = 0;
45
46 #ifdef SIG_MATH
47 void (* last_sig)(int);
48 last_sig = signal(SIGFPE, nr_sig);
49 #endif
50
51 for (j=1; j<=n; j++) { // Calculate V W U T.
52 for (i=1; i<=m; i++) {
53 s=0.0;
54 for (k=1;k<=n;k++) {
55 C(s += u[i][k]*v[j][k]/w[k]);
56 }
57 tmp_nm[j][i]=s;
58 }
59 }
60
61 for (i=1;i<=m;i++) { // Matrix multiply A * tmp
62 for (j=1;j<=m;j++) {
63 s=0.0;
64 for (k=1;k<=n;k++) {
65 C(s += a[j][k]*tmp_nm[k][j]);
66 }
67 h[i][j] = s;
68 }
69 }
70
71 done:
72 #ifdef SIG_MATH
73 signal(SIGFPE, last_sig);
74 #endif
75 }
76
77
78 /**
79 * Computes (a^2 + b^2)^1/2 without destructive underflow or overflow
80 *
81 */
82 float pythag(float a, float b)
83 {
84 float absa, absb;
85
86 absa = fabs(a);
87 absb = fabs(b);
88
89 if (absa > absb)
90 return absa*sqrtf(1.0 + SQR(absb/absa));
91 else
92 return (absb==0?0.0:absb*sqrtf(1.0+SQR(absa/absb)));
93 }
94
95 /*
96 * Given a matrix a[1..m][1..n], this routine computes its singular
97 * value decomposition, * A =
U W V T . The matrix U replaces
98 * a on output. The diagonal matrix of singular values W is output as
99 * a Vector w[1..n]. The matrix V (not the transpose VT) is output
100 * as v[1..n][1..n].
101 *
102 * rv1 is a 1-n temp vector
103 */
104
105 void svdcmp(float **a, int m, int n, float w[], float **v, float *rv1)
106 {
107 int flag,i,its,j,jj,k,l=0,nm=0;
108 float anorm,c,f,g,h,s,scale,x,y,z;
109
110 nrerror_set = 0;
111
112 #ifdef SIG_MATH
113 void (* last_sig)(int);
114 last_sig = signal(SIGFPE, nr_sig);
115 #endif
116
117 g=scale=anorm=0.0; //Householder reduction to bidiagonal form.
118 for (i=1;i<=n;i++) {
119 l=i+1;
120 rv1[i]=scale*g;
121 g=s=scale=0.0;
122 if (i <= m) {
123 for (k=i;k<=m;k++) C(scale += fabs(a[k][i]));
124 if (scale) {
125 for (k=i;k<=m;k++) {
126 a[k][i] /= scale;
127 C(s += RMULT(a[k][i],a[k][i]));
128 }
129
130 f=a[i][i];
131 g = -SIGN(sqrtf(s),f);
132 h=f*g-s;
133 C(a[i][i]=f-g);
134 for (j=l;j<=n;j++) {
135 for (s=0.0,k=i;k<=m;k++) C(s += RMULT(a[k][i],a[k][j]));
136 f=s/h;
137 for (k=i;k<=m;k++) C(a[k][j] += RMULT(f,a[k][i]));
138 }
139
140 for (k=i;k<=m;k++) C(a[k][i] *= scale);
141 }
142 }
143
144 w[i]=scale *g;
145 C(g=s=scale=0.0);
146 if(i<= m && i != n) {
147 for (k=l;k<=n;k++) C(scale += fabs(a[i][k]));
148 if (scale) {
149 for (k=l;k<=n;k++) {
150 a[i][k] /= scale;
151 C(s += a[i][k]*a[i][k]);
152 }
153
154 f=a[i][l];
155 g = -SIGN(sqrtf(s),f);
156 h=f*g-s;
157 C(a[i][l]=f-g);
158
159 for (k=l;k<=n;k++) C(rv1[k]=a[i][k]/h);
160 for (j=l;j<=m;j++) {
161 for (s=0.0,k=l;k<=n;k++) C(s += a[j][k]*a[i][k]);
162 for (k=l;k<=n;k++) C(a[j][k] += s*rv1[k]);
163 }
164
165 for (k=l;k<=n;k++) C(a[i][k] *= scale);
166 }
167 }
168
169 C(anorm=FMAX(anorm,(fabs(w[i])+fabs(rv1[i]))));
170 }
171
172 for (i=n;i>=1;i--) { //Accumulation of right-hand transformations.
173 if(i < n) {
174 if (g) {
175 for (j=l;j<=n;j++) // Double division to avoid possible under ow.
176 C(v[j][i]=(a[i][j]/a[i][l])/g);
177 for (j=l;j<=n;j++) {
178 for (s=0.0,k=l;k<=n;k++) C(s += a[i][k]*v[k][j]);
179 for (k=l;k<=n;k++) C(v[k][j] += s*v[k][i]);
180 }
181 }
182
183 for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
184 }
185
186 v[i][i]=1.0;
187 g=rv1[i];
188 l=i;
189 }
190
191 for (i=IMIN(m,n);i>=1;i--) { //Accumulation of left-hand transformations.
192 l=i+1;
193 g=w[i];
194 for (j=l;j<=n;j++) a[i][j]=0.0;
195 if (g) {
196 g=1.0/g;
197 for (j=l;j<=n;j++) {
198 for (s=0.0,k=l;k<=m;k++) C(s += RMULT(a[k][i],a[k][j]));
199 ;
200 f=(s/a[i][i])*g;
201 ;
202 for (k=i;k<=m;k++) C(a[k][j] += RMULT(f,a[k][i]));
203 ;
204 }
205
206 for (j=i;j<=m;j++) C(a[j][i] *= g);
207 } else for (j=i;j<=m;j++) a[j][i]=0.0;
208
209 ++a[i][i];
210 }
211
212 for (k=n;k>=1;k--) {
213 //Diagonalization of the bidiagonal form: Loop over
214 //singular values, and over allowed iterations.
215 for (its=1;its<=30;its++) {
216 flag=1;
217 for (l=k;l>=1;l--) { //Test for splitting.
218 nm=l-1; //Note that rv1[1] is always zero.
219 if ((float)(fabs(rv1[l])+anorm) == anorm) {
220 flag=0;
221 break;
222 }
223
224 if ((float)(fabs(w[nm])+anorm) == anorm) break;
225 }
226 if (flag) {
227 c=0.0; //Cancellation of rv1[l], ifl>1.
228 s=1.0;
229 for (i=l;i<=k;i++) {
230 f=s*rv1[i];
231 C(rv1[i]=c*rv1[i]);
232 if ((float)(fabs(f)+anorm) == anorm) break;
233 g=w[i];
234 h=pythag(f,g);
235 w[i]=h;
236 h=1.0/h;
237 c=g*h;
238 C(s = -f*h);
239 for (j=1;j<=m;j++) {
240 y=a[j][nm];
241 z=a[j][i];
242 a[j][nm]=y*c+z*s;
243 C(a[j][i]=z*c-y*s);
244 }
245 }
246 }
247 z=w[k];
248
249 if (l == k) { //Convergence.
250 if (z < 0.0) { //Singular value is made nonnegative.
251 w[k] = -z;
252 for (j=1;j<=n;j++) C(v[j][k] = -v[j][k]);
253 }
254 break;
255 }
256
257 if (its == 30) {
258 nrerror("no convergence in 30 svdcmp iterations");
259 goto done;
260 }
261 x=w[l]; //Shiftfrom bottom 2-by-2minor.
262 nm=k-1;
263 y=w[nm];
264 g=rv1[nm];
265 h=rv1[k];
266 f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
267 g=pythag(f,1.0);
268 C(f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x);
269 c=s=1.0; //Next QR transformation:
270 for (j=l;j<=nm;j++) {
271 i=j+1;
272 g=rv1[i];
273 y=w[i];
274 h=s*g;
275 g=c*g;
276 z=pythag(f,h);
277 rv1[j]=z;
278 c=f/z;
279 s=h/z;
280 f=x*c+g*s;
281 g = g*c-x*s;
282 h=y*s;
283 C(y *= c);
284
285 for (jj=1;jj<=n;jj++) {
286 x=v[jj][j];
287 z=v[jj][i];
288 v[jj][j]=x*c+z*s;
289 C(v[jj][i]=z*c-x*s);
290 }
291
292 z=pythag(f,h);
293 w[j]=z; //Rotation can be arbitrary if z = 0.
294
295 if (z) {
296 z=1.0/z;
297 c=f*z;
298 s=h*z;
299 }
300
301 f=c*g+s*y;
302 C(x=c*y-s*g);
303 for (jj=1;jj<=m;jj++) {
304 y=a[jj][j];
305 z=a[jj][i];
306 a[jj][j]=y*c+z*s;
307 C(a[jj][i]=z*c-y*s);
308 }
309 }
310
311 rv1[l]=0.0;
312 rv1[k]=f;
313 w[k]=x;
314 }
315 }
316
317 done:
318 #ifdef SIG_MATH
319 signal(SIGFPE, last_sig);
320 #endif
321 }
322
323 /*
324 * Solves A.X = B for a Vector X, where A is speci ed by the arrays u[1..m][1..n],
325 * w[1..n],
v[1..n][1..n] as returned by svdcmp. m and n are the dimensions of a,
326 * and will be equal for
square matrices. b[1..m] is the input right-hand side.
327 * x[1..n] is the output solution Vector.
328 * No input quantities are destroyed, so the routine may be called sequentially
329 * with different b's.
330 *
331 * tmp is a 1-n tmp vector
332 */
333 void svbksb(float **u, float w[], float **v, int m, int n, float b[], float x[],
334 float *tmp)
335 {
336 int jj, j, i;
337 float s;
338
339 for (j=1; j<=n; j++) { // Calculate U T B.
340 s=0.0;
341 if (w[j]) { // Nonzero result only if wj is nonzero.
342 for (i=1;i<=m;i++)
343 s += u[i][j]*b[i];
344 s /= w[j]; // This is the divide by wj .
345 }
346
347 tmp[j]=s;
348 }
349
350 for (j=1;j<=n;j++) { // Matrix multiply by V to get answer.
351 s=0.0;
352 for (jj=1;jj<=n;jj++) s += v[j][jj]*tmp[jj];
353 x[j]=s;
354 }
355
356 }
357
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.