1 /* linalg/svdstep.c
2 *
3 * Copyright (C) 2007, 2010 Brian Gough
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 3 of the License, or (at
8 * your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 * General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18 */
19
20 static void
chop_small_elements(gsl_vector * d,gsl_vector * f)21 chop_small_elements (gsl_vector * d, gsl_vector * f)
22 {
23 const size_t N = d->size;
24 double d_i = gsl_vector_get (d, 0);
25
26 size_t i;
27
28 for (i = 0; i < N - 1; i++)
29 {
30 double f_i = gsl_vector_get (f, i);
31 double d_ip1 = gsl_vector_get (d, i + 1);
32
33 if (fabs (f_i) < GSL_DBL_EPSILON * (fabs (d_i) + fabs (d_ip1)))
34 {
35 gsl_vector_set (f, i, 0.0);
36 }
37
38 d_i = d_ip1;
39 }
40
41 }
42
43 static double
trailing_eigenvalue(const gsl_vector * d,const gsl_vector * f)44 trailing_eigenvalue (const gsl_vector * d, const gsl_vector * f)
45 {
46 const size_t n = d->size;
47
48 double da = gsl_vector_get (d, n - 2);
49 double db = gsl_vector_get (d, n - 1);
50 double fa = (n > 2) ? gsl_vector_get (f, n - 3) : 0.0;
51 double fb = gsl_vector_get (f, n - 2);
52
53 double mu;
54
55 #if GOLUB_VAN_LOAN_8_3_2
56
57 /* Golub and van Loan, Algorithm 8.3.2
58 The full SVD algorithm is described in section 8.6.2 */
59
60 double ta = da * da + fa * fa;
61 double tb = db * db + fb * fb;
62 double tab = da * fb;
63
64 double dt = (ta - tb) / 2.0;
65
66
67 if (dt >= 0)
68 {
69 mu = tb - (tab * tab) / (dt + hypot (dt, tab));
70 }
71 else
72 {
73 mu = tb + (tab * tab) / ((-dt) + hypot (dt, tab));
74 }
75
76 #else
77 {
78 /* We can compute mu more accurately than using the formula above
79 since we know the roots cannot be negative. This also avoids
80 the possibility of NaNs in the formula above.
81
82 The matrix is [ da^2 + fa^2, da fb ;
83 da fb , db^2 + fb^2 ]
84 and mu is the eigenvalue closest to the bottom right element.
85 */
86
87 double ta = da * da + fa * fa;
88 double tb = db * db + fb * fb;
89 double tab = da * fb;
90
91 double dt = (ta - tb) / 2.0;
92
93 double S = ta + tb;
94 double da2 = da * da, db2 = db * db;
95 double fa2 = fa * fa, fb2 = fb * fb;
96 double P = (da2 * db2) + (fa2 * db2) + (fa2 * fb2);
97 double D = hypot(dt, tab);
98 double r1 = S/2 + D;
99
100 if (dt >= 0)
101 {
102 /* tb < ta, choose smaller root */
103 mu = (r1 > 0) ? P / r1 : 0.0;
104 }
105 else
106 {
107 /* tb > ta, choose larger root */
108 mu = r1;
109 }
110 }
111
112 #endif
113
114 return mu;
115 }
116
117 static void
create_schur(double d0,double f0,double d1,double * c,double * s)118 create_schur (double d0, double f0, double d1, double * c, double * s)
119 {
120 double apq = 2.0 * d0 * f0;
121
122 if (d0 == 0 || f0 == 0)
123 {
124 *c = 1.0;
125 *s = 0.0;
126 return;
127 }
128
129 /* Check if we need to rescale to avoid underflow/overflow */
130 if (fabs(d0) < GSL_SQRT_DBL_MIN || fabs(d0) > GSL_SQRT_DBL_MAX
131 || fabs(f0) < GSL_SQRT_DBL_MIN || fabs(f0) > GSL_SQRT_DBL_MAX
132 || fabs(d1) < GSL_SQRT_DBL_MIN || fabs(d1) > GSL_SQRT_DBL_MAX)
133 {
134 double scale;
135 int d0_exp, f0_exp;
136 frexp(d0, &d0_exp);
137 frexp(f0, &f0_exp);
138 /* Bring |d0*f0| into the range GSL_DBL_MIN to GSL_DBL_MAX */
139 scale = ldexp(1.0, -(d0_exp + f0_exp)/4);
140 d0 *= scale;
141 f0 *= scale;
142 d1 *= scale;
143 apq = 2.0 * d0 * f0;
144 }
145
146 if (apq != 0.0)
147 {
148 double t;
149 double tau = (f0*f0 + (d1 + d0)*(d1 - d0)) / apq;
150
151 if (tau >= 0.0)
152 {
153 t = 1.0/(tau + hypot(1.0, tau));
154 }
155 else
156 {
157 t = -1.0/(-tau + hypot(1.0, tau));
158 }
159
160 *c = 1.0 / hypot(1.0, t);
161 *s = t * (*c);
162 }
163 else
164 {
165 *c = 1.0;
166 *s = 0.0;
167 }
168 }
169
170 static void
svd2(gsl_vector * d,gsl_vector * f,gsl_matrix * U,gsl_matrix * V)171 svd2 (gsl_vector * d, gsl_vector * f, gsl_matrix * U, gsl_matrix * V)
172 {
173 size_t i;
174 double c, s, a11, a12, a21, a22;
175
176 const size_t M = U->size1;
177 const size_t N = V->size1;
178
179 double d0 = gsl_vector_get (d, 0);
180 double f0 = gsl_vector_get (f, 0);
181
182 double d1 = gsl_vector_get (d, 1);
183
184 if (d0 == 0.0)
185 {
186 /* Eliminate off-diagonal element in [0,f0;0,d1] to make [d,0;0,0] */
187
188 create_givens (f0, d1, &c, &s);
189
190 /* compute B <= G^T B X, where X = [0,1;1,0] */
191
192 gsl_vector_set (d, 0, c * f0 - s * d1);
193 gsl_vector_set (f, 0, s * f0 + c * d1);
194 gsl_vector_set (d, 1, 0.0);
195
196 /* Compute U <= U G */
197
198 for (i = 0; i < M; i++)
199 {
200 double Uip = gsl_matrix_get (U, i, 0);
201 double Uiq = gsl_matrix_get (U, i, 1);
202 gsl_matrix_set (U, i, 0, c * Uip - s * Uiq);
203 gsl_matrix_set (U, i, 1, s * Uip + c * Uiq);
204 }
205
206 /* Compute V <= V X */
207
208 gsl_matrix_swap_columns (V, 0, 1);
209
210 return;
211 }
212 else if (d1 == 0.0)
213 {
214 /* Eliminate off-diagonal element in [d0,f0;0,0] */
215
216 create_givens (d0, f0, &c, &s);
217
218 /* compute B <= B G */
219
220 gsl_vector_set (d, 0, d0 * c - f0 * s);
221 gsl_vector_set (f, 0, 0.0);
222
223 /* Compute V <= V G */
224
225 for (i = 0; i < N; i++)
226 {
227 double Vip = gsl_matrix_get (V, i, 0);
228 double Viq = gsl_matrix_get (V, i, 1);
229 gsl_matrix_set (V, i, 0, c * Vip - s * Viq);
230 gsl_matrix_set (V, i, 1, s * Vip + c * Viq);
231 }
232
233 return;
234 }
235 else
236 {
237 /* Make columns orthogonal, A = [d0, f0; 0, d1] * G */
238
239 create_schur (d0, f0, d1, &c, &s);
240
241 /* compute B <= B G */
242
243 a11 = c * d0 - s * f0;
244 a21 = - s * d1;
245
246 a12 = s * d0 + c * f0;
247 a22 = c * d1;
248
249 /* Compute V <= V G */
250
251 for (i = 0; i < N; i++)
252 {
253 double Vip = gsl_matrix_get (V, i, 0);
254 double Viq = gsl_matrix_get (V, i, 1);
255 gsl_matrix_set (V, i, 0, c * Vip - s * Viq);
256 gsl_matrix_set (V, i, 1, s * Vip + c * Viq);
257 }
258
259 /* Eliminate off-diagonal elements, bring column with largest
260 norm to first column */
261
262 if (hypot(a11, a21) < hypot(a12,a22))
263 {
264 double t1, t2;
265
266 /* B <= B X */
267
268 t1 = a11; a11 = a12; a12 = t1;
269 t2 = a21; a21 = a22; a22 = t2;
270
271 /* V <= V X */
272
273 gsl_matrix_swap_columns(V, 0, 1);
274 }
275
276 create_givens (a11, a21, &c, &s);
277
278 /* compute B <= G^T B */
279
280 gsl_vector_set (d, 0, c * a11 - s * a21);
281 gsl_vector_set (f, 0, c * a12 - s * a22);
282 gsl_vector_set (d, 1, s * a12 + c * a22);
283
284 /* Compute U <= U G */
285
286 for (i = 0; i < M; i++)
287 {
288 double Uip = gsl_matrix_get (U, i, 0);
289 double Uiq = gsl_matrix_get (U, i, 1);
290 gsl_matrix_set (U, i, 0, c * Uip - s * Uiq);
291 gsl_matrix_set (U, i, 1, s * Uip + c * Uiq);
292 }
293
294 return;
295 }
296 }
297
298
299 static void
chase_out_intermediate_zero(gsl_vector * d,gsl_vector * f,gsl_matrix * U,size_t k0)300 chase_out_intermediate_zero (gsl_vector * d, gsl_vector * f, gsl_matrix * U, size_t k0)
301 {
302 #if !USE_BLAS
303 const size_t M = U->size1;
304 #endif
305 const size_t n = d->size;
306 double c, s;
307 double x, y;
308 size_t k;
309
310 x = gsl_vector_get (f, k0);
311 y = gsl_vector_get (d, k0+1);
312
313 for (k = k0; k < n - 1; k++)
314 {
315 create_givens (y, -x, &c, &s);
316
317 /* Compute U <= U G */
318
319 #ifdef USE_BLAS
320 {
321 gsl_vector_view Uk0 = gsl_matrix_column(U,k0);
322 gsl_vector_view Ukp1 = gsl_matrix_column(U,k+1);
323 gsl_blas_drot(&Uk0.vector, &Ukp1.vector, c, -s);
324 }
325 #else
326 {
327 size_t i;
328
329 for (i = 0; i < M; i++)
330 {
331 double Uip = gsl_matrix_get (U, i, k0);
332 double Uiq = gsl_matrix_get (U, i, k + 1);
333 gsl_matrix_set (U, i, k0, c * Uip - s * Uiq);
334 gsl_matrix_set (U, i, k + 1, s * Uip + c * Uiq);
335 }
336 }
337 #endif
338
339 /* compute B <= G^T B */
340
341 gsl_vector_set (d, k + 1, s * x + c * y);
342
343 if (k == k0)
344 gsl_vector_set (f, k, c * x - s * y );
345
346 if (k < n - 2)
347 {
348 double z = gsl_vector_get (f, k + 1);
349 gsl_vector_set (f, k + 1, c * z);
350
351 x = -s * z ;
352 y = gsl_vector_get (d, k + 2);
353 }
354 }
355 }
356
357 static void
chase_out_trailing_zero(gsl_vector * d,gsl_vector * f,gsl_matrix * V)358 chase_out_trailing_zero (gsl_vector * d, gsl_vector * f, gsl_matrix * V)
359 {
360 #if !USE_BLAS
361 const size_t N = V->size1;
362 #endif
363 const size_t n = d->size;
364 double c, s;
365 double x, y;
366 size_t k;
367
368 x = gsl_vector_get (d, n - 2);
369 y = gsl_vector_get (f, n - 2);
370
371 for (k = n - 1; k-- > 0;)
372 {
373 create_givens (x, y, &c, &s);
374
375 /* Compute V <= V G where G = [c, s ; -s, c] */
376
377 #ifdef USE_BLAS
378 {
379 gsl_vector_view Vp = gsl_matrix_column(V,k);
380 gsl_vector_view Vq = gsl_matrix_column(V,n-1);
381 gsl_blas_drot(&Vp.vector, &Vq.vector, c, -s);
382 }
383 #else
384 {
385 size_t i;
386
387 for (i = 0; i < N; i++)
388 {
389 double Vip = gsl_matrix_get (V, i, k);
390 double Viq = gsl_matrix_get (V, i, n - 1);
391 gsl_matrix_set (V, i, k, c * Vip - s * Viq);
392 gsl_matrix_set (V, i, n - 1, s * Vip + c * Viq);
393 }
394 }
395 #endif
396
397 /* compute B <= B G */
398
399 gsl_vector_set (d, k, c * x - s * y);
400
401 if (k == n - 2)
402 gsl_vector_set (f, k, s * x + c * y );
403
404 if (k > 0)
405 {
406 double z = gsl_vector_get (f, k - 1);
407 gsl_vector_set (f, k - 1, c * z);
408
409 x = gsl_vector_get (d, k - 1);
410 y = s * z ;
411 }
412 }
413 }
414
415 static void
qrstep(gsl_vector * d,gsl_vector * f,gsl_matrix * U,gsl_matrix * V)416 qrstep (gsl_vector * d, gsl_vector * f, gsl_matrix * U, gsl_matrix * V)
417 {
418 #if !USE_BLAS
419 const size_t M = U->size1;
420 const size_t N = V->size1;
421 #endif
422 const size_t n = d->size;
423 double y, z;
424 double ak, bk, zk, ap, bp, aq, bq;
425 size_t i, k;
426
427 if (n == 1)
428 return; /* shouldn't happen */
429
430 /* Compute 2x2 svd directly */
431
432 if (n == 2)
433 {
434 svd2 (d, f, U, V);
435 return;
436 }
437
438 /* Chase out any zeroes on the diagonal */
439
440 for (i = 0; i < n - 1; i++)
441 {
442 double d_i = gsl_vector_get (d, i);
443
444 if (d_i == 0.0)
445 {
446 chase_out_intermediate_zero (d, f, U, i);
447 return;
448 }
449 }
450
451 /* Chase out any zero at the end of the diagonal */
452
453 {
454 double d_nm1 = gsl_vector_get (d, n - 1);
455
456 if (d_nm1 == 0.0)
457 {
458 chase_out_trailing_zero (d, f, V);
459 return;
460 }
461 }
462
463
464 /* Apply QR reduction steps to the diagonal and offdiagonal */
465
466 {
467 double d0 = gsl_vector_get (d, 0);
468 double f0 = gsl_vector_get (f, 0);
469
470 double d1 = gsl_vector_get (d, 1);
471 double f1 = gsl_vector_get (f, 1);
472
473 {
474 double mu = trailing_eigenvalue (d, f);
475
476 y = d0 * d0 - mu;
477 z = d0 * f0;
478 }
479
480 /* Set up the recurrence for Givens rotations on a bidiagonal matrix */
481
482 ak = 0;
483 bk = 0;
484
485 ap = d0;
486 bp = f0;
487
488 aq = d1;
489 bq = f1;
490 }
491
492 for (k = 0; k < n - 1; k++)
493 {
494 double c, s;
495 create_givens (y, z, &c, &s);
496
497 /* Compute V <= V G */
498
499 #ifdef USE_BLAS
500 {
501 gsl_vector_view Vk = gsl_matrix_column(V,k);
502 gsl_vector_view Vkp1 = gsl_matrix_column(V,k+1);
503 gsl_blas_drot(&Vk.vector, &Vkp1.vector, c, -s);
504 }
505 #else
506 for (i = 0; i < N; i++)
507 {
508 double Vip = gsl_matrix_get (V, i, k);
509 double Viq = gsl_matrix_get (V, i, k + 1);
510 gsl_matrix_set (V, i, k, c * Vip - s * Viq);
511 gsl_matrix_set (V, i, k + 1, s * Vip + c * Viq);
512 }
513 #endif
514
515 /* compute B <= B G */
516
517 {
518 double bk1 = c * bk - s * z;
519
520 double ap1 = c * ap - s * bp;
521 double bp1 = s * ap + c * bp;
522 double zp1 = -s * aq;
523
524 double aq1 = c * aq;
525
526 if (k > 0)
527 {
528 gsl_vector_set (f, k - 1, bk1);
529 }
530
531 ak = ap1;
532 bk = bp1;
533 zk = zp1;
534
535 ap = aq1;
536
537 if (k < n - 2)
538 {
539 bp = gsl_vector_get (f, k + 1);
540 }
541 else
542 {
543 bp = 0.0;
544 }
545
546 y = ak;
547 z = zk;
548 }
549
550 create_givens (y, z, &c, &s);
551
552 /* Compute U <= U G */
553
554 #ifdef USE_BLAS
555 {
556 gsl_vector_view Uk = gsl_matrix_column(U,k);
557 gsl_vector_view Ukp1 = gsl_matrix_column(U,k+1);
558 gsl_blas_drot(&Uk.vector, &Ukp1.vector, c, -s);
559 }
560 #else
561 for (i = 0; i < M; i++)
562 {
563 double Uip = gsl_matrix_get (U, i, k);
564 double Uiq = gsl_matrix_get (U, i, k + 1);
565 gsl_matrix_set (U, i, k, c * Uip - s * Uiq);
566 gsl_matrix_set (U, i, k + 1, s * Uip + c * Uiq);
567 }
568 #endif
569
570 /* compute B <= G^T B */
571
572 {
573 double ak1 = c * ak - s * zk;
574 double bk1 = c * bk - s * ap;
575 double zk1 = -s * bp;
576
577 double ap1 = s * bk + c * ap;
578 double bp1 = c * bp;
579
580 gsl_vector_set (d, k, ak1);
581
582 ak = ak1;
583 bk = bk1;
584 zk = zk1;
585
586 ap = ap1;
587 bp = bp1;
588
589 if (k < n - 2)
590 {
591 aq = gsl_vector_get (d, k + 2);
592 }
593 else
594 {
595 aq = 0.0;
596 }
597
598 y = bk;
599 z = zk;
600 }
601 }
602
603 gsl_vector_set (f, n - 2, bk);
604 gsl_vector_set (d, n - 1, ap);
605 }
606
607
608