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 
10 namespace 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::VectorXD
Definition: MatrixXX.h:24
GfxTL::Array::value_type
std::iterator_traits< IteratorT >::value_type value_type
Definition: Array.h:84
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
GfxTL::Array
Definition: Array.h:81
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:143
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:281
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
Definition: AABox.h:9
MatrixXX.h
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:704
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:38
MathHelper.h
__GfxTL_JACOBI_ROTATE
#define __GfxTL_JACOBI_ROTATE(a, i, j, k, l)
Definition: Jacobi.h:16
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33