00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00035 #ifdef HAVE_CONFIG_H
00036 # include <config.h>
00037 #endif
00038
00039 #include "sinfo_solve_poly_root.h"
00040
00041
00042 #define GSL_SET_COMPLEX_PACKED(zp,n,x,y) \
00043 do {*((zp)+2*(n))=(x); *((zp)+(2*(n)+1))=(y);} while(0)
00044 #define GSL_DBL_EPSILON 2.2204460492503131e-16
00045
00046
00047 int
00048 sinfo_qr_companion (double *h, size_t nc, gsl_complex_packed_ptr zroot)
00049 {
00050 double t = 0.0;
00051
00052 size_t iterations, e, i, j, k, m;
00053
00054 double w, x, y, s, z;
00055
00056 double p = 0, q = 0, r = 0;
00057
00058
00059
00060
00061
00062
00063 int notlast;
00064
00065 size_t n = nc;
00066
00067 next_root:
00068
00069 if (n == 0)
00070 return 1 ;
00071
00072 iterations = 0;
00073
00074 next_iteration:
00075
00076 for (e = n; e >= 2; e--)
00077 {
00078 double a1 = fabs (FMAT (h, e, e - 1, nc));
00079 double a2 = fabs (FMAT (h, e - 1, e - 1, nc));
00080 double a3 = fabs (FMAT (h, e, e, nc));
00081
00082 if (a1 <= GSL_DBL_EPSILON * (a2 + a3))
00083 break;
00084 }
00085
00086 x = FMAT (h, n, n, nc);
00087
00088 if (e == n)
00089 {
00090 GSL_SET_COMPLEX_PACKED (zroot, n-1, x + t, 0);
00091 n--;
00092 goto next_root;
00093
00094 }
00095
00096 y = FMAT (h, n - 1, n - 1, nc);
00097 w = FMAT (h, n - 1, n, nc) * FMAT (h, n, n - 1, nc);
00098
00099 if (e == n - 1)
00100 {
00101 p = (y - x) / 2;
00102 q = p * p + w;
00103 y = sqrt (fabs (q));
00104
00105 x += t;
00106
00107 if (q > 0)
00108 {
00109 if (p < 0)
00110 y = -y;
00111 y += p;
00112
00113 GSL_SET_COMPLEX_PACKED (zroot, n-1, x - w / y, 0);
00114 GSL_SET_COMPLEX_PACKED (zroot, n-2, x + y, 0);
00115 }
00116 else
00117 {
00118 GSL_SET_COMPLEX_PACKED (zroot, n-1, x + p, -y);
00119 GSL_SET_COMPLEX_PACKED (zroot, n-2, x + p, y);
00120 }
00121 n -= 2;
00122
00123 goto next_root;
00124
00125 }
00126
00127
00128
00129 if (iterations == 60)
00130 {
00131
00132 cpl_msg_error("qr:","too many iterations-give up") ;
00133 return -1 ;
00134 }
00135
00136 if (iterations % 10 == 0 && iterations > 0)
00137 {
00138
00139
00140 t += x;
00141
00142 for (i = 1; i <= n; i++)
00143 {
00144 FMAT (h, i, i, nc) -= x;
00145 }
00146
00147 s = fabs (FMAT (h, n, n - 1, nc)) + fabs (FMAT (h, n - 1, n - 2, nc));
00148 y = 0.75 * s;
00149 x = y;
00150 w = -0.4375 * s * s;
00151 }
00152
00153 iterations++;
00154
00155 for (m = n - 2; m >= e; m--)
00156 {
00157 double a1, a2, a3;
00158
00159 z = FMAT (h, m, m, nc);
00160 r = x - z;
00161 s = y - z;
00162 p = FMAT (h, m, m + 1, nc) + (r * s - w) / FMAT (h, m + 1, m, nc);
00163 q = FMAT (h, m + 1, m + 1, nc) - z - r - s;
00164 r = FMAT (h, m + 2, m + 1, nc);
00165 s = fabs (p) + fabs (q) + fabs (r);
00166 p /= s;
00167 q /= s;
00168 r /= s;
00169
00170 if (m == e)
00171 break;
00172
00173 a1 = fabs (FMAT (h, m, m - 1, nc));
00174 a2 = fabs (FMAT (h, m - 1, m - 1, nc));
00175 a3 = fabs (FMAT (h, m + 1, m + 1, nc));
00176
00177 if (a1 * (fabs (q) + fabs (r)) <= GSL_DBL_EPSILON * fabs (p) * (a2 + a3))
00178 break;
00179 }
00180
00181 for (i = m + 2; i <= n; i++)
00182 {
00183 FMAT (h, i, i - 2, nc) = 0;
00184 }
00185
00186 for (i = m + 3; i <= n; i++)
00187 {
00188 FMAT (h, i, i - 3, nc) = 0;
00189 }
00190
00191
00192
00193 for (k = m; k <= n - 1; k++)
00194 {
00195 notlast = (k != n - 1);
00196
00197 if (k != m)
00198 {
00199 p = FMAT (h, k, k - 1, nc);
00200 q = FMAT (h, k + 1, k - 1, nc);
00201 r = notlast ? FMAT (h, k + 2, k - 1, nc) : 0.0;
00202
00203 x = fabs (p) + fabs (q) + fabs (r);
00204
00205 if (x == 0)
00206 continue;
00207
00208 p /= x;
00209 q /= x;
00210 r /= x;
00211 }
00212
00213 s = sqrt (p * p + q * q + r * r);
00214
00215 if (p < 0)
00216 s = -s;
00217
00218 if (k != m)
00219 {
00220 FMAT (h, k, k - 1, nc) = -s * x;
00221 }
00222 else if (e != m)
00223 {
00224 FMAT (h, k, k - 1, nc) *= -1;
00225 }
00226
00227 p += s;
00228 x = p / s;
00229 y = q / s;
00230 z = r / s;
00231 q /= p;
00232 r /= p;
00233
00234
00235
00236 for (j = k; j <= n; j++)
00237 {
00238 p = FMAT (h, k, j, nc) + q * FMAT (h, k + 1, j, nc);
00239
00240 if (notlast)
00241 {
00242 p += r * FMAT (h, k + 2, j, nc);
00243 FMAT (h, k + 2, j, nc) -= p * z;
00244 }
00245
00246 FMAT (h, k + 1, j, nc) -= p * y;
00247 FMAT (h, k, j, nc) -= p * x;
00248 }
00249
00250 j = (k + 3 < n) ? (k + 3) : n;
00251
00252
00253
00254 for (i = e; i <= j; i++)
00255 {
00256 p = x * FMAT (h, i, k, nc) + y * FMAT (h, i, k + 1, nc);
00257
00258 if (notlast)
00259 {
00260 p += z * FMAT (h, i, k + 2, nc);
00261 FMAT (h, i, k + 2, nc) -= p * r;
00262 }
00263 FMAT (h, i, k + 1, nc) -= p * q;
00264 FMAT (h, i, k, nc) -= p;
00265 }
00266 }
00267
00268 goto next_iteration;
00269 }