1 /**
2 @file
3 @ingroup nr
4 */
5 #include <bpm/bpm_messages.h>
6 #include <bpm/bpm_nr.h>
7
gsl_linalg_householder_hm(double tau,const gsl_vector * v,gsl_matrix * A)8 int gsl_linalg_householder_hm (double tau, const gsl_vector * v, gsl_matrix * A)
9 {
10 /**
11 applies a householder transformation v,tau to matrix m
12
13 *
14 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2004 Gerard Jungman, Brian Gough
15 *
16 * This program is free software; you can redistribute it and/or modify
17 * it under the terms of the GNU General Public License as published by
18 * the Free Software Foundation; either version 2 of the License, or (at
19 * your option) any later version.
20 *
21 * This program is distributed in the hope that it will be useful, but
22 * WITHOUT ANY WARRANTY; without even the implied warranty of
23 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
24 * General Public License for more details.
25 *
26 * You should have received a copy of the GNU General Public License
27 * along with this program; if not, write to the Free Software
28 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
29
30 */
31
32 if (tau == 0.0)
33 {
34 return BPM_SUCCESS;
35 }
36
37 #ifdef USE_BLAS
38 {
39 gsl_vector_const_view v1 = gsl_vector_const_subvector (v, 1, v->size - 1);
40 gsl_matrix_view A1 = gsl_matrix_submatrix (A, 1, 0, A->size1 - 1, A->size2);
41 size_t j;
42
43 for (j = 0; j < A->size2; j++)
44 {
45 double wj = 0.0;
46 gsl_vector_view A1j = gsl_matrix_column(&A1.matrix, j);
47 gsl_blas_ddot (&A1j.vector, &v1.vector, &wj);
48 wj += gsl_matrix_get(A,0,j);
49
50 {
51 double A0j = gsl_matrix_get (A, 0, j);
52 gsl_matrix_set (A, 0, j, A0j - tau * wj);
53 }
54
55 gsl_blas_daxpy (-tau * wj, &v1.vector, &A1j.vector);
56 }
57 }
58 #else
59 {
60 size_t i, j;
61
62 for (j = 0; j < A->size2; j++)
63 {
64 /* Compute wj = Akj vk */
65
66 double wj = gsl_matrix_get(A,0,j);
67
68 for (i = 1; i < A->size1; i++) /* note, computed for v(0) = 1 above */
69 {
70 wj += gsl_matrix_get(A,i,j) * gsl_vector_get(v,i);
71 }
72
73 /* Aij = Aij - tau vi wj */
74
75 /* i = 0 */
76 {
77 double A0j = gsl_matrix_get (A, 0, j);
78 gsl_matrix_set (A, 0, j, A0j - tau * wj);
79 }
80
81 /* i = 1 .. M-1 */
82
83 for (i = 1; i < A->size1; i++)
84 {
85 double Aij = gsl_matrix_get (A, i, j);
86 double vi = gsl_vector_get (v, i);
87 gsl_matrix_set (A, i, j, Aij - tau * vi * wj);
88 }
89 }
90 }
91 #endif
92
93 return BPM_SUCCESS;
94 }
95
gsl_linalg_householder_hm1(double tau,gsl_matrix * A)96 int gsl_linalg_householder_hm1 (double tau, gsl_matrix * A)
97 {
98 /**
99 applies a householder transformation v,tau to a matrix being
100 build up from the identity matrix, using the first column of A as
101 a householder vector
102 */
103
104 if (tau == 0)
105 {
106 size_t i,j;
107
108 gsl_matrix_set (A, 0, 0, 1.0);
109
110 for (j = 1; j < A->size2; j++)
111 {
112 gsl_matrix_set (A, 0, j, 0.0);
113 }
114
115 for (i = 1; i < A->size1; i++)
116 {
117 gsl_matrix_set (A, i, 0, 0.0);
118 }
119
120 return BPM_SUCCESS;
121 }
122
123 /* w = A' v */
124
125 #ifdef USE_BLAS
126 {
127 gsl_matrix_view A1 = gsl_matrix_submatrix (A, 1, 0, A->size1 - 1, A->size2);
128 gsl_vector_view v1 = gsl_matrix_column (&A1.matrix, 0);
129 size_t j;
130
131 for (j = 1; j < A->size2; j++)
132 {
133 double wj = 0.0; /* A0j * v0 */
134
135 gsl_vector_view A1j = gsl_matrix_column(&A1.matrix, j);
136 gsl_blas_ddot (&A1j.vector, &v1.vector, &wj);
137
138 /* A = A - tau v w' */
139
140 gsl_matrix_set (A, 0, j, - tau * wj);
141
142 gsl_blas_daxpy(-tau*wj, &v1.vector, &A1j.vector);
143 }
144
145 gsl_blas_dscal(-tau, &v1.vector);
146
147 gsl_matrix_set (A, 0, 0, 1.0 - tau);
148 }
149 #else
150 {
151 size_t i, j;
152
153 for (j = 1; j < A->size2; j++)
154 {
155 double wj = 0.0; /* A0j * v0 */
156
157 for (i = 1; i < A->size1; i++)
158 {
159 double vi = gsl_matrix_get(A, i, 0);
160 wj += gsl_matrix_get(A,i,j) * vi;
161 }
162
163 /* A = A - tau v w' */
164
165 gsl_matrix_set (A, 0, j, - tau * wj);
166
167 for (i = 1; i < A->size1; i++)
168 {
169 double vi = gsl_matrix_get (A, i, 0);
170 double Aij = gsl_matrix_get (A, i, j);
171 gsl_matrix_set (A, i, j, Aij - tau * vi * wj);
172 }
173 }
174
175 for (i = 1; i < A->size1; i++)
176 {
177 double vi = gsl_matrix_get(A, i, 0);
178 gsl_matrix_set(A, i, 0, -tau * vi);
179 }
180
181 gsl_matrix_set (A, 0, 0, 1.0 - tau);
182 }
183 #endif
184
185 return BPM_SUCCESS;
186 }
187
create_givens(const double a,const double b,double * c,double * s)188 void create_givens (const double a, const double b, double *c, double *s)
189 {
190 if (b == 0)
191 {
192 *c = 1;
193 *s = 0;
194 }
195 else if (fabs (b) > fabs (a))
196 {
197 double t = -a / b;
198 double s1 = 1.0 / sqrt (1 + t * t);
199 *s = s1;
200 *c = s1 * t;
201 }
202 else
203 {
204 double t = -b / a;
205 double c1 = 1.0 / sqrt (1 + t * t);
206 *c = c1;
207 *s = c1 * t;
208 }
209 }
210
211
gsl_linalg_bidiag_decomp(gsl_matrix * A,gsl_vector * tau_U,gsl_vector * tau_V)212 int gsl_linalg_bidiag_decomp (gsl_matrix * A, gsl_vector * tau_U, gsl_vector * tau_V)
213 {
214 if (A->size1 < A->size2)
215 {
216 bpm_error("bidiagonal decomposition requires M>=N in gsl_linalg_bidag_decomp(...)",
217 __FILE__, __LINE__);
218 return BPM_SUCCESS;
219 }
220 else if (tau_U->size != A->size2)
221 {
222 bpm_error("size of tau_U must be N in gsl_linalg_bidag_decomp(...)",
223 __FILE__, __LINE__);
224 return BPM_SUCCESS;
225 }
226 else if (tau_V->size + 1 != A->size2)
227 {
228 bpm_error("size of tau_V must be (N - 1) in gsl_linalg_bidag_decomp(...)",
229 __FILE__, __LINE__);
230 return BPM_SUCCESS;
231 }
232 else
233 {
234 const size_t M = A->size1;
235 const size_t N = A->size2;
236 size_t i;
237
238 for (i = 0 ; i < N; i++)
239 {
240 /* Apply Householder transformation to current column */
241
242 {
243 gsl_vector_view c = gsl_matrix_column (A, i);
244 gsl_vector_view v = gsl_vector_subvector (&c.vector, i, M - i);
245 double tau_i = gsl_linalg_householder_transform (&v.vector);
246
247 /* Apply the transformation to the remaining columns */
248
249 if (i + 1 < N)
250 {
251 gsl_matrix_view m =
252 gsl_matrix_submatrix (A, i, i + 1, M - i, N - (i + 1));
253 gsl_linalg_householder_hm (tau_i, &v.vector, &m.matrix);
254 }
255
256 gsl_vector_set (tau_U, i, tau_i);
257
258 }
259
260 /* Apply Householder transformation to current row */
261
262 if (i + 1 < N)
263 {
264 gsl_vector_view r = gsl_matrix_row (A, i);
265 gsl_vector_view v = gsl_vector_subvector (&r.vector, i + 1, N - (i + 1));
266 double tau_i = gsl_linalg_householder_transform (&v.vector);
267
268 /* Apply the transformation to the remaining rows */
269
270 if (i + 1 < M)
271 {
272 gsl_matrix_view m =
273 gsl_matrix_submatrix (A, i+1, i+1, M - (i+1), N - (i+1));
274 gsl_linalg_householder_mh (tau_i, &v.vector, &m.matrix);
275 }
276
277 gsl_vector_set (tau_V, i, tau_i);
278 }
279 }
280 }
281
282 return BPM_SUCCESS;
283 }
284
gsl_linalg_householder_transform(gsl_vector * v)285 double gsl_linalg_householder_transform (gsl_vector * v)
286 {
287 /**
288 replace v[0:n-1] with a householder vector (v[0:n-1]) and
289 coefficient tau that annihilate v[1:n-1]
290 */
291
292 const size_t n = v->size ;
293
294 if (n == 1)
295 {
296 return 0.0; /* tau = 0 */
297 }
298 else
299 {
300 double alpha, beta, tau ;
301
302 gsl_vector_view x = gsl_vector_subvector (v, 1, n - 1) ;
303
304 double xnorm = gsl_blas_dnrm2 (&x.vector);
305
306 if (xnorm == 0)
307 {
308 return 0.0; /* tau = 0 */
309 }
310
311 alpha = gsl_vector_get (v, 0) ;
312 beta = - (alpha >= 0.0 ? +1.0 : -1.0) * hypot(alpha, xnorm) ;
313 tau = (beta - alpha) / beta ;
314
315 gsl_blas_dscal (1.0 / (alpha - beta), &x.vector);
316 gsl_vector_set (v, 0, beta) ;
317
318 return tau;
319 }
320 }
321
gsl_linalg_householder_mh(double tau,const gsl_vector * v,gsl_matrix * A)322 int gsl_linalg_householder_mh (double tau, const gsl_vector * v, gsl_matrix * A)
323 {
324 /**
325 applies a householder transformation v,tau to matrix m from the
326 right hand side in order to zero out rows
327 */
328
329 if (tau == 0)
330 return BPM_SUCCESS;
331
332 /* A = A - tau w v' */
333
334 #ifdef USE_BLAS
335 {
336 gsl_vector_const_view v1 = gsl_vector_const_subvector (v, 1, v->size - 1);
337 gsl_matrix_view A1 = gsl_matrix_submatrix (A, 0, 1, A->size1, A->size2-1);
338 size_t i;
339
340 for (i = 0; i < A->size1; i++)
341 {
342 double wi = 0.0;
343 gsl_vector_view A1i = gsl_matrix_row(&A1.matrix, i);
344 gsl_blas_ddot (&A1i.vector, &v1.vector, &wi);
345 wi += gsl_matrix_get(A,i,0);
346
347 {
348 double Ai0 = gsl_matrix_get (A, i, 0);
349 gsl_matrix_set (A, i, 0, Ai0 - tau * wi);
350 }
351
352 gsl_blas_daxpy(-tau * wi, &v1.vector, &A1i.vector);
353 }
354 }
355 #else
356 {
357 size_t i, j;
358
359 for (i = 0; i < A->size1; i++)
360 {
361 double wi = gsl_matrix_get(A,i,0);
362
363 for (j = 1; j < A->size2; j++) /* note, computed for v(0) = 1 above */
364 {
365 wi += gsl_matrix_get(A,i,j) * gsl_vector_get(v,j);
366 }
367
368 /* j = 0 */
369
370 {
371 double Ai0 = gsl_matrix_get (A, i, 0);
372 gsl_matrix_set (A, i, 0, Ai0 - tau * wi);
373 }
374
375 /* j = 1 .. N-1 */
376
377 for (j = 1; j < A->size2; j++)
378 {
379 double vj = gsl_vector_get (v, j);
380 double Aij = gsl_matrix_get (A, i, j);
381 gsl_matrix_set (A, i, j, Aij - tau * wi * vj);
382 }
383 }
384 }
385 #endif
386
387 return BPM_SUCCESS;
388 }
389
gsl_linalg_SV_solve(const gsl_matrix * U,const gsl_matrix * V,const gsl_vector * S,const gsl_vector * b,gsl_vector * x)390 int gsl_linalg_SV_solve (const gsl_matrix * U,
391 const gsl_matrix * V,
392 const gsl_vector * S,
393 const gsl_vector * b, gsl_vector * x)
394 {
395 if (U->size1 != b->size)
396 {
397 bpm_error("first dimension of matrix U must size of vector b in gsl_linalg_SV_solve(..)",
398 __FILE__, __LINE__);
399 return BPM_SUCCESS;
400 }
401 else if (U->size2 != S->size)
402 {
403 bpm_error("length of vector S must match second dimension of matrix U in gsl_linalg_SV_solve(..)",
404 __FILE__, __LINE__);
405 return BPM_SUCCESS;
406 }
407 else if (V->size1 != V->size2)
408 {
409 bpm_error("matrix V must be square in gsl_linalg_SV_solve(..)",
410 __FILE__, __LINE__);
411 return BPM_SUCCESS;
412 }
413 else if (S->size != V->size1)
414 {
415 bpm_error("length of vector S must match size of matrix V in gsl_linalg_SV_solve(..)",
416 __FILE__, __LINE__);
417 return BPM_SUCCESS;
418 }
419 else if (V->size2 != x->size)
420 {
421 bpm_error("size of matrix V must match size of vector x in gsl_linalg_SV_solve(..)",
422 __FILE__, __LINE__);
423 return BPM_SUCCESS;
424 }
425 else
426 {
427 const size_t N = U->size2;
428 size_t i;
429
430 gsl_vector *w = gsl_vector_calloc (N);
431
432 gsl_blas_dgemv (CblasTrans, 1.0, U, b, 0.0, w);
433
434 for (i = 0; i < N; i++)
435 {
436 double wi = gsl_vector_get (w, i);
437 double alpha = gsl_vector_get (S, i);
438 if (alpha != 0)
439 alpha = 1.0 / alpha;
440 gsl_vector_set (w, i, alpha * wi);
441 }
442
443 gsl_blas_dgemv (CblasNoTrans, 1.0, V, w, 0.0, x);
444
445 gsl_vector_free (w);
446
447 return BPM_SUCCESS;
448 }
449 }
450
gsl_isnan(const double x)451 int gsl_isnan (const double x)
452 {
453 int status = (x != x);
454 return status;
455 }
456
chop_small_elements(gsl_vector * d,gsl_vector * f)457 void chop_small_elements (gsl_vector * d, gsl_vector * f)
458 {
459 const size_t N = d->size;
460 double d_i = gsl_vector_get (d, 0);
461
462 size_t i;
463
464 for (i = 0; i < N - 1; i++)
465 {
466 double f_i = gsl_vector_get (f, i);
467 double d_ip1 = gsl_vector_get (d, i + 1);
468
469 if (fabs (f_i) < GSL_DBL_EPSILON * (fabs (d_i) + fabs (d_ip1)))
470 {
471 gsl_vector_set (f, i, 0.0);
472 }
473 d_i = d_ip1;
474 }
475
476 }
477
qrstep(gsl_vector * d,gsl_vector * f,gsl_matrix * U,gsl_matrix * V)478 void qrstep (gsl_vector * d, gsl_vector * f, gsl_matrix * U, gsl_matrix * V)
479 {
480 #if !USE_BLAS
481 const size_t M = U->size1;
482 const size_t N = V->size1;
483 #endif
484 const size_t n = d->size;
485 double y, z;
486 double ak, bk, zk, ap, bp, aq, bq;
487 size_t i, k;
488
489 if (n == 1)
490 return; /* shouldn't happen */
491
492 /* Compute 2x2 svd directly */
493
494 if (n == 2)
495 {
496 svd2 (d, f, U, V);
497 return;
498 }
499
500 /* Chase out any zeroes on the diagonal */
501
502 for (i = 0; i < n - 1; i++)
503 {
504 double d_i = gsl_vector_get (d, i);
505
506 if (d_i == 0.0)
507 {
508 chase_out_intermediate_zero (d, f, U, i);
509 return;
510 }
511 }
512
513 /* Chase out any zero at the end of the diagonal */
514
515 {
516 double d_nm1 = gsl_vector_get (d, n - 1);
517
518 if (d_nm1 == 0.0)
519 {
520 chase_out_trailing_zero (d, f, V);
521 return;
522 }
523 }
524
525
526 /* Apply QR reduction steps to the diagonal and offdiagonal */
527
528 {
529 double d0 = gsl_vector_get (d, 0);
530 double f0 = gsl_vector_get (f, 0);
531
532 double d1 = gsl_vector_get (d, 1);
533 double f1 = gsl_vector_get (f, 1);
534
535 {
536 double mu = trailing_eigenvalue (d, f);
537
538 y = d0 * d0 - mu;
539 z = d0 * f0;
540 }
541
542 /* Set up the recurrence for Givens rotations on a bidiagonal matrix */
543
544 ak = 0;
545 bk = 0;
546
547 ap = d0;
548 bp = f0;
549
550 aq = d1;
551 bq = f1;
552 }
553
554 for (k = 0; k < n - 1; k++)
555 {
556 double c, s;
557 create_givens (y, z, &c, &s);
558
559 /* Compute V <= V G */
560
561 #ifdef USE_BLAS
562 {
563 gsl_vector_view Vk = gsl_matrix_column(V,k);
564 gsl_vector_view Vkp1 = gsl_matrix_column(V,k+1);
565 gsl_blas_drot(&Vk.vector, &Vkp1.vector, c, -s);
566 }
567 #else
568 for (i = 0; i < N; i++)
569 {
570 double Vip = gsl_matrix_get (V, i, k);
571 double Viq = gsl_matrix_get (V, i, k + 1);
572 gsl_matrix_set (V, i, k, c * Vip - s * Viq);
573 gsl_matrix_set (V, i, k + 1, s * Vip + c * Viq);
574 }
575 #endif
576
577 /* compute B <= B G */
578
579 {
580 double bk1 = c * bk - s * z;
581
582 double ap1 = c * ap - s * bp;
583 double bp1 = s * ap + c * bp;
584 double zp1 = -s * aq;
585
586 double aq1 = c * aq;
587
588 if (k > 0)
589 {
590 gsl_vector_set (f, k - 1, bk1);
591 }
592
593 ak = ap1;
594 bk = bp1;
595 zk = zp1;
596
597 ap = aq1;
598
599 if (k < n - 2)
600 {
601 bp = gsl_vector_get (f, k + 1);
602 }
603 else
604 {
605 bp = 0.0;
606 }
607
608 y = ak;
609 z = zk;
610 }
611
612 create_givens (y, z, &c, &s);
613
614 /* Compute U <= U G */
615
616 #ifdef USE_BLAS
617 {
618 gsl_vector_view Uk = gsl_matrix_column(U,k);
619 gsl_vector_view Ukp1 = gsl_matrix_column(U,k+1);
620 gsl_blas_drot(&Uk.vector, &Ukp1.vector, c, -s);
621 }
622 #else
623 for (i = 0; i < M; i++)
624 {
625 double Uip = gsl_matrix_get (U, i, k);
626 double Uiq = gsl_matrix_get (U, i, k + 1);
627 gsl_matrix_set (U, i, k, c * Uip - s * Uiq);
628 gsl_matrix_set (U, i, k + 1, s * Uip + c * Uiq);
629 }
630 #endif
631
632 /* compute B <= G^T B */
633
634 {
635 double ak1 = c * ak - s * zk;
636 double bk1 = c * bk - s * ap;
637 double zk1 = -s * bp;
638
639 double ap1 = s * bk + c * ap;
640 double bp1 = c * bp;
641
642 gsl_vector_set (d, k, ak1);
643
644 ak = ak1;
645 bk = bk1;
646 zk = zk1;
647
648 ap = ap1;
649 bp = bp1;
650
651 if (k < n - 2)
652 {
653 aq = gsl_vector_get (d, k + 2);
654 }
655 else
656 {
657 aq = 0.0;
658 }
659
660 y = bk;
661 z = zk;
662 }
663 }
664
665 gsl_vector_set (f, n - 2, bk);
666 gsl_vector_set (d, n - 1, ap);
667 }
668
trailing_eigenvalue(const gsl_vector * d,const gsl_vector * f)669 double trailing_eigenvalue (const gsl_vector * d, const gsl_vector * f)
670 {
671
672 const size_t n = d->size;
673
674 double da = gsl_vector_get (d, n - 2);
675 double db = gsl_vector_get (d, n - 1);
676 double fa = (n > 2) ? gsl_vector_get (f, n - 3) : 0.0;
677 double fb = gsl_vector_get (f, n - 2);
678
679 double ta = da * da + fa * fa;
680 double tb = db * db + fb * fb;
681 double tab = da * fb;
682
683 double dt = (ta - tb) / 2.0;
684
685 double mu;
686
687 if (dt >= 0)
688 {
689 mu = tb - (tab * tab) / (dt + hypot (dt, tab));
690 }
691 else
692 {
693 mu = tb + (tab * tab) / ((-dt) + hypot (dt, tab));
694 }
695
696 return mu;
697 }
698
create_schur(double d0,double f0,double d1,double * c,double * s)699 void create_schur (double d0, double f0, double d1, double * c, double * s)
700 {
701
702 double apq = 2.0 * d0 * f0;
703
704 if (apq != 0.0)
705 {
706 double t;
707 double tau = (f0*f0 + (d1 + d0)*(d1 - d0)) / apq;
708
709 if (tau >= 0.0)
710 {
711 t = 1.0/(tau + hypot(1.0, tau));
712 }
713 else
714 {
715 t = -1.0/(-tau + hypot(1.0, tau));
716 }
717
718 *c = 1.0 / hypot(1.0, t);
719 *s = t * (*c);
720 }
721 else
722 {
723 *c = 1.0;
724 *s = 0.0;
725 }
726 }
727
svd2(gsl_vector * d,gsl_vector * f,gsl_matrix * U,gsl_matrix * V)728 void svd2 (gsl_vector * d, gsl_vector * f, gsl_matrix * U, gsl_matrix * V)
729 {
730
731 size_t i;
732 double c, s, a11, a12, a21, a22;
733
734 const size_t M = U->size1;
735 const size_t N = V->size1;
736
737 double d0 = gsl_vector_get (d, 0);
738 double f0 = gsl_vector_get (f, 0);
739
740 double d1 = gsl_vector_get (d, 1);
741
742 if (d0 == 0.0)
743 {
744 /* Eliminate off-diagonal element in [0,f0;0,d1] to make [d,0;0,0] */
745
746 create_givens (f0, d1, &c, &s);
747
748 /* compute B <= G^T B X, where X = [0,1;1,0] */
749
750 gsl_vector_set (d, 0, c * f0 - s * d1);
751 gsl_vector_set (f, 0, s * f0 + c * d1);
752 gsl_vector_set (d, 1, 0.0);
753
754 /* Compute U <= U G */
755
756 for (i = 0; i < M; i++)
757 {
758 double Uip = gsl_matrix_get (U, i, 0);
759 double Uiq = gsl_matrix_get (U, i, 1);
760 gsl_matrix_set (U, i, 0, c * Uip - s * Uiq);
761 gsl_matrix_set (U, i, 1, s * Uip + c * Uiq);
762 }
763
764 /* Compute V <= V X */
765
766 gsl_matrix_swap_columns (V, 0, 1);
767
768 return;
769 }
770 else if (d1 == 0.0)
771 {
772 /* Eliminate off-diagonal element in [d0,f0;0,0] */
773
774 create_givens (d0, f0, &c, &s);
775
776 /* compute B <= B G */
777
778 gsl_vector_set (d, 0, d0 * c - f0 * s);
779 gsl_vector_set (f, 0, 0.0);
780
781 /* Compute V <= V G */
782
783 for (i = 0; i < N; i++)
784 {
785 double Vip = gsl_matrix_get (V, i, 0);
786 double Viq = gsl_matrix_get (V, i, 1);
787 gsl_matrix_set (V, i, 0, c * Vip - s * Viq);
788 gsl_matrix_set (V, i, 1, s * Vip + c * Viq);
789 }
790
791 return;
792 }
793 else
794 {
795 /* Make columns orthogonal, A = [d0, f0; 0, d1] * G */
796
797 create_schur (d0, f0, d1, &c, &s);
798
799 /* compute B <= B G */
800
801 a11 = c * d0 - s * f0;
802 a21 = - s * d1;
803
804 a12 = s * d0 + c * f0;
805 a22 = c * d1;
806
807 /* Compute V <= V G */
808
809 for (i = 0; i < N; i++)
810 {
811 double Vip = gsl_matrix_get (V, i, 0);
812 double Viq = gsl_matrix_get (V, i, 1);
813 gsl_matrix_set (V, i, 0, c * Vip - s * Viq);
814 gsl_matrix_set (V, i, 1, s * Vip + c * Viq);
815 }
816
817 /* Eliminate off-diagonal elements, bring column with largest
818 norm to first column */
819
820 if (hypot(a11, a21) < hypot(a12,a22))
821 {
822 double t1, t2;
823
824 /* B <= B X */
825
826 t1 = a11; a11 = a12; a12 = t1;
827 t2 = a21; a21 = a22; a22 = t2;
828
829 /* V <= V X */
830
831 gsl_matrix_swap_columns(V, 0, 1);
832 }
833
834 create_givens (a11, a21, &c, &s);
835
836 /* compute B <= G^T B */
837
838 gsl_vector_set (d, 0, c * a11 - s * a21);
839 gsl_vector_set (f, 0, c * a12 - s * a22);
840 gsl_vector_set (d, 1, s * a12 + c * a22);
841
842 /* Compute U <= U G */
843
844 for (i = 0; i < M; i++)
845 {
846 double Uip = gsl_matrix_get (U, i, 0);
847 double Uiq = gsl_matrix_get (U, i, 1);
848 gsl_matrix_set (U, i, 0, c * Uip - s * Uiq);
849 gsl_matrix_set (U, i, 1, s * Uip + c * Uiq);
850 }
851
852 return;
853 }
854 }
855
856
chase_out_intermediate_zero(gsl_vector * d,gsl_vector * f,gsl_matrix * U,size_t k0)857 void chase_out_intermediate_zero (gsl_vector * d, gsl_vector * f, gsl_matrix * U, size_t k0)
858 {
859
860 #if !USE_BLAS
861 const size_t M = U->size1;
862 #endif
863 const size_t n = d->size;
864 double c, s;
865 double x, y;
866 size_t k;
867
868 x = gsl_vector_get (f, k0);
869 y = gsl_vector_get (d, k0+1);
870
871 for (k = k0; k < n - 1; k++)
872 {
873 create_givens (y, -x, &c, &s);
874
875 /* Compute U <= U G */
876
877 #ifdef USE_BLAS
878 {
879 gsl_vector_view Uk0 = gsl_matrix_column(U,k0);
880 gsl_vector_view Ukp1 = gsl_matrix_column(U,k+1);
881 gsl_blas_drot(&Uk0.vector, &Ukp1.vector, c, -s);
882 }
883 #else
884 {
885 size_t i;
886
887 for (i = 0; i < M; i++)
888 {
889 double Uip = gsl_matrix_get (U, i, k0);
890 double Uiq = gsl_matrix_get (U, i, k + 1);
891 gsl_matrix_set (U, i, k0, c * Uip - s * Uiq);
892 gsl_matrix_set (U, i, k + 1, s * Uip + c * Uiq);
893 }
894 }
895 #endif
896
897 /* compute B <= G^T B */
898
899 gsl_vector_set (d, k + 1, s * x + c * y);
900
901 if (k == k0)
902 gsl_vector_set (f, k, c * x - s * y );
903
904 if (k < n - 2)
905 {
906 double z = gsl_vector_get (f, k + 1);
907 gsl_vector_set (f, k + 1, c * z);
908
909 x = -s * z ;
910 y = gsl_vector_get (d, k + 2);
911 }
912 }
913 }
914
chase_out_trailing_zero(gsl_vector * d,gsl_vector * f,gsl_matrix * V)915 void chase_out_trailing_zero (gsl_vector * d, gsl_vector * f, gsl_matrix * V)
916 {
917 #if !USE_BLAS
918 const size_t N = V->size1;
919 #endif
920 const size_t n = d->size;
921 double c, s;
922 double x, y;
923 size_t k;
924
925 x = gsl_vector_get (d, n - 2);
926 y = gsl_vector_get (f, n - 2);
927
928 for (k = n - 1; k > 0 && k--;)
929 {
930 create_givens (x, y, &c, &s);
931
932 /* Compute V <= V G where G = [c, s ; -s, c] */
933
934 #ifdef USE_BLAS
935 {
936 gsl_vector_view Vp = gsl_matrix_column(V,k);
937 gsl_vector_view Vq = gsl_matrix_column(V,n-1);
938 gsl_blas_drot(&Vp.vector, &Vq.vector, c, -s);
939 }
940 #else
941 {
942 size_t i;
943
944 for (i = 0; i < N; i++)
945 {
946 double Vip = gsl_matrix_get (V, i, k);
947 double Viq = gsl_matrix_get (V, i, n - 1);
948 gsl_matrix_set (V, i, k, c * Vip - s * Viq);
949 gsl_matrix_set (V, i, n - 1, s * Vip + c * Viq);
950 }
951 }
952 #endif
953
954 /* compute B <= B G */
955
956 gsl_vector_set (d, k, c * x - s * y);
957
958 if (k == n - 2)
959 gsl_vector_set (f, k, s * x + c * y );
960
961 if (k > 0)
962 {
963 double z = gsl_vector_get (f, k - 1);
964 gsl_vector_set (f, k - 1, c * z);
965
966 x = gsl_vector_get (d, k - 1);
967 y = s * z ;
968 }
969 }
970 }
971
gsl_linalg_bidiag_unpack(const gsl_matrix * A,const gsl_vector * tau_U,gsl_matrix * U,const gsl_vector * tau_V,gsl_matrix * V,gsl_vector * diag,gsl_vector * superdiag)972 int gsl_linalg_bidiag_unpack (const gsl_matrix * A, const gsl_vector * tau_U,
973 gsl_matrix * U, const gsl_vector * tau_V,
974 gsl_matrix * V, gsl_vector * diag,
975 gsl_vector * superdiag) {
976 const size_t M = A->size1;
977 const size_t N = A->size2;
978
979 const size_t K = GSL_MIN(M, N);
980
981 if (M < N)
982 {
983 bpm_error("matrix A must have M >= N in gsl_linalg_bidiag_unpack(...)",
984 __FILE__, __LINE__);
985 return BPM_FAILURE;
986 }
987 else if (tau_U->size != K)
988 {
989 bpm_error("size of tau must be MIN(M,N) in gsl_linalg_bidiag_unpack(...)",
990 __FILE__, __LINE__);
991 return BPM_FAILURE;
992 }
993 else if (tau_V->size + 1 != K)
994 {
995 bpm_error("size of tau must be MIN(M,N) - 1 in gsl_linalg_bidiag_unpack(...)",
996 __FILE__, __LINE__);
997 return BPM_FAILURE;
998 }
999 else if (U->size1 != M || U->size2 != N)
1000 {
1001 bpm_error("size of U must be M x N in gsl_linalg_bidiag_unpack(...)",
1002 __FILE__, __LINE__);
1003 return BPM_FAILURE;
1004 }
1005 else if (V->size1 != N || V->size2 != N)
1006 {
1007 bpm_error("size of V must be N x N in gsl_linalg_bidiag_unpack(...)",
1008 __FILE__, __LINE__);
1009 return BPM_FAILURE;
1010 }
1011 else if (diag->size != K)
1012 {
1013 bpm_error("size of diagonal must match size of A in gsl_linalg_bidiag_unpack(...)",
1014 __FILE__, __LINE__);
1015 return BPM_FAILURE;
1016 }
1017 else if (superdiag->size + 1 != K)
1018 {
1019 bpm_error("size of subdiagonal must be (diagonal size - 1) in gsl_linalg_bidiag_unpack(...)",
1020 __FILE__, __LINE__);
1021 return BPM_FAILURE;
1022 }
1023 else
1024 {
1025 size_t i, j;
1026
1027 /* Copy diagonal into diag */
1028
1029 for (i = 0; i < N; i++)
1030 {
1031 double Aii = gsl_matrix_get (A, i, i);
1032 gsl_vector_set (diag, i, Aii);
1033 }
1034
1035 /* Copy superdiagonal into superdiag */
1036
1037 for (i = 0; i < N - 1; i++)
1038 {
1039 double Aij = gsl_matrix_get (A, i, i+1);
1040 gsl_vector_set (superdiag, i, Aij);
1041 }
1042
1043 /* Initialize V to the identity */
1044
1045 gsl_matrix_set_identity (V);
1046
1047 for (i = N - 1; i > 0 && i--;)
1048 {
1049 /* Householder row transformation to accumulate V */
1050 gsl_vector_const_view r = gsl_matrix_const_row (A, i);
1051 gsl_vector_const_view h =
1052 gsl_vector_const_subvector (&r.vector, i + 1, N - (i+1));
1053
1054 double ti = gsl_vector_get (tau_V, i);
1055
1056 gsl_matrix_view m =
1057 gsl_matrix_submatrix (V, i + 1, i + 1, N-(i+1), N-(i+1));
1058
1059 gsl_linalg_householder_hm (ti, &h.vector, &m.matrix);
1060 }
1061
1062 /* Initialize U to the identity */
1063
1064 gsl_matrix_set_identity (U);
1065
1066 for (j = N; j > 0 && j--;)
1067 {
1068 /* Householder column transformation to accumulate U */
1069 gsl_vector_const_view c = gsl_matrix_const_column (A, j);
1070 gsl_vector_const_view h = gsl_vector_const_subvector (&c.vector, j, M - j);
1071 double tj = gsl_vector_get (tau_U, j);
1072
1073 gsl_matrix_view m =
1074 gsl_matrix_submatrix (U, j, j, M-j, N-j);
1075
1076 gsl_linalg_householder_hm (tj, &h.vector, &m.matrix);
1077 }
1078
1079 return BPM_SUCCESS;
1080 }
1081 }
1082
gsl_linalg_bidiag_unpack2(gsl_matrix * A,gsl_vector * tau_U,gsl_vector * tau_V,gsl_matrix * V)1083 int gsl_linalg_bidiag_unpack2 (gsl_matrix * A, gsl_vector * tau_U, gsl_vector * tau_V, gsl_matrix * V)
1084 {
1085
1086 const size_t M = A->size1;
1087 const size_t N = A->size2;
1088
1089 const size_t K = GSL_MIN(M, N);
1090
1091 if (M < N)
1092 {
1093 bpm_error("matrix A must have M >= N in gsl_linalg_bidiag_unpack2(...)",
1094 __FILE__, __LINE__);
1095 return BPM_FAILURE;
1096 }
1097 else if (tau_U->size != K)
1098 {
1099 bpm_error("size of tau must be MIN(M,N) in gsl_linalg_bidiag_unpack2(...)",
1100 __FILE__, __LINE__);
1101 return BPM_FAILURE;
1102 }
1103 else if (tau_V->size + 1 != K)
1104 {
1105 bpm_error("size of tau must be MIN(M,N) - 1 in gsl_linalg_bidiag_unpack2(...)",
1106 __FILE__, __LINE__);
1107 return BPM_FAILURE;
1108 }
1109 else if (V->size1 != N || V->size2 != N)
1110 {
1111 bpm_error("size of V must be N x N in gsl_linalg_bidiag_unpack2(...)",
1112 __FILE__, __LINE__);
1113 return BPM_FAILURE;
1114 }
1115 else
1116 {
1117 size_t i, j;
1118
1119 /* Initialize V to the identity */
1120
1121 gsl_matrix_set_identity (V);
1122
1123 for (i = N - 1; i > 0 && i--;)
1124 {
1125 /* Householder row transformation to accumulate V */
1126 gsl_vector_const_view r = gsl_matrix_const_row (A, i);
1127 gsl_vector_const_view h =
1128 gsl_vector_const_subvector (&r.vector, i + 1, N - (i+1));
1129
1130 double ti = gsl_vector_get (tau_V, i);
1131
1132 gsl_matrix_view m =
1133 gsl_matrix_submatrix (V, i + 1, i + 1, N-(i+1), N-(i+1));
1134
1135 gsl_linalg_householder_hm (ti, &h.vector, &m.matrix);
1136 }
1137
1138 /* Copy superdiagonal into tau_v */
1139
1140 for (i = 0; i < N - 1; i++)
1141 {
1142 double Aij = gsl_matrix_get (A, i, i+1);
1143 gsl_vector_set (tau_V, i, Aij);
1144 }
1145
1146 /* Allow U to be unpacked into the same memory as A, copy
1147 diagonal into tau_U */
1148
1149 for (j = N; j > 0 && j--;)
1150 {
1151 /* Householder column transformation to accumulate U */
1152 double tj = gsl_vector_get (tau_U, j);
1153 double Ajj = gsl_matrix_get (A, j, j);
1154 gsl_matrix_view m = gsl_matrix_submatrix (A, j, j, M-j, N-j);
1155
1156 gsl_vector_set (tau_U, j, Ajj);
1157 gsl_linalg_householder_hm1 (tj, &m.matrix);
1158 }
1159
1160 return BPM_SUCCESS;
1161 }
1162 }
1163
gsl_linalg_SV_decomp(gsl_matrix * A,gsl_matrix * V,gsl_vector * S,gsl_vector * work)1164 int gsl_linalg_SV_decomp (gsl_matrix * A, gsl_matrix * V, gsl_vector * S,
1165 gsl_vector * work) {
1166
1167 size_t a, b, i, j;
1168
1169 const size_t M = A->size1;
1170 const size_t N = A->size2;
1171 const size_t K = GSL_MIN (M, N);
1172
1173 if (M < N)
1174 {
1175 bpm_error( "svd of MxN matrix, M<N, is not implemented in gsl_linalg_SV_solve(...)",
1176 __FILE__, __LINE__);
1177 }
1178 else if (V->size1 != N)
1179 {
1180 bpm_error( "square matrix V must match second dimension of matrix A in gsl_linalg_SV_solve(...)",
1181 __FILE__, __LINE__);
1182 }
1183 else if (V->size1 != V->size2)
1184 {
1185 bpm_error( "matrix V must be square in gsl_linalg_SV_solve(...)",
1186 __FILE__, __LINE__);
1187 }
1188 else if (S->size != N)
1189 {
1190 bpm_error( "length of vector S must match second dimension of matrix A in gsl_linalg_SV_solve(...)",
1191 __FILE__, __LINE__);
1192 }
1193 else if (work->size != N)
1194 {
1195 bpm_error( "length of workspace must match second dimension of matrix A in gsl_linalg_SV_solve(...)",
1196 __FILE__, __LINE__);
1197 }
1198
1199 /* Handle the case of N = 1 (SVD of a column vector) */
1200
1201 if (N == 1)
1202 {
1203 gsl_vector_view column = gsl_matrix_column (A, 0);
1204 double norm = gsl_blas_dnrm2 (&column.vector);
1205
1206 gsl_vector_set (S, 0, norm);
1207 gsl_matrix_set (V, 0, 0, 1.0);
1208
1209 if (norm != 0.0)
1210 {
1211 gsl_blas_dscal (1.0/norm, &column.vector);
1212 }
1213
1214 return BPM_SUCCESS;
1215 }
1216
1217 {
1218 gsl_vector_view f = gsl_vector_subvector (work, 0, K - 1);
1219
1220 /* bidiagonalize matrix A, unpack A into U S V */
1221
1222 gsl_linalg_bidiag_decomp (A, S, &f.vector);
1223 gsl_linalg_bidiag_unpack2 (A, S, &f.vector, V);
1224
1225 /* apply reduction steps to B=(S,Sd) */
1226 chop_small_elements (S, &f.vector);
1227
1228 /* Progressively reduce the matrix until it is diagonal */
1229
1230 b = N - 1;
1231
1232 while (b > 0)
1233 {
1234 double fbm1 = gsl_vector_get (&f.vector, b - 1);
1235
1236 if (fbm1 == 0.0 || gsl_isnan (fbm1))
1237 {
1238 b--;
1239 continue;
1240 }
1241
1242 /* Find the largest unreduced block (a,b) starting from b
1243 and working backwards */
1244
1245 a = b - 1;
1246
1247 while (a > 0)
1248 {
1249 double fam1 = gsl_vector_get (&f.vector, a - 1);
1250
1251 if (fam1 == 0.0 || gsl_isnan (fam1))
1252 {
1253 break;
1254 }
1255
1256 a--;
1257
1258 }
1259
1260 {
1261 const size_t n_block = b - a + 1;
1262
1263 gsl_vector_view S_block = gsl_vector_subvector (S, a, n_block);
1264 gsl_vector_view f_block = gsl_vector_subvector (&f.vector, a, n_block - 1);
1265
1266 gsl_matrix_view U_block =
1267 gsl_matrix_submatrix (A, 0, a, A->size1, n_block);
1268 gsl_matrix_view V_block =
1269 gsl_matrix_submatrix (V, 0, a, V->size1, n_block);
1270
1271 qrstep (&S_block.vector, &f_block.vector, &U_block.matrix, &V_block.matrix);
1272 /* remove any small off-diagonal elements */
1273
1274 chop_small_elements (&S_block.vector, &f_block.vector);
1275 }
1276 }
1277 }
1278 /* Make singular values positive by reflections if necessary */
1279
1280 for (j = 0; j < K; j++)
1281 {
1282 double Sj = gsl_vector_get (S, j);
1283
1284 if (Sj < 0.0)
1285 {
1286 for (i = 0; i < N; i++)
1287 {
1288 double Vij = gsl_matrix_get (V, i, j);
1289
1290 gsl_matrix_set (V, i, j, -Vij);
1291 }
1292
1293 gsl_vector_set (S, j, -Sj);
1294 }
1295 }
1296
1297 /* Sort singular values into decreasing order */
1298
1299 for (i = 0; i < K; i++)
1300 {
1301 double S_max = gsl_vector_get (S, i);
1302 size_t i_max = i;
1303
1304 for (j = i + 1; j < K; j++)
1305 {
1306 double Sj = gsl_vector_get (S, j);
1307
1308 if (Sj > S_max)
1309 {
1310 S_max = Sj;
1311 i_max = j;
1312 }
1313 }
1314
1315 if (i_max != i)
1316 {
1317 /* swap eigenvalues */
1318 gsl_vector_swap_elements (S, i, i_max);
1319
1320 /* swap eigenvectors */
1321 gsl_matrix_swap_columns (A, i, i_max);
1322 gsl_matrix_swap_columns (V, i, i_max);
1323 }
1324 }
1325
1326 return BPM_SUCCESS;
1327 }
1328
1329