sinfo_qr.c

00001 /* $Id: sinfo_qr.c,v 1.4 2007/06/06 07:10:45 amodigli Exp $
00002  *
00003  * This file is part of the SINFONI Pipeline
00004  * Copyright (C) 2002,2003 European Southern Observatory
00005  *
00006  * This program is free software; you can redistribute it and/or modify
00007  * it under the terms of the GNU General Public License as published by
00008  * the Free Software Foundation; either version 2 of the License, or
00009  * (at your option) any later version.
00010  *
00011  * This program is distributed in the hope that it will be useful,
00012  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  * GNU General Public License for more details.
00015  *
00016  * You should have received a copy of the GNU General Public License
00017  * along with this program; if not, write to the Free Software
00018  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  */
00020 
00021 /*
00022  * $Author: amodigli $
00023  * $Date: 2007/06/06 07:10:45 $
00024  * $Revision: 1.4 $
00025  * $Name: sinfo-2_2_5 $
00026  */
00034 #ifdef HAVE_CONFIG_H
00035 #  include <config.h>
00036 #endif
00037 
00038 #include "sinfo_solve_poly_root.h"
00039 //#include "sinfoni_recipes_defaults.h"
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   /* FIXME: if p,q,r, are not set to zero then the compiler complains
00058      that they ``might be used uninitialized in this
00059      function''. Looking at the code this does seem possible, so this
00060      should be checked. */
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); /* one real root */
00090       n--;
00091       goto next_root;
00092       /*continue;*/
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)        /* two real roots */
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       /*continue;*/
00124     }
00125 
00126   /* No more roots found yet, do another iteration */
00127 
00128   if (iterations == 60)  /* increased from 30 to 60 */
00129     {
00130       /* too many iterations - give up! */
00131       cpl_msg_error("qr:","too many iterations-give up") ;
00132       return -1 ;
00133     }
00134 
00135   if (iterations % 10 == 0 && iterations > 0)
00136     {
00137       /* use an exceptional shift */
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   /* double QR step */
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;        /* FIXME????? */
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       /* do row modifications */
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       /* do column modifications */
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 

Generated on 8 Mar 2011 for SINFONI Pipeline Reference Manual by  doxygen 1.6.1