00001
00005 #include <bpm/bpm_messages.h>
00006 #include <bpm/bpm_nr.h>
00007
00008
00009
00010
00011 void chop_small_elements (gsl_vector * d, gsl_vector * f)
00012 {
00013 const size_t N = d->size;
00014 double d_i = gsl_vector_get (d, 0);
00015
00016 size_t i;
00017
00018 for (i = 0; i < N - 1; i++)
00019 {
00020 double f_i = gsl_vector_get (f, i);
00021 double d_ip1 = gsl_vector_get (d, i + 1);
00022
00023 if (fabs (f_i) < GSL_DBL_EPSILON * (fabs (d_i) + fabs (d_ip1)))
00024 {
00025 gsl_vector_set (f, i, 0.0);
00026 }
00027 d_i = d_ip1;
00028 }
00029
00030 }
00031
00032 void qrstep (gsl_vector * d, gsl_vector * f, gsl_matrix * U, gsl_matrix * V)
00033 {
00034 #if !USE_BLAS
00035 const size_t M = U->size1;
00036 const size_t N = V->size1;
00037 #endif
00038 const size_t n = d->size;
00039 double y, z;
00040 double ak, bk, zk, ap, bp, aq, bq;
00041 size_t i, k;
00042
00043 if (n == 1)
00044 return;
00045
00046
00047
00048 if (n == 2)
00049 {
00050 svd2 (d, f, U, V);
00051 return;
00052 }
00053
00054
00055
00056 for (i = 0; i < n - 1; i++)
00057 {
00058 double d_i = gsl_vector_get (d, i);
00059
00060 if (d_i == 0.0)
00061 {
00062 chase_out_intermediate_zero (d, f, U, i);
00063 return;
00064 }
00065 }
00066
00067
00068
00069 {
00070 double d_nm1 = gsl_vector_get (d, n - 1);
00071
00072 if (d_nm1 == 0.0)
00073 {
00074 chase_out_trailing_zero (d, f, V);
00075 return;
00076 }
00077 }
00078
00079
00080
00081
00082 {
00083 double d0 = gsl_vector_get (d, 0);
00084 double f0 = gsl_vector_get (f, 0);
00085
00086 double d1 = gsl_vector_get (d, 1);
00087 double f1 = gsl_vector_get (f, 1);
00088
00089 {
00090 double mu = trailing_eigenvalue (d, f);
00091
00092 y = d0 * d0 - mu;
00093 z = d0 * f0;
00094 }
00095
00096
00097
00098 ak = 0;
00099 bk = 0;
00100
00101 ap = d0;
00102 bp = f0;
00103
00104 aq = d1;
00105 bq = f1;
00106 }
00107
00108 for (k = 0; k < n - 1; k++)
00109 {
00110 double c, s;
00111 create_givens (y, z, &c, &s);
00112
00113
00114
00115 #ifdef USE_BLAS
00116 {
00117 gsl_vector_view Vk = gsl_matrix_column(V,k);
00118 gsl_vector_view Vkp1 = gsl_matrix_column(V,k+1);
00119 gsl_blas_drot(&Vk.vector, &Vkp1.vector, c, -s);
00120 }
00121 #else
00122 for (i = 0; i < N; i++)
00123 {
00124 double Vip = gsl_matrix_get (V, i, k);
00125 double Viq = gsl_matrix_get (V, i, k + 1);
00126 gsl_matrix_set (V, i, k, c * Vip - s * Viq);
00127 gsl_matrix_set (V, i, k + 1, s * Vip + c * Viq);
00128 }
00129 #endif
00130
00131
00132
00133 {
00134 double bk1 = c * bk - s * z;
00135
00136 double ap1 = c * ap - s * bp;
00137 double bp1 = s * ap + c * bp;
00138 double zp1 = -s * aq;
00139
00140 double aq1 = c * aq;
00141
00142 if (k > 0)
00143 {
00144 gsl_vector_set (f, k - 1, bk1);
00145 }
00146
00147 ak = ap1;
00148 bk = bp1;
00149 zk = zp1;
00150
00151 ap = aq1;
00152
00153 if (k < n - 2)
00154 {
00155 bp = gsl_vector_get (f, k + 1);
00156 }
00157 else
00158 {
00159 bp = 0.0;
00160 }
00161
00162 y = ak;
00163 z = zk;
00164 }
00165
00166 create_givens (y, z, &c, &s);
00167
00168
00169
00170 #ifdef USE_BLAS
00171 {
00172 gsl_vector_view Uk = gsl_matrix_column(U,k);
00173 gsl_vector_view Ukp1 = gsl_matrix_column(U,k+1);
00174 gsl_blas_drot(&Uk.vector, &Ukp1.vector, c, -s);
00175 }
00176 #else
00177 for (i = 0; i < M; i++)
00178 {
00179 double Uip = gsl_matrix_get (U, i, k);
00180 double Uiq = gsl_matrix_get (U, i, k + 1);
00181 gsl_matrix_set (U, i, k, c * Uip - s * Uiq);
00182 gsl_matrix_set (U, i, k + 1, s * Uip + c * Uiq);
00183 }
00184 #endif
00185
00186
00187
00188 {
00189 double ak1 = c * ak - s * zk;
00190 double bk1 = c * bk - s * ap;
00191 double zk1 = -s * bp;
00192
00193 double ap1 = s * bk + c * ap;
00194 double bp1 = c * bp;
00195
00196 gsl_vector_set (d, k, ak1);
00197
00198 ak = ak1;
00199 bk = bk1;
00200 zk = zk1;
00201
00202 ap = ap1;
00203 bp = bp1;
00204
00205 if (k < n - 2)
00206 {
00207 aq = gsl_vector_get (d, k + 2);
00208 }
00209 else
00210 {
00211 aq = 0.0;
00212 }
00213
00214 y = bk;
00215 z = zk;
00216 }
00217 }
00218
00219 gsl_vector_set (f, n - 2, bk);
00220 gsl_vector_set (d, n - 1, ap);
00221 }
00222
00223 double trailing_eigenvalue (const gsl_vector * d, const gsl_vector * f)
00224 {
00225 const size_t n = d->size;
00226
00227 double da = gsl_vector_get (d, n - 2);
00228 double db = gsl_vector_get (d, n - 1);
00229 double fa = (n > 2) ? gsl_vector_get (f, n - 3) : 0.0;
00230 double fb = gsl_vector_get (f, n - 2);
00231
00232 double ta = da * da + fa * fa;
00233 double tb = db * db + fb * fb;
00234 double tab = da * fb;
00235
00236 double dt = (ta - tb) / 2.0;
00237
00238 double mu;
00239
00240 if (dt >= 0)
00241 {
00242 mu = tb - (tab * tab) / (dt + hypot (dt, tab));
00243 }
00244 else
00245 {
00246 mu = tb + (tab * tab) / ((-dt) + hypot (dt, tab));
00247 }
00248
00249 return mu;
00250 }
00251
00252 void create_schur (double d0, double f0, double d1, double * c, double * s)
00253 {
00254 double apq = 2.0 * d0 * f0;
00255
00256 if (apq != 0.0)
00257 {
00258 double t;
00259 double tau = (f0*f0 + (d1 + d0)*(d1 - d0)) / apq;
00260
00261 if (tau >= 0.0)
00262 {
00263 t = 1.0/(tau + hypot(1.0, tau));
00264 }
00265 else
00266 {
00267 t = -1.0/(-tau + hypot(1.0, tau));
00268 }
00269
00270 *c = 1.0 / hypot(1.0, t);
00271 *s = t * (*c);
00272 }
00273 else
00274 {
00275 *c = 1.0;
00276 *s = 0.0;
00277 }
00278 }
00279
00280 void svd2 (gsl_vector * d, gsl_vector * f, gsl_matrix * U, gsl_matrix * V)
00281 {
00282 size_t i;
00283 double c, s, a11, a12, a21, a22;
00284
00285 const size_t M = U->size1;
00286 const size_t N = V->size1;
00287
00288 double d0 = gsl_vector_get (d, 0);
00289 double f0 = gsl_vector_get (f, 0);
00290
00291 double d1 = gsl_vector_get (d, 1);
00292
00293 if (d0 == 0.0)
00294 {
00295
00296
00297 create_givens (f0, d1, &c, &s);
00298
00299
00300
00301 gsl_vector_set (d, 0, c * f0 - s * d1);
00302 gsl_vector_set (f, 0, s * f0 + c * d1);
00303 gsl_vector_set (d, 1, 0.0);
00304
00305
00306
00307 for (i = 0; i < M; i++)
00308 {
00309 double Uip = gsl_matrix_get (U, i, 0);
00310 double Uiq = gsl_matrix_get (U, i, 1);
00311 gsl_matrix_set (U, i, 0, c * Uip - s * Uiq);
00312 gsl_matrix_set (U, i, 1, s * Uip + c * Uiq);
00313 }
00314
00315
00316
00317 gsl_matrix_swap_columns (V, 0, 1);
00318
00319 return;
00320 }
00321 else if (d1 == 0.0)
00322 {
00323
00324
00325 create_givens (d0, f0, &c, &s);
00326
00327
00328
00329 gsl_vector_set (d, 0, d0 * c - f0 * s);
00330 gsl_vector_set (f, 0, 0.0);
00331
00332
00333
00334 for (i = 0; i < N; i++)
00335 {
00336 double Vip = gsl_matrix_get (V, i, 0);
00337 double Viq = gsl_matrix_get (V, i, 1);
00338 gsl_matrix_set (V, i, 0, c * Vip - s * Viq);
00339 gsl_matrix_set (V, i, 1, s * Vip + c * Viq);
00340 }
00341
00342 return;
00343 }
00344 else
00345 {
00346
00347
00348 create_schur (d0, f0, d1, &c, &s);
00349
00350
00351
00352 a11 = c * d0 - s * f0;
00353 a21 = - s * d1;
00354
00355 a12 = s * d0 + c * f0;
00356 a22 = c * d1;
00357
00358
00359
00360 for (i = 0; i < N; i++)
00361 {
00362 double Vip = gsl_matrix_get (V, i, 0);
00363 double Viq = gsl_matrix_get (V, i, 1);
00364 gsl_matrix_set (V, i, 0, c * Vip - s * Viq);
00365 gsl_matrix_set (V, i, 1, s * Vip + c * Viq);
00366 }
00367
00368
00369
00370
00371 if (hypot(a11, a21) < hypot(a12,a22))
00372 {
00373 double t1, t2;
00374
00375
00376
00377 t1 = a11; a11 = a12; a12 = t1;
00378 t2 = a21; a21 = a22; a22 = t2;
00379
00380
00381
00382 gsl_matrix_swap_columns(V, 0, 1);
00383 }
00384
00385 create_givens (a11, a21, &c, &s);
00386
00387
00388
00389 gsl_vector_set (d, 0, c * a11 - s * a21);
00390 gsl_vector_set (f, 0, c * a12 - s * a22);
00391 gsl_vector_set (d, 1, s * a12 + c * a22);
00392
00393
00394
00395 for (i = 0; i < M; i++)
00396 {
00397 double Uip = gsl_matrix_get (U, i, 0);
00398 double Uiq = gsl_matrix_get (U, i, 1);
00399 gsl_matrix_set (U, i, 0, c * Uip - s * Uiq);
00400 gsl_matrix_set (U, i, 1, s * Uip + c * Uiq);
00401 }
00402
00403 return;
00404 }
00405 }
00406
00407
00408 void chase_out_intermediate_zero (gsl_vector * d, gsl_vector * f, gsl_matrix * U, size_t k0)
00409 {
00410 #if !USE_BLAS
00411 const size_t M = U->size1;
00412 #endif
00413 const size_t n = d->size;
00414 double c, s;
00415 double x, y;
00416 size_t k;
00417
00418 x = gsl_vector_get (f, k0);
00419 y = gsl_vector_get (d, k0+1);
00420
00421 for (k = k0; k < n - 1; k++)
00422 {
00423 create_givens (y, -x, &c, &s);
00424
00425
00426
00427 #ifdef USE_BLAS
00428 {
00429 gsl_vector_view Uk0 = gsl_matrix_column(U,k0);
00430 gsl_vector_view Ukp1 = gsl_matrix_column(U,k+1);
00431 gsl_blas_drot(&Uk0.vector, &Ukp1.vector, c, -s);
00432 }
00433 #else
00434 {
00435 size_t i;
00436
00437 for (i = 0; i < M; i++)
00438 {
00439 double Uip = gsl_matrix_get (U, i, k0);
00440 double Uiq = gsl_matrix_get (U, i, k + 1);
00441 gsl_matrix_set (U, i, k0, c * Uip - s * Uiq);
00442 gsl_matrix_set (U, i, k + 1, s * Uip + c * Uiq);
00443 }
00444 }
00445 #endif
00446
00447
00448
00449 gsl_vector_set (d, k + 1, s * x + c * y);
00450
00451 if (k == k0)
00452 gsl_vector_set (f, k, c * x - s * y );
00453
00454 if (k < n - 2)
00455 {
00456 double z = gsl_vector_get (f, k + 1);
00457 gsl_vector_set (f, k + 1, c * z);
00458
00459 x = -s * z ;
00460 y = gsl_vector_get (d, k + 2);
00461 }
00462 }
00463 }
00464
00465 void chase_out_trailing_zero (gsl_vector * d, gsl_vector * f, gsl_matrix * V)
00466 {
00467 #if !USE_BLAS
00468 const size_t N = V->size1;
00469 #endif
00470 const size_t n = d->size;
00471 double c, s;
00472 double x, y;
00473 size_t k;
00474
00475 x = gsl_vector_get (d, n - 2);
00476 y = gsl_vector_get (f, n - 2);
00477
00478 for (k = n - 1; k > 0 && k--;)
00479 {
00480 create_givens (x, y, &c, &s);
00481
00482
00483
00484 #ifdef USE_BLAS
00485 {
00486 gsl_vector_view Vp = gsl_matrix_column(V,k);
00487 gsl_vector_view Vq = gsl_matrix_column(V,n-1);
00488 gsl_blas_drot(&Vp.vector, &Vq.vector, c, -s);
00489 }
00490 #else
00491 {
00492 size_t i;
00493
00494 for (i = 0; i < N; i++)
00495 {
00496 double Vip = gsl_matrix_get (V, i, k);
00497 double Viq = gsl_matrix_get (V, i, n - 1);
00498 gsl_matrix_set (V, i, k, c * Vip - s * Viq);
00499 gsl_matrix_set (V, i, n - 1, s * Vip + c * Viq);
00500 }
00501 }
00502 #endif
00503
00504
00505
00506 gsl_vector_set (d, k, c * x - s * y);
00507
00508 if (k == n - 2)
00509 gsl_vector_set (f, k, s * x + c * y );
00510
00511 if (k > 0)
00512 {
00513 double z = gsl_vector_get (f, k - 1);
00514 gsl_vector_set (f, k - 1, c * z);
00515
00516 x = gsl_vector_get (d, k - 1);
00517 y = s * z ;
00518 }
00519 }
00520 }