sinfo_qr.c

00001 /* $Id: sinfo_qr.c,v 1.5 2012/03/03 10:18:26 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: 2012/03/03 10:18:26 $
00024  * $Revision: 1.5 $
00025  * $Name: HEAD $
00026  */
00035 #ifdef HAVE_CONFIG_H
00036 #  include <config.h>
00037 #endif
00038 
00039 #include "sinfo_solve_poly_root.h"
00040 //#include "sinfoni_recipes_defaults.h"
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   /* FIXME: if p,q,r, are not set to zero then the compiler complains
00059      that they ``might be used uninitialized in this
00060      function''. Looking at the code this does seem possible, so this
00061      should be checked. */
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); /* one real root */
00091       n--;
00092       goto next_root;
00093       /*continue;*/
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)        /* two real roots */
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       /*continue;*/
00125     }
00126 
00127   /* No more roots found yet, do another iteration */
00128 
00129   if (iterations == 60)  /* increased from 30 to 60 */
00130     {
00131       /* too many iterations - give up! */
00132       cpl_msg_error("qr:","too many iterations-give up") ;
00133       return -1 ;
00134     }
00135 
00136   if (iterations % 10 == 0 && iterations > 0)
00137     {
00138       /* use an exceptional shift */
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   /* double QR step */
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;        /* FIXME????? */
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       /* do row modifications */
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       /* do column modifications */
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 }

Generated on 3 Mar 2013 for SINFONI Pipeline Reference Manual by  doxygen 1.6.1