| /* remove off-diagonal elements which are neglegible compared with the |
| neighboring diagonal elements */ |
| |
| inline static void |
| chop_small_elements (const size_t N, const double d[], double sd[]) |
| { |
| double d_i = d[0]; |
| |
| size_t i; |
| |
| for (i = 0; i < N - 1; i++) |
| { |
| double sd_i = sd[i]; |
| double d_ip1 = d[i + 1]; |
| |
| if (fabs (sd_i) < GSL_DBL_EPSILON * (fabs (d_i) + fabs (d_ip1))) |
| { |
| sd[i] = 0.0; |
| } |
| d_i = d_ip1; |
| } |
| } |
| |
| /* Generate a Givens rotation (cos,sin) which takes v=(x,y) to (|v|,0) |
| |
| From Golub and Van Loan, "Matrix Computations", Section 5.1.8 */ |
| |
| inline static void |
| create_givens (const double a, const double b, double *c, double *s) |
| { |
| if (b == 0) |
| { |
| *c = 1; |
| *s = 0; |
| } |
| else if (fabs (b) > fabs (a)) |
| { |
| double t = -a / b; |
| double s1 = 1.0 / sqrt (1 + t * t); |
| *s = s1; |
| *c = s1 * t; |
| } |
| else |
| { |
| double t = -b / a; |
| double c1 = 1.0 / sqrt (1 + t * t); |
| *c = c1; |
| *s = c1 * t; |
| } |
| } |
| |
| inline static double |
| trailing_eigenvalue (const size_t n, const double d[], const double sd[]) |
| { |
| double ta = d[n - 2]; |
| double tb = d[n - 1]; |
| double tab = sd[n - 2]; |
| |
| double dt = (ta - tb) / 2.0; |
| |
| double mu; |
| |
| if (dt >= 0) |
| { |
| mu = tb - (tab * tab) / (dt + hypot (dt, tab)); |
| } |
| else |
| { |
| mu = tb + (tab * tab) / ((-dt) + hypot (dt, tab)); |
| } |
| |
| return mu; |
| } |
| |
| 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; |
| } |