|
Packit |
67cb25 |
/* linalg/svdstep.c
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* Copyright (C) 2007, 2010 Brian Gough
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* This program is free software; you can redistribute it and/or modify
|
|
Packit |
67cb25 |
* it under the terms of the GNU General Public License as published by
|
|
Packit |
67cb25 |
* the Free Software Foundation; either version 3 of the License, or (at
|
|
Packit |
67cb25 |
* your option) any later version.
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* This program is distributed in the hope that it will be useful, but
|
|
Packit |
67cb25 |
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
Packit |
67cb25 |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
Packit |
67cb25 |
* General Public License for more details.
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* You should have received a copy of the GNU General Public License
|
|
Packit |
67cb25 |
* along with this program; if not, write to the Free Software
|
|
Packit |
67cb25 |
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static void
|
|
Packit |
67cb25 |
chop_small_elements (gsl_vector * d, gsl_vector * f)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
const size_t N = d->size;
|
|
Packit |
67cb25 |
double d_i = gsl_vector_get (d, 0);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
size_t i;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (i = 0; i < N - 1; i++)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double f_i = gsl_vector_get (f, i);
|
|
Packit |
67cb25 |
double d_ip1 = gsl_vector_get (d, i + 1);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (fabs (f_i) < GSL_DBL_EPSILON * (fabs (d_i) + fabs (d_ip1)))
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_vector_set (f, i, 0.0);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
d_i = d_ip1;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static double
|
|
Packit |
67cb25 |
trailing_eigenvalue (const gsl_vector * d, const gsl_vector * f)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
const size_t n = d->size;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double da = gsl_vector_get (d, n - 2);
|
|
Packit |
67cb25 |
double db = gsl_vector_get (d, n - 1);
|
|
Packit |
67cb25 |
double fa = (n > 2) ? gsl_vector_get (f, n - 3) : 0.0;
|
|
Packit |
67cb25 |
double fb = gsl_vector_get (f, n - 2);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double mu;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
#if GOLUB_VAN_LOAN_8_3_2
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Golub and van Loan, Algorithm 8.3.2
|
|
Packit |
67cb25 |
The full SVD algorithm is described in section 8.6.2 */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double ta = da * da + fa * fa;
|
|
Packit |
67cb25 |
double tb = db * db + fb * fb;
|
|
Packit |
67cb25 |
double tab = da * fb;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double dt = (ta - tb) / 2.0;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (dt >= 0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
mu = tb - (tab * tab) / (dt + hypot (dt, tab));
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
mu = tb + (tab * tab) / ((-dt) + hypot (dt, tab));
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
#else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
/* We can compute mu more accurately than using the formula above
|
|
Packit |
67cb25 |
since we know the roots cannot be negative. This also avoids
|
|
Packit |
67cb25 |
the possibility of NaNs in the formula above.
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
The matrix is [ da^2 + fa^2, da fb ;
|
|
Packit |
67cb25 |
da fb , db^2 + fb^2 ]
|
|
Packit |
67cb25 |
and mu is the eigenvalue closest to the bottom right element.
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double ta = da * da + fa * fa;
|
|
Packit |
67cb25 |
double tb = db * db + fb * fb;
|
|
Packit |
67cb25 |
double tab = da * fb;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double dt = (ta - tb) / 2.0;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double S = ta + tb;
|
|
Packit |
67cb25 |
double da2 = da * da, db2 = db * db;
|
|
Packit |
67cb25 |
double fa2 = fa * fa, fb2 = fb * fb;
|
|
Packit |
67cb25 |
double P = (da2 * db2) + (fa2 * db2) + (fa2 * fb2);
|
|
Packit |
67cb25 |
double D = hypot(dt, tab);
|
|
Packit |
67cb25 |
double r1 = S/2 + D;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (dt >= 0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
/* tb < ta, choose smaller root */
|
|
Packit |
67cb25 |
mu = (r1 > 0) ? P / r1 : 0.0;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
/* tb > ta, choose larger root */
|
|
Packit |
67cb25 |
mu = r1;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
#endif
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return mu;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static void
|
|
Packit |
67cb25 |
create_schur (double d0, double f0, double d1, double * c, double * s)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double apq = 2.0 * d0 * f0;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (d0 == 0 || f0 == 0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
*c = 1.0;
|
|
Packit |
67cb25 |
*s = 0.0;
|
|
Packit |
67cb25 |
return;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Check if we need to rescale to avoid underflow/overflow */
|
|
Packit |
67cb25 |
if (fabs(d0) < GSL_SQRT_DBL_MIN || fabs(d0) > GSL_SQRT_DBL_MAX
|
|
Packit |
67cb25 |
|| fabs(f0) < GSL_SQRT_DBL_MIN || fabs(f0) > GSL_SQRT_DBL_MAX
|
|
Packit |
67cb25 |
|| fabs(d1) < GSL_SQRT_DBL_MIN || fabs(d1) > GSL_SQRT_DBL_MAX)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double scale;
|
|
Packit |
67cb25 |
int d0_exp, f0_exp;
|
|
Packit |
67cb25 |
frexp(d0, &d0_exp);
|
|
Packit |
67cb25 |
frexp(f0, &f0_exp);
|
|
Packit |
67cb25 |
/* Bring |d0*f0| into the range GSL_DBL_MIN to GSL_DBL_MAX */
|
|
Packit |
67cb25 |
scale = ldexp(1.0, -(d0_exp + f0_exp)/4);
|
|
Packit |
67cb25 |
d0 *= scale;
|
|
Packit |
67cb25 |
f0 *= scale;
|
|
Packit |
67cb25 |
d1 *= scale;
|
|
Packit |
67cb25 |
apq = 2.0 * d0 * f0;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (apq != 0.0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double t;
|
|
Packit |
67cb25 |
double tau = (f0*f0 + (d1 + d0)*(d1 - d0)) / apq;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (tau >= 0.0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
t = 1.0/(tau + hypot(1.0, tau));
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
t = -1.0/(-tau + hypot(1.0, tau));
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
*c = 1.0 / hypot(1.0, t);
|
|
Packit |
67cb25 |
*s = t * (*c);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
*c = 1.0;
|
|
Packit |
67cb25 |
*s = 0.0;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static void
|
|
Packit |
67cb25 |
svd2 (gsl_vector * d, gsl_vector * f, gsl_matrix * U, gsl_matrix * V)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
size_t i;
|
|
Packit |
67cb25 |
double c, s, a11, a12, a21, a22;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
const size_t M = U->size1;
|
|
Packit |
67cb25 |
const size_t N = V->size1;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double d0 = gsl_vector_get (d, 0);
|
|
Packit |
67cb25 |
double f0 = gsl_vector_get (f, 0);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double d1 = gsl_vector_get (d, 1);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (d0 == 0.0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
/* Eliminate off-diagonal element in [0,f0;0,d1] to make [d,0;0,0] */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_linalg_givens (f0, d1, &c, &s);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute B <= G^T B X, where X = [0,1;1,0] */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_vector_set (d, 0, c * f0 - s * d1);
|
|
Packit |
67cb25 |
gsl_vector_set (f, 0, s * f0 + c * d1);
|
|
Packit |
67cb25 |
gsl_vector_set (d, 1, 0.0);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Compute U <= U G */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (i = 0; i < M; i++)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double Uip = gsl_matrix_get (U, i, 0);
|
|
Packit |
67cb25 |
double Uiq = gsl_matrix_get (U, i, 1);
|
|
Packit |
67cb25 |
gsl_matrix_set (U, i, 0, c * Uip - s * Uiq);
|
|
Packit |
67cb25 |
gsl_matrix_set (U, i, 1, s * Uip + c * Uiq);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Compute V <= V X */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_matrix_swap_columns (V, 0, 1);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else if (d1 == 0.0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
/* Eliminate off-diagonal element in [d0,f0;0,0] */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_linalg_givens (d0, f0, &c, &s);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute B <= B G */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_vector_set (d, 0, d0 * c - f0 * s);
|
|
Packit |
67cb25 |
gsl_vector_set (f, 0, 0.0);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Compute V <= V G */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (i = 0; i < N; i++)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double Vip = gsl_matrix_get (V, i, 0);
|
|
Packit |
67cb25 |
double Viq = gsl_matrix_get (V, i, 1);
|
|
Packit |
67cb25 |
gsl_matrix_set (V, i, 0, c * Vip - s * Viq);
|
|
Packit |
67cb25 |
gsl_matrix_set (V, i, 1, s * Vip + c * Viq);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
/* Make columns orthogonal, A = [d0, f0; 0, d1] * G */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
create_schur (d0, f0, d1, &c, &s);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute B <= B G */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
a11 = c * d0 - s * f0;
|
|
Packit |
67cb25 |
a21 = - s * d1;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
a12 = s * d0 + c * f0;
|
|
Packit |
67cb25 |
a22 = c * d1;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Compute V <= V G */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (i = 0; i < N; i++)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double Vip = gsl_matrix_get (V, i, 0);
|
|
Packit |
67cb25 |
double Viq = gsl_matrix_get (V, i, 1);
|
|
Packit |
67cb25 |
gsl_matrix_set (V, i, 0, c * Vip - s * Viq);
|
|
Packit |
67cb25 |
gsl_matrix_set (V, i, 1, s * Vip + c * Viq);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Eliminate off-diagonal elements, bring column with largest
|
|
Packit |
67cb25 |
norm to first column */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (hypot(a11, a21) < hypot(a12,a22))
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double t1, t2;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* B <= B X */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
t1 = a11; a11 = a12; a12 = t1;
|
|
Packit |
67cb25 |
t2 = a21; a21 = a22; a22 = t2;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* V <= V X */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_matrix_swap_columns(V, 0, 1);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_linalg_givens (a11, a21, &c, &s);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute B <= G^T B */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_vector_set (d, 0, c * a11 - s * a21);
|
|
Packit |
67cb25 |
gsl_vector_set (f, 0, c * a12 - s * a22);
|
|
Packit |
67cb25 |
gsl_vector_set (d, 1, s * a12 + c * a22);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Compute U <= U G */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (i = 0; i < M; i++)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double Uip = gsl_matrix_get (U, i, 0);
|
|
Packit |
67cb25 |
double Uiq = gsl_matrix_get (U, i, 1);
|
|
Packit |
67cb25 |
gsl_matrix_set (U, i, 0, c * Uip - s * Uiq);
|
|
Packit |
67cb25 |
gsl_matrix_set (U, i, 1, s * Uip + c * Uiq);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static void
|
|
Packit |
67cb25 |
chase_out_intermediate_zero (gsl_vector * d, gsl_vector * f, gsl_matrix * U, size_t k0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
#if !USE_BLAS
|
|
Packit |
67cb25 |
const size_t M = U->size1;
|
|
Packit |
67cb25 |
#endif
|
|
Packit |
67cb25 |
const size_t n = d->size;
|
|
Packit |
67cb25 |
double c, s;
|
|
Packit |
67cb25 |
double x, y;
|
|
Packit |
67cb25 |
size_t k;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
x = gsl_vector_get (f, k0);
|
|
Packit |
67cb25 |
y = gsl_vector_get (d, k0+1);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (k = k0; k < n - 1; k++)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_linalg_givens (y, -x, &c, &s);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Compute U <= U G */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
#ifdef USE_BLAS
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_vector_view Uk0 = gsl_matrix_column(U,k0);
|
|
Packit |
67cb25 |
gsl_vector_view Ukp1 = gsl_matrix_column(U,k+1);
|
|
Packit |
67cb25 |
gsl_blas_drot(&Uk0.vector, &Ukp1.vector, c, -s);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
#else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
size_t i;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (i = 0; i < M; i++)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double Uip = gsl_matrix_get (U, i, k0);
|
|
Packit |
67cb25 |
double Uiq = gsl_matrix_get (U, i, k + 1);
|
|
Packit |
67cb25 |
gsl_matrix_set (U, i, k0, c * Uip - s * Uiq);
|
|
Packit |
67cb25 |
gsl_matrix_set (U, i, k + 1, s * Uip + c * Uiq);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
#endif
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute B <= G^T B */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_vector_set (d, k + 1, s * x + c * y);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (k == k0)
|
|
Packit |
67cb25 |
gsl_vector_set (f, k, c * x - s * y );
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (k < n - 2)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double z = gsl_vector_get (f, k + 1);
|
|
Packit |
67cb25 |
gsl_vector_set (f, k + 1, c * z);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
x = -s * z ;
|
|
Packit |
67cb25 |
y = gsl_vector_get (d, k + 2);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static void
|
|
Packit |
67cb25 |
chase_out_trailing_zero (gsl_vector * d, gsl_vector * f, gsl_matrix * V)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
#if !USE_BLAS
|
|
Packit |
67cb25 |
const size_t N = V->size1;
|
|
Packit |
67cb25 |
#endif
|
|
Packit |
67cb25 |
const size_t n = d->size;
|
|
Packit |
67cb25 |
double c, s;
|
|
Packit |
67cb25 |
double x, y;
|
|
Packit |
67cb25 |
size_t k;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
x = gsl_vector_get (d, n - 2);
|
|
Packit |
67cb25 |
y = gsl_vector_get (f, n - 2);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (k = n - 1; k-- > 0;)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_linalg_givens (x, y, &c, &s);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Compute V <= V G where G = [c, s ; -s, c] */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
#ifdef USE_BLAS
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_vector_view Vp = gsl_matrix_column(V,k);
|
|
Packit |
67cb25 |
gsl_vector_view Vq = gsl_matrix_column(V,n-1);
|
|
Packit |
67cb25 |
gsl_blas_drot(&Vp.vector, &Vq.vector, c, -s);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
#else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
size_t i;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (i = 0; i < N; i++)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double Vip = gsl_matrix_get (V, i, k);
|
|
Packit |
67cb25 |
double Viq = gsl_matrix_get (V, i, n - 1);
|
|
Packit |
67cb25 |
gsl_matrix_set (V, i, k, c * Vip - s * Viq);
|
|
Packit |
67cb25 |
gsl_matrix_set (V, i, n - 1, s * Vip + c * Viq);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
#endif
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute B <= B G */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_vector_set (d, k, c * x - s * y);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (k == n - 2)
|
|
Packit |
67cb25 |
gsl_vector_set (f, k, s * x + c * y );
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (k > 0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double z = gsl_vector_get (f, k - 1);
|
|
Packit |
67cb25 |
gsl_vector_set (f, k - 1, c * z);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
x = gsl_vector_get (d, k - 1);
|
|
Packit |
67cb25 |
y = s * z ;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static void
|
|
Packit |
67cb25 |
qrstep (gsl_vector * d, gsl_vector * f, gsl_matrix * U, gsl_matrix * V)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
#if !USE_BLAS
|
|
Packit |
67cb25 |
const size_t M = U->size1;
|
|
Packit |
67cb25 |
const size_t N = V->size1;
|
|
Packit |
67cb25 |
#endif
|
|
Packit |
67cb25 |
const size_t n = d->size;
|
|
Packit |
67cb25 |
double y, z;
|
|
Packit |
67cb25 |
double ak, bk, zk, ap, bp, aq;
|
|
Packit |
67cb25 |
size_t i, k;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (n == 1)
|
|
Packit |
67cb25 |
return; /* shouldn't happen */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Compute 2x2 svd directly */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (n == 2)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
svd2 (d, f, U, V);
|
|
Packit |
67cb25 |
return;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Chase out any zeroes on the diagonal */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (i = 0; i < n - 1; i++)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double d_i = gsl_vector_get (d, i);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (d_i == 0.0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
chase_out_intermediate_zero (d, f, U, i);
|
|
Packit |
67cb25 |
return;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Chase out any zero at the end of the diagonal */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double d_nm1 = gsl_vector_get (d, n - 1);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (d_nm1 == 0.0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
chase_out_trailing_zero (d, f, V);
|
|
Packit |
67cb25 |
return;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Apply QR reduction steps to the diagonal and offdiagonal */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double d0 = gsl_vector_get (d, 0);
|
|
Packit |
67cb25 |
double f0 = gsl_vector_get (f, 0);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double d1 = gsl_vector_get (d, 1);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double mu = trailing_eigenvalue (d, f);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
y = d0 * d0 - mu;
|
|
Packit |
67cb25 |
z = d0 * f0;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Set up the recurrence for Givens rotations on a bidiagonal matrix */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
ak = 0;
|
|
Packit |
67cb25 |
bk = 0;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
ap = d0;
|
|
Packit |
67cb25 |
bp = f0;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
aq = d1;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
for (k = 0; k < n - 1; k++)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double c, s;
|
|
Packit |
67cb25 |
gsl_linalg_givens (y, z, &c, &s);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Compute V <= V G */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
#ifdef USE_BLAS
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_vector_view Vk = gsl_matrix_column(V,k);
|
|
Packit |
67cb25 |
gsl_vector_view Vkp1 = gsl_matrix_column(V,k+1);
|
|
Packit |
67cb25 |
gsl_blas_drot(&Vk.vector, &Vkp1.vector, c, -s);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
#else
|
|
Packit |
67cb25 |
for (i = 0; i < N; i++)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double Vip = gsl_matrix_get (V, i, k);
|
|
Packit |
67cb25 |
double Viq = gsl_matrix_get (V, i, k + 1);
|
|
Packit |
67cb25 |
gsl_matrix_set (V, i, k, c * Vip - s * Viq);
|
|
Packit |
67cb25 |
gsl_matrix_set (V, i, k + 1, s * Vip + c * Viq);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
#endif
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute B <= B G */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double bk1 = c * bk - s * z;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double ap1 = c * ap - s * bp;
|
|
Packit |
67cb25 |
double bp1 = s * ap + c * bp;
|
|
Packit |
67cb25 |
double zp1 = -s * aq;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double aq1 = c * aq;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (k > 0)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_vector_set (f, k - 1, bk1);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
ak = ap1;
|
|
Packit |
67cb25 |
bk = bp1;
|
|
Packit |
67cb25 |
zk = zp1;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
ap = aq1;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (k < n - 2)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
bp = gsl_vector_get (f, k + 1);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
bp = 0.0;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
y = ak;
|
|
Packit |
67cb25 |
z = zk;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_linalg_givens (y, z, &c, &s);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* Compute U <= U G */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
#ifdef USE_BLAS
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
gsl_vector_view Uk = gsl_matrix_column(U,k);
|
|
Packit |
67cb25 |
gsl_vector_view Ukp1 = gsl_matrix_column(U,k+1);
|
|
Packit |
67cb25 |
gsl_blas_drot(&Uk.vector, &Ukp1.vector, c, -s);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
#else
|
|
Packit |
67cb25 |
for (i = 0; i < M; i++)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double Uip = gsl_matrix_get (U, i, k);
|
|
Packit |
67cb25 |
double Uiq = gsl_matrix_get (U, i, k + 1);
|
|
Packit |
67cb25 |
gsl_matrix_set (U, i, k, c * Uip - s * Uiq);
|
|
Packit |
67cb25 |
gsl_matrix_set (U, i, k + 1, s * Uip + c * Uiq);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
#endif
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute B <= G^T B */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double ak1 = c * ak - s * zk;
|
|
Packit |
67cb25 |
double bk1 = c * bk - s * ap;
|
|
Packit |
67cb25 |
double zk1 = -s * bp;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
double ap1 = s * bk + c * ap;
|
|
Packit |
67cb25 |
double bp1 = c * bp;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_vector_set (d, k, ak1);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
ak = ak1;
|
|
Packit |
67cb25 |
bk = bk1;
|
|
Packit |
67cb25 |
zk = zk1;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
ap = ap1;
|
|
Packit |
67cb25 |
bp = bp1;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (k < n - 2)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
aq = gsl_vector_get (d, k + 2);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
else
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
aq = 0.0;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
y = bk;
|
|
Packit |
67cb25 |
z = zk;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_vector_set (f, n - 2, bk);
|
|
Packit |
67cb25 |
gsl_vector_set (d, n - 1, ap);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
|