qr.c

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   /* FIXME: if p,q,r, are not set to zero then the compiler complains
00020      that they ``might be used uninitialized in this
00021      function''. Looking at the code this does seem possible, so this
00022      should be checked. */
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); /* one real root */
00052       n--;
00053       goto next_root;
00054       /*continue;*/
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)        /* two real roots */
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       /*continue;*/
00086     }
00087 
00088   /* No more roots found yet, do another iteration */
00089 
00090   if (iterations == 60)  /* increased from 30 to 60 */
00091     {
00092       /* too many iterations - give up! */
00093       cpl_msg_error("qr:","too many iterations-give up") ;
00094       return -1 ;
00095     }
00096 
00097   if (iterations % 10 == 0 && iterations > 0)
00098     {
00099       /* use an exceptional shift */
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   /* double QR step */
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;       /* FIXME????? */
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       /* do row modifications */
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       /* do column modifications */
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 

Generated on Wed Oct 26 13:08:54 2005 for SINFONI Pipeline Reference Manual by doxygen1.2.13.1 written by Dimitri van Heesch, © 1997-2001