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