void
eigen_symm2x2 (float A11, float A12, float A22, float *l1, float *l2,
float *e11, float *e12, float *e21, float *e22)
{
struct mat2x2 A;
struct mat2x2 E = {1, 0, 0, 1};
struct mat2x2 Q, Qt;
float c, s;
A.a = A11; A.b = A.c = A12; A.d = A22;
chop_small_elements (&A);
if (!((A.b == 0.0f) || isnan (A.b))) {
float x = A.a - trailing_eigenvalue (A);
float z = A.b;
/* Calculate Givens rotation coefficients. */
if (z == 0) {
s = 0; c = 1;
} else if (fabsf (z) > fabsf (x)) {
float t = -x / z;
float u = copysign (sqrtf (1 + t*t), 1);
s = 1.0 / u;
c = s * t;
} else {
float t = -z / x;
float u = copysign (sqrtf (1 + t*t), 1);
c = 1.0 / u;
s = c * t;
}
/* Create rotation matrices */
Qt.a = Q.d = c;
Qt.b = -s;
Qt.c = s;
Q.a = Qt.d = c;
Q.b = s;
Q.c = -s;
/* Rotate source matrix and eigenvector matrix together */
A = mat2x2mul (mat2x2mul (Q, A), Qt);
E = Qt;
}
/* Return values */
*l1 = A.a;
*l2 = A.d;
*e11 = E.a;
*e12 = E.b;
*e21 = E.c;
*e22 = E.d;
}
void qrstep (gsl_vector * d, gsl_vector * f, gsl_matrix * U,
gsl_matrix * V) {
const size_t M=U->size1;
const size_t N=V->size1;
const size_t n=d->size;
double y, z;
double ak, bk, zk, ap, bp, aq, bq;
size_t i, k;
//std::cout << "M,N,n: " << M << " " << N << " " << n << std::endl;
if (n == 1)
return; /* shouldn't happen */
/* Compute 2x2 svd directly */
if (n == 2)
{
svd2 (d, f, U, V);
return;
}
/* Chase out any zeroes on the diagonal */
for (i=0; i < n - 1; i++)
{
double d_i=gsl_vector_get (d, i);
//std::cout << "d_i: " << i << " " << n << " "
//<< d_i << std::endl;
if (d_i == 0.0)
{
chase_out_intermediate_zero (d, f, U, i);
return;
}
}
/* Chase out any zero at the end of the diagonal */
{
double d_nm1=gsl_vector_get (d, n - 1);
//std::cout << "d_nm1: " << d_nm1 << std::endl;
if (d_nm1 == 0.0)
{
chase_out_trailing_zero (d, f, V);
return;
}
}
/* Apply QR reduction steps to the diagonal and offdiagonal */
{
double d0=gsl_vector_get (d, 0);
double f0=gsl_vector_get (f, 0);
double d1=gsl_vector_get (d, 1);
double f1=gsl_vector_get (f, 1);
//std::cout << "d0,f0,d1,f1: " << d0 << " " << f0 << " " << d1 << " "
//<< f1 << std::endl;
{
double mu=trailing_eigenvalue (d, f);
y=d0 * d0 - mu;
z=d0 * f0;
}
/* Set up the recurrence for Givens rotations on a bidiagonal matrix */
ak=0;
bk=0;
ap=d0;
bp=f0;
aq=d1;
bq=f1;
}
for (k=0; k < n - 1; k++)
{
double c, s;
create_givens (y, z, &c, &s);
/* Compute V <= V G */
for (i=0; i < N; i++)
{
double Vip=gsl_matrix_get (V, i, k);
double Viq=gsl_matrix_get (V, i, k + 1);
//std::cout << "Vip,Viq: " << Vip << " " << Viq << std::endl;
gsl_matrix_set (V, i, k, c * Vip - s * Viq);
gsl_matrix_set (V, i, k + 1, s * Vip + c * Viq);
}
/* compute B <= B G */
{
double bk1=c * bk - s * z;
double ap1=c * ap - s * bp;
double bp1=s * ap + c * bp;
double zp1=-s * aq;
double aq1=c * aq;
if (k > 0)
{
gsl_vector_set (f, k - 1, bk1);
}
ak=ap1;
bk=bp1;
zk=zp1;
ap=aq1;
if (k < n - 2)
{
bp=gsl_vector_get (f, k + 1);
}
else
{
bp=0.0;
}
y=ak;
z=zk;
}
create_givens (y, z, &c, &s);
/* Compute U <= U G */
for (i=0; i < M; i++)
{
double Uip=gsl_matrix_get (U, i, k);
double Uiq=gsl_matrix_get (U, i, k + 1);
//std::cout << "Uip2,Uiq2: " << Uip << " " << Uiq << std::endl;
gsl_matrix_set (U, i, k, c * Uip - s * Uiq);
gsl_matrix_set (U, i, k + 1, s * Uip + c * Uiq);
}
/* compute B <= G^T B */
//std::cout << "k,bk,ap2: " << k << " " << bk << " " << ap << std::endl;
//std::cout << "ak,zk,bp: " << ak << " " << zk << " "
// << bp << std::endl;
{
//std::cout << "prod1: " << c*ak << " " << s*zk << std::endl;
//std::cout << "prod2: " << c*bk << " " << s*ap << std::endl;
//std::cout << "prod3: " << s*bk << " " << c*ap << std::endl;
double ak1=c * ak - s * zk;
double bk1=c * bk - s * ap;
double zk1=-s * bp;
double ap1=s * bk + c * ap;
double bp1=c * bp;
gsl_vector_set (d, k, ak1);
ak=ak1;
bk=bk1;
zk=zk1;
ap=ap1;
bp=bp1;
//std::cout << "c,s: " << c << " " << s << std::endl;
//std::cout << "k,bk,ap: " << k << " " << bk << " " << ap << std::endl;
if (k < n - 2)
{
aq=gsl_vector_get (d, k + 2);
}
else
{
aq=0.0;
}
y=bk;
z=zk;
}
}
gsl_vector_set (f, n - 2, bk);
gsl_vector_set (d, n - 1, ap);
//std::cout << "bk,ap: " << bk << " " << ap << std::endl;
}
static void
qrstep (const size_t n, double d[], double sd[], double gc[], double gs[])
{
double x, z;
double ak, bk, zk, ap, bp, aq, bq;
size_t k;
double mu = trailing_eigenvalue (n, d, sd);
x = d[0] - mu;
z = sd[0];
ak = 0;
bk = 0;
zk = 0;
ap = d[0];
bp = sd[0];
aq = d[1];
if (n == 2)
{
double c, s;
create_givens (x, z, &c, &s);
if (gc != NULL)
gc[0] = c;
if (gs != NULL)
gs[0] = s;
{
double ap1 = c * (c * ap - s * bp) + s * (s * aq - c * bp);
double bp1 = c * (s * ap + c * bp) - s * (s * bp + c * aq);
double aq1 = s * (s * ap + c * bp) + c * (s * bp + c * aq);
ak = ap1;
bk = bp1;
ap = aq1;
}
d[0] = ak;
sd[0] = bk;
d[1] = ap;
return;
}
bq = sd[1];
for (k = 0; k < n - 1; k++)
{
double c, s;
create_givens (x, z, &c, &s);
/* store Givens rotation */
if (gc != NULL)
gc[k] = c;
if (gs != NULL)
gs[k] = s;
/* compute G' T G */
{
double bk1 = c * bk - s * zk;
double ap1 = c * (c * ap - s * bp) + s * (s * aq - c * bp);
double bp1 = c * (s * ap + c * bp) - s * (s * bp + c * aq);
double zp1 = -s * bq;
double aq1 = s * (s * ap + c * bp) + c * (s * bp + c * aq);
double bq1 = c * bq;
ak = ap1;
bk = bp1;
zk = zp1;
ap = aq1;
bp = bq1;
if (k < n - 2)
aq = d[k + 2];
if (k < n - 3)
bq = sd[k + 2];
d[k] = ak;
if (k > 0)
sd[k - 1] = bk1;
if (k < n - 2)
sd[k + 1] = bp;
x = bk;
z = zk;
}
}
/* k = n - 1 */
d[k] = ap;
sd[k - 1] = bk;
}
