Jacobi.h
Go to the documentation of this file.
1#ifndef __GfxTL_JACOBI_HEADER__
2#define __GfxTL_JACOBI_HEADER__
3#include <memory>
4
5#include <GfxTL/Array.h>
6#include <GfxTL/MathHelper.h>
7#include <GfxTL/MatrixXX.h>
8#include <GfxTL/VectorXD.h>
9
10namespace GfxTL
11{
12 //Computes all eigenvalues and eigenvectors of a real symmetric matrix a[1..n][1..n]. On
13 //output, elements of a above the diagonal are destroyed. d[1..n] returns the eigenvalues of a.
14 //v[1..n][1..n] is a matrix whose columns contain, on output, the normalized eigenvectors of
15 //a. nrot returns the number of Jacobi rotations that were required.
16#define __GfxTL_JACOBI_ROTATE(a, i, j, k, l) \
17 g = (a)[j][i]; \
18 h = (a)[l][k]; \
19 (a)[j][i] = g - s * (h + g * tau); \
20 (a)[l][k] = h + s * (g - h * tau);
21
22 template <unsigned int N, class T>
23 bool
24 Jacobi(const MatrixXX<N, N, T>& m, VectorXD<N, T>* d, MatrixXX<N, N, T>* v, int* nrot = NULL)
25 {
26 using namespace std;
27 MatrixXX<N, N, T> a(m);
28 int j, iq, ip, i;
29 T tresh, theta, tau, t, sm, s, h, g, c;
30 T volatile temp1, temp2;
31 VectorXD<N, T> b, z;
32 for (ip = 0; ip < N; ip++)
33 {
34 for (iq = 0; iq < N; iq++)
35 {
36 (*v)[iq][ip] = T(0);
37 }
38 (*v)[ip][ip] = T(1);
39 }
40 for (ip = 0; ip < N; ip++)
41 {
42 b[ip] = (*d)[ip] = a[ip][ip];
43 z[ip] = T(0);
44 }
45 if (nrot)
46 {
47 *nrot = 0;
48 }
49 for (i = 1; i <= 200; i++)
50 {
51 sm = T(0);
52 for (ip = 0; ip < N - 1; ip++)
53 {
54 for (iq = ip + 1; iq < N; iq++)
55 {
56 sm += abs(a[iq][ip]);
57 }
58 }
59 if (sm == T(0))
60 {
61 return true;
62 }
63 if (i < 4)
64 {
65 tresh = T(0.2) * sm / (N * N);
66 }
67 else
68 {
69 tresh = T(0.0);
70 }
71 for (ip = 0; ip < N - 1; ip++)
72 {
73 for (iq = ip + 1; iq < N; iq++)
74 {
75 g = T(100) * abs(a[iq][ip]);
76 temp1 = abs((*d)[ip]) + g;
77 temp2 = abs((*d)[iq]) + g;
78 if (i > 4 && temp1 == abs((*d)[ip]) && temp2 == abs((*d)[iq]))
79 {
80 a[iq][ip] = T(0);
81 }
82 else if (abs(a[iq][ip]) > tresh)
83 {
84 h = (*d)[iq] - (*d)[ip];
85 temp1 = (abs(h) + g);
86 if (temp1 == abs(h))
87 {
88 t = a[iq][ip] / h;
89 }
90 else
91 {
92 theta = T(0.5) * h / a[iq][ip];
93 t = T(1) / (abs(theta) + sqrt(T(1) + theta * theta));
94 if (theta < T(0))
95 {
96 t = -t;
97 }
98 }
99 c = T(1) / sqrt(T(1) + t * t);
100 s = t * c;
101 tau = s / (T(1) + c);
102 h = t * a[iq][ip];
103 z[ip] -= h;
104 z[iq] += h;
105 (*d)[ip] -= h;
106 (*d)[iq] += h;
107 a[iq][ip] = T(0);
108 for (j = 0; j <= ip - 1; j++)
109 {
110 __GfxTL_JACOBI_ROTATE(a, j, ip, j, iq)
111 }
112 for (j = ip + 1; j <= iq - 1; j++)
113 {
114 __GfxTL_JACOBI_ROTATE(a, ip, j, j, iq)
115 }
116 for (j = iq + 1; j < N; j++)
117 {
118 __GfxTL_JACOBI_ROTATE(a, ip, j, iq, j)
119 }
120 for (j = 0; j < N; j++)
121 {
122 __GfxTL_JACOBI_ROTATE((*v), j, ip, j, iq)
123 }
124 if (nrot)
125 {
126 ++(*nrot);
127 }
128 }
129 }
130 }
131 for (ip = 0; ip < N; ip++)
132 {
133 b[ip] += z[ip];
134 (*d)[ip] = b[ip];
135 z[ip] = T(0);
136 }
137 }
138 return false;
139 }
140
141 template <unsigned int N, class T>
142 void
143 EigSortDesc(VectorXD<N, T>* d, MatrixXX<N, N, T>* v)
144 {
145 int k, j, i;
146 T p;
147 for (i = 0; i < N; ++i)
148 {
149 p = (*d)[k = i];
150 for (j = i + 1; j < N; ++j)
151 if ((*d)[j] >= p)
152 {
153 p = (*d)[k = j];
154 }
155 if (k != i)
156 {
157 (*d)[k] = (*d)[i];
158 (*d)[i] = p;
159 for (j = 0; j < N; ++j)
160 {
161 p = (*v)[i][j];
162 (*v)[i][j] = (*v)[k][j];
163 (*v)[k][j] = p;
164 }
165 }
166 }
167 }
168
169 //Computes all eigenvalues and eigenvectors of a real symmetric matrix a[1..n][1..n]. On
170 //output, elements of a above the diagonal are destroyed. d[1..n] returns the eigenvalues of a.
171 //v[1..n][1..n] is a matrix whose columns contain, on output, the normalized eigenvectors of
172 //a. nrot returns the number of Jacobi rotations that were required.
173 template <class IteratorT, class VectorT>
174 bool
175 Jacobi(Array<2, IteratorT>* a, VectorT* d, Array<2, IteratorT>* v, int* nrot = NULL)
176 {
177 using namespace std;
178 typedef typename Array<2, IteratorT>::value_type T;
179 intptr_t N = (*a).Extent(0);
180 intptr_t j, iq, ip, i;
181 T tresh, theta, tau, t, sm, s, h, g, c;
182 T volatile temp1, temp2;
183 auto_ptr<T> ab(new T[N]), az(new T[N]);
184 T *b = ab.get(), *z = az.get();
185 for (ip = 0; ip < N; ip++)
186 {
187 for (iq = 0; iq < N; iq++)
188 {
189 (*v)[iq][ip] = T(0);
190 }
191 (*v)[ip][ip] = T(1);
192 }
193 for (ip = 0; ip < N; ip++)
194 {
195 b[ip] = (*d)[ip] = (*a)[ip][ip];
196 z[ip] = T(0);
197 }
198 if (nrot)
199 {
200 *nrot = 0;
201 }
202 for (i = 1; i <= 200; i++)
203 {
204 sm = T(0);
205 for (ip = 0; ip < N - 1; ip++)
206 {
207 for (iq = ip + 1; iq < N; iq++)
208 {
209 sm += abs((*a)[iq][ip]);
210 }
211 }
212 if (sm == T(0))
213 {
214 return true;
215 }
216 if (i < 4)
217 {
218 tresh = T(0.2) * sm / (N * N);
219 }
220 else
221 {
222 tresh = T(0.0);
223 }
224 for (ip = 0; ip < N - 1; ip++)
225 {
226 for (iq = ip + 1; iq < N; iq++)
227 {
228 g = T(100) * abs((*a)[iq][ip]);
229 temp1 = abs((*d)[ip]) + g;
230 temp2 = abs((*d)[iq]) + g;
231 if (i > 4 && temp1 == abs((*d)[ip]) && temp2 == abs((*d)[iq]))
232 {
233 (*a)[iq][ip] = T(0);
234 }
235 else if (abs((*a)[iq][ip]) > tresh)
236 {
237 h = (*d)[iq] - (*d)[ip];
238 temp1 = (abs(h) + g);
239 if (temp1 == abs(h))
240 {
241 t = (*a)[iq][ip] / h;
242 }
243 else
244 {
245 theta = T(0.5) * h / (*a)[iq][ip];
246 t = T(1) / (abs(theta) + sqrt(T(1) + theta * theta));
247 if (theta < T(0))
248 {
249 t = -t;
250 }
251 }
252 c = T(1) / sqrt(T(1) + t * t);
253 s = t * c;
254 tau = s / (T(1) + c);
255 h = t * (*a)[iq][ip];
256 z[ip] -= h;
257 z[iq] += h;
258 (*d)[ip] -= h;
259 (*d)[iq] += h;
260 (*a)[iq][ip] = T(0);
261 for (j = 0; j <= ip - 1; j++)
262 {
263 __GfxTL_JACOBI_ROTATE((*a), j, ip, j, iq)
264 }
265 for (j = ip + 1; j <= iq - 1; j++)
266 {
267 __GfxTL_JACOBI_ROTATE((*a), ip, j, j, iq)
268 }
269 for (j = iq + 1; j < N; j++)
270 {
271 __GfxTL_JACOBI_ROTATE((*a), ip, j, iq, j)
272 }
273 for (j = 0; j < N; j++)
274 {
275 __GfxTL_JACOBI_ROTATE((*v), j, ip, j, iq)
276 }
277 if (nrot)
278 {
279 ++(*nrot);
280 }
281 }
282 }
283 }
284 for (ip = 0; ip < N; ip++)
285 {
286 b[ip] += z[ip];
287 (*d)[ip] = b[ip];
288 z[ip] = T(0);
289 }
290 }
291 return false;
292 }
293
294 template <class IteratorT, class VectorT>
295 void
296 EigSortDesc(VectorT* d, Array<2, IteratorT>* v)
297 {
298 typedef typename Array<2, IteratorT>::value_type T;
299 size_t N = v->Extent(0);
300 int k, j, i;
301 T p;
302 for (i = 0; i < N; ++i)
303 {
304 p = (*d)[k = i];
305 for (j = i + 1; j < N; ++j)
306 if ((*d)[j] >= p)
307 {
308 p = (*d)[k = j];
309 }
310 if (k != i)
311 {
312 (*d)[k] = (*d)[i];
313 (*d)[i] = p;
314 for (j = 0; j < N; ++j)
315 {
316 p = (*v)[i][j];
317 (*v)[i][j] = (*v)[k][j];
318 (*v)[k][j] = p;
319 }
320 }
321 }
322 }
323
324#undef __GfxTL_JACOBI_ROTATE
325}; // namespace GfxTL
326
327#endif
__GfxTL_JACOBI_ROTATE
#define __GfxTL_JACOBI_ROTATE(a, i, j, k, l)
Definition Jacobi.h:16
c
constexpr T c
Definition UnscentedKalmanFilterTest.cpp:46
GfxTL::Array::value_type
std::iterator_traits< IteratorT >::value_type value_type
Definition Array.h:84
GfxTL
Definition AABox.h:10
GfxTL::Jacobi
bool Jacobi(const MatrixXX< N, N, T > &m, VectorXD< N, T > *d, MatrixXX< N, N, T > *v, int *nrot=NULL)
Definition Jacobi.h:24
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition VectorXD.h:704
GfxTL::EigSortDesc
void EigSortDesc(VectorXD< N, T > *d, MatrixXX< N, N, T > *v)
Definition Jacobi.h:143