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