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