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