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


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:36