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
34static int n = 3;
35
36static double hypot2(double x, double y) {
37 return sqrt(x*x+y*y);
38}
39
40// Symmetric Householder reduction to tridiagonal form.
41
42static 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,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 double 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
161static 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,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 int 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
283void 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}