Fawkes API  Fawkes Development Version
eig3.c
1 
2 /***************************************************************************
3  * eig3.c: Eigen decomposition code for symmetric 3x3 matrices
4  *
5  * Created: Thu May 24 18:38:12 2012
6  * Copyright 2000 Brian Gerkey
7  * 2000 Kasper Stoy
8  * 2012 Tim Niemueller [www.niemueller.de]
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 /* Eigen decomposition code for symmetric 3x3 matrices, copied from the public
25  domain Java Matrix library JAMA. */
26 
27 #include <math.h>
28 
29 #ifndef MAX
30 #define MAX(a, b) ((a)>(b)?(a):(b))
31 #endif
32 
33 //#define n 3
34 static int n = 3;
35 
36 static double hypot2(double x, double y) {
37  return sqrt(x*x+y*y);
38 }
39 
40 // Symmetric Householder reduction to tridiagonal form.
41 
42 static void tred2(double V[n][n], double d[n], double e[n]) {
43 
44 // This is derived from the Algol procedures tred2 by
45 // Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
46 // Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
47 // Fortran subroutine in EISPACK.
48 
49  int i,j,k;
50  double f,g,h,hh;
51  for (j = 0; j < n; j++) {
52  d[j] = V[n-1][j];
53  }
54 
55  // Householder reduction to tridiagonal form.
56 
57  for (i = n-1; i > 0; i--) {
58 
59  // Scale to avoid under/overflow.
60 
61  double scale = 0.0;
62  double h = 0.0;
63  for (k = 0; k < i; k++) {
64  scale = scale + fabs(d[k]);
65  }
66  if (scale == 0.0) {
67  e[i] = d[i-1];
68  for (j = 0; j < i; j++) {
69  d[j] = V[i-1][j];
70  V[i][j] = 0.0;
71  V[j][i] = 0.0;
72  }
73  } else {
74 
75  // Generate Householder vector.
76 
77  for (k = 0; k < i; k++) {
78  d[k] /= scale;
79  h += d[k] * d[k];
80  }
81  f = d[i-1];
82  g = sqrt(h);
83  if (f > 0) {
84  g = -g;
85  }
86  e[i] = scale * g;
87  h = h - f * g;
88  d[i-1] = f - g;
89  for (j = 0; j < i; j++) {
90  e[j] = 0.0;
91  }
92 
93  // Apply similarity transformation to remaining columns.
94 
95  for (j = 0; j < i; j++) {
96  f = d[j];
97  V[j][i] = f;
98  g = e[j] + V[j][j] * f;
99  for (k = j+1; k <= i-1; k++) {
100  g += V[k][j] * d[k];
101  e[k] += V[k][j] * f;
102  }
103  e[j] = g;
104  }
105  f = 0.0;
106  for (j = 0; j < i; j++) {
107  e[j] /= h;
108  f += e[j] * d[j];
109  }
110  hh = f / (h + h);
111  for (j = 0; j < i; j++) {
112  e[j] -= hh * d[j];
113  }
114  for (j = 0; j < i; j++) {
115  f = d[j];
116  g = e[j];
117  for (k = j; k <= i-1; k++) {
118  V[k][j] -= (f * e[k] + g * d[k]);
119  }
120  d[j] = V[i-1][j];
121  V[i][j] = 0.0;
122  }
123  }
124  d[i] = h;
125  }
126 
127  // Accumulate transformations.
128 
129  for (i = 0; i < n-1; i++) {
130  V[n-1][i] = V[i][i];
131  V[i][i] = 1.0;
132  h = d[i+1];
133  if (h != 0.0) {
134  for (k = 0; k <= i; k++) {
135  d[k] = V[k][i+1] / h;
136  }
137  for (j = 0; j <= i; j++) {
138  g = 0.0;
139  for (k = 0; k <= i; k++) {
140  g += V[k][i+1] * V[k][j];
141  }
142  for (k = 0; k <= i; k++) {
143  V[k][j] -= g * d[k];
144  }
145  }
146  }
147  for (k = 0; k <= i; k++) {
148  V[k][i+1] = 0.0;
149  }
150  }
151  for (j = 0; j < n; j++) {
152  d[j] = V[n-1][j];
153  V[n-1][j] = 0.0;
154  }
155  V[n-1][n-1] = 1.0;
156  e[0] = 0.0;
157 }
158 
159 // Symmetric tridiagonal QL algorithm.
160 
161 static void tql2(double V[n][n], double d[n], double e[n]) {
162 
163 // This is derived from the Algol procedures tql2, by
164 // Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
165 // Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
166 // Fortran subroutine in EISPACK.
167 
168  int i,j,m,l,k;
169  double g,p,r,dl1,h,f,tst1,eps;
170  double c,c2,c3,el1,s,s2;
171 
172  for (i = 1; i < n; i++) {
173  e[i-1] = e[i];
174  }
175  e[n-1] = 0.0;
176 
177  f = 0.0;
178  tst1 = 0.0;
179  eps = pow(2.0,-52.0);
180  for (l = 0; l < n; l++) {
181 
182  // Find small subdiagonal element
183 
184  tst1 = MAX(tst1,fabs(d[l]) + fabs(e[l]));
185  m = l;
186  while (m < n) {
187  if (fabs(e[m]) <= eps*tst1) {
188  break;
189  }
190  m++;
191  }
192 
193  // If m == l, d[l] is an eigenvalue,
194  // otherwise, iterate.
195 
196  if (m > l) {
197  int iter = 0;
198  do {
199  iter = iter + 1; // (Could check iteration count here.)
200 
201  // Compute implicit shift
202 
203  g = d[l];
204  p = (d[l+1] - g) / (2.0 * e[l]);
205  r = hypot2(p,1.0);
206  if (p < 0) {
207  r = -r;
208  }
209  d[l] = e[l] / (p + r);
210  d[l+1] = e[l] * (p + r);
211  dl1 = d[l+1];
212  h = g - d[l];
213  for (i = l+2; i < n; i++) {
214  d[i] -= h;
215  }
216  f = f + h;
217 
218  // Implicit QL transformation.
219 
220  p = d[m];
221  c = 1.0;
222  c2 = c;
223  c3 = c;
224  el1 = e[l+1];
225  s = 0.0;
226  s2 = 0.0;
227  for (i = m-1; i >= l; i--) {
228  c3 = c2;
229  c2 = c;
230  s2 = s;
231  g = c * e[i];
232  h = c * p;
233  r = hypot2(p,e[i]);
234  e[i+1] = s * r;
235  s = e[i] / r;
236  c = p / r;
237  p = c * d[i] - s * g;
238  d[i+1] = h + s * (c * g + s * d[i]);
239 
240  // Accumulate transformation.
241 
242  for (k = 0; k < n; k++) {
243  h = V[k][i+1];
244  V[k][i+1] = s * V[k][i] + c * h;
245  V[k][i] = c * V[k][i] - s * h;
246  }
247  }
248  p = -s * s2 * c3 * el1 * e[l] / dl1;
249  e[l] = s * p;
250  d[l] = c * p;
251 
252  // Check for convergence.
253 
254  } while (fabs(e[l]) > eps*tst1);
255  }
256  d[l] = d[l] + f;
257  e[l] = 0.0;
258  }
259 
260  // Sort eigenvalues and corresponding vectors.
261 
262  for (i = 0; i < n-1; i++) {
263  k = i;
264  p = d[i];
265  for (j = i+1; j < n; j++) {
266  if (d[j] < p) {
267  k = j;
268  p = d[j];
269  }
270  }
271  if (k != i) {
272  d[k] = d[i];
273  d[i] = p;
274  for (j = 0; j < n; j++) {
275  p = V[j][i];
276  V[j][i] = V[j][k];
277  V[j][k] = p;
278  }
279  }
280  }
281 }
282 
283 void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) {
284  int i,j;
285  double e[n];
286  for (i = 0; i < n; i++) {
287  for (j = 0; j < n; j++) {
288  V[i][j] = A[i][j];
289  }
290  }
291  tred2(V, d, e);
292  tql2(V, d, e);
293 }