SINFONI Pipeline Reference Manual  2.5.2
sinfo_qr.c
1 /* $Id: sinfo_qr.c,v 1.5 2012-03-03 10:18:26 amodigli Exp $
2  *
3  * This file is part of the SINFONI Pipeline
4  * Copyright (C) 2002,2003 European Southern Observatory
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19  */
20 
21 /*
22  * $Author: amodigli $
23  * $Date: 2012-03-03 10:18:26 $
24  * $Revision: 1.5 $
25  * $Name: not supported by cvs2svn $
26  */
35 #ifdef HAVE_CONFIG_H
36 # include <config.h>
37 #endif
38 
39 #include "sinfo_solve_poly_root.h"
40 //#include "sinfoni_recipes_defaults.h"
41 
42 #define GSL_SET_COMPLEX_PACKED(zp,n,x,y) \
43  do {*((zp)+2*(n))=(x); *((zp)+(2*(n)+1))=(y);} while(0)
44 #define GSL_DBL_EPSILON 2.2204460492503131e-16
45 
46 
47 int
48 sinfo_qr_companion (double *h, size_t nc, gsl_complex_packed_ptr zroot)
49 {
50  double t = 0.0;
51 
52  size_t iterations, e, i, j, k, m;
53 
54  double w, x, y, s, z;
55 
56  double p = 0, q = 0, r = 0;
57 
58  /* FIXME: if p,q,r, are not set to zero then the compiler complains
59  that they ``might be used uninitialized in this
60  function''. Looking at the code this does seem possible, so this
61  should be checked. */
62 
63  int notlast;
64 
65  size_t n = nc;
66 
67  next_root:
68 
69  if (n == 0)
70  return 1 ;
71 
72  iterations = 0;
73 
74  next_iteration:
75 
76  for (e = n; e >= 2; e--)
77  {
78  double a1 = fabs (FMAT (h, e, e - 1, nc));
79  double a2 = fabs (FMAT (h, e - 1, e - 1, nc));
80  double a3 = fabs (FMAT (h, e, e, nc));
81 
82  if (a1 <= GSL_DBL_EPSILON * (a2 + a3))
83  break;
84  }
85 
86  x = FMAT (h, n, n, nc);
87 
88  if (e == n)
89  {
90  GSL_SET_COMPLEX_PACKED (zroot, n-1, x + t, 0); /* one real root */
91  n--;
92  goto next_root;
93  /*continue;*/
94  }
95 
96  y = FMAT (h, n - 1, n - 1, nc);
97  w = FMAT (h, n - 1, n, nc) * FMAT (h, n, n - 1, nc);
98 
99  if (e == n - 1)
100  {
101  p = (y - x) / 2;
102  q = p * p + w;
103  y = sqrt (fabs (q));
104 
105  x += t;
106 
107  if (q > 0) /* two real roots */
108  {
109  if (p < 0)
110  y = -y;
111  y += p;
112 
113  GSL_SET_COMPLEX_PACKED (zroot, n-1, x - w / y, 0);
114  GSL_SET_COMPLEX_PACKED (zroot, n-2, x + y, 0);
115  }
116  else
117  {
118  GSL_SET_COMPLEX_PACKED (zroot, n-1, x + p, -y);
119  GSL_SET_COMPLEX_PACKED (zroot, n-2, x + p, y);
120  }
121  n -= 2;
122 
123  goto next_root;
124  /*continue;*/
125  }
126 
127  /* No more roots found yet, do another iteration */
128 
129  if (iterations == 60) /* increased from 30 to 60 */
130  {
131  /* too many iterations - give up! */
132  cpl_msg_error("qr:","too many iterations-give up") ;
133  return -1 ;
134  }
135 
136  if (iterations % 10 == 0 && iterations > 0)
137  {
138  /* use an exceptional shift */
139 
140  t += x;
141 
142  for (i = 1; i <= n; i++)
143  {
144  FMAT (h, i, i, nc) -= x;
145  }
146 
147  s = fabs (FMAT (h, n, n - 1, nc)) + fabs (FMAT (h, n - 1, n - 2, nc));
148  y = 0.75 * s;
149  x = y;
150  w = -0.4375 * s * s;
151  }
152 
153  iterations++;
154 
155  for (m = n - 2; m >= e; m--)
156  {
157  double a1, a2, a3;
158 
159  z = FMAT (h, m, m, nc);
160  r = x - z;
161  s = y - z;
162  p = FMAT (h, m, m + 1, nc) + (r * s - w) / FMAT (h, m + 1, m, nc);
163  q = FMAT (h, m + 1, m + 1, nc) - z - r - s;
164  r = FMAT (h, m + 2, m + 1, nc);
165  s = fabs (p) + fabs (q) + fabs (r);
166  p /= s;
167  q /= s;
168  r /= s;
169 
170  if (m == e)
171  break;
172 
173  a1 = fabs (FMAT (h, m, m - 1, nc));
174  a2 = fabs (FMAT (h, m - 1, m - 1, nc));
175  a3 = fabs (FMAT (h, m + 1, m + 1, nc));
176 
177  if (a1 * (fabs (q) + fabs (r)) <= GSL_DBL_EPSILON * fabs (p) * (a2 + a3))
178  break;
179  }
180 
181  for (i = m + 2; i <= n; i++)
182  {
183  FMAT (h, i, i - 2, nc) = 0;
184  }
185 
186  for (i = m + 3; i <= n; i++)
187  {
188  FMAT (h, i, i - 3, nc) = 0;
189  }
190 
191  /* double QR step */
192 
193  for (k = m; k <= n - 1; k++)
194  {
195  notlast = (k != n - 1);
196 
197  if (k != m)
198  {
199  p = FMAT (h, k, k - 1, nc);
200  q = FMAT (h, k + 1, k - 1, nc);
201  r = notlast ? FMAT (h, k + 2, k - 1, nc) : 0.0;
202 
203  x = fabs (p) + fabs (q) + fabs (r);
204 
205  if (x == 0)
206  continue; /* FIXME????? */
207 
208  p /= x;
209  q /= x;
210  r /= x;
211  }
212 
213  s = sqrt (p * p + q * q + r * r);
214 
215  if (p < 0)
216  s = -s;
217 
218  if (k != m)
219  {
220  FMAT (h, k, k - 1, nc) = -s * x;
221  }
222  else if (e != m)
223  {
224  FMAT (h, k, k - 1, nc) *= -1;
225  }
226 
227  p += s;
228  x = p / s;
229  y = q / s;
230  z = r / s;
231  q /= p;
232  r /= p;
233 
234  /* do row modifications */
235 
236  for (j = k; j <= n; j++)
237  {
238  p = FMAT (h, k, j, nc) + q * FMAT (h, k + 1, j, nc);
239 
240  if (notlast)
241  {
242  p += r * FMAT (h, k + 2, j, nc);
243  FMAT (h, k + 2, j, nc) -= p * z;
244  }
245 
246  FMAT (h, k + 1, j, nc) -= p * y;
247  FMAT (h, k, j, nc) -= p * x;
248  }
249 
250  j = (k + 3 < n) ? (k + 3) : n;
251 
252  /* do column modifications */
253 
254  for (i = e; i <= j; i++)
255  {
256  p = x * FMAT (h, i, k, nc) + y * FMAT (h, i, k + 1, nc);
257 
258  if (notlast)
259  {
260  p += z * FMAT (h, i, k + 2, nc);
261  FMAT (h, i, k + 2, nc) -= p * r;
262  }
263  FMAT (h, i, k + 1, nc) -= p * q;
264  FMAT (h, i, k, nc) -= p;
265  }
266  }
267 
268  goto next_iteration;
269 }