Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/orxonox/trunk/src/lib/newmat/evalue.cpp @ 4617

Last change on this file since 4617 was 4565, checked in by patrick, 19 years ago

orxonox/trunk: added the newmat library to the project. needs some translation in directory, temp under util/newmat. is needed by the collision detection engine to perform lin alg operations such as eigenvector decomposition. perhaps we will make our own library to do that later.

File size: 9.0 KB
Line 
1//$$evalue.cpp                           eigen-value decomposition
2
3// Copyright (C) 1991,2,3,4: R B Davies
4
5#define WANT_MATH
6
7#include "include.h"
8#include "newmatap.h"
9#include "newmatrm.h"
10#include "precisio.h"
11
12#ifdef use_namespace
13namespace NEWMAT {
14#endif
15
16#ifdef DO_REPORT
17#define REPORT { static ExeCounter ExeCount(__LINE__,17); ++ExeCount; }
18#else
19#define REPORT {}
20#endif
21
22
23
24static void tred2(const SymmetricMatrix& A, DiagonalMatrix& D,
25   DiagonalMatrix& E, Matrix& Z)
26{
27   Tracer et("Evalue(tred2)");
28   REPORT
29   Real tol =
30      FloatingPointPrecision::Minimum()/FloatingPointPrecision::Epsilon();
31   int n = A.Nrows(); Z.ReSize(n,n); Z.Inject(A);
32   D.ReSize(n); E.ReSize(n);
33   Real* z = Z.Store(); int i;
34
35   for (i=n-1; i > 0; i--)                   // i=0 is excluded
36   {
37      Real f = Z.element(i,i-1); Real g = 0.0;
38      int k = i-1; Real* zik = z + i*n;
39      while (k--) g += square(*zik++);
40      Real h = g + square(f);
41      if (g <= tol) { REPORT E.element(i) = f; h = 0.0; }
42      else
43      {
44         REPORT
45         g = sign(-sqrt(h), f); E.element(i) = g; h -= f*g;
46         Z.element(i,i-1) = f-g; f = 0.0;
47         Real* zji = z + i; Real* zij = z + i*n; Real* ej = E.Store();
48         int j;
49         for (j=0; j<i; j++)
50         {
51            *zji = (*zij++)/h; g = 0.0;
52            Real* zjk = z + j*n; zik = z + i*n;
53            k = j; while (k--) g += *zjk++ * (*zik++);
54            k = i-j;
55            if (k) for(;;)
56               { g += *zjk * (*zik++); if (!(--k)) break; zjk += n; }
57            *ej++ = g/h; f += g * (*zji); zji += n;
58         }
59         Real hh = f / (h + h); zij = z + i*n; ej = E.Store();
60         for (j=0; j<i; j++)
61         {
62            f = *zij++; g = *ej - hh * f; *ej++ = g;
63            Real* zjk = z + j*n; Real* zik = z + i*n;
64            Real* ek = E.Store(); k = j+1;
65            while (k--)  *zjk++ -= ( f*(*ek++) + g*(*zik++) ); 
66         }
67      }
68      D.element(i) = h;
69   }
70
71   D.element(0) = 0.0; E.element(0) = 0.0;
72   for (i=0; i<n; i++)
73   {
74      if (D.element(i) != 0.0)
75      {
76         REPORT
77         for (int j=0; j<i; j++)
78         {
79            Real g = 0.0;
80            Real* zik = z + i*n; Real* zkj = z + j;
81            int k = i;
82            if (k) for (;;)
83               { g += *zik++ * (*zkj); if (!(--k)) break; zkj += n; }
84            Real* zki = z + i; zkj = z + j;
85            k = i;
86            if (k) for (;;)
87               { *zkj -= g * (*zki); if (!(--k)) break; zkj += n; zki += n; }
88         }
89      }
90      Real* zij = z + i*n; Real* zji = z + i;
91      int j = i;
92      if (j) for (;;)
93         { *zij++ = 0.0; *zji = 0.0; if (!(--j)) break; zji += n; }
94      D.element(i) = *zij; *zij = 1.0;
95   }
96}
97
98static void tql2(DiagonalMatrix& D, DiagonalMatrix& E, Matrix& Z)
99{
100   Tracer et("Evalue(tql2)");
101   REPORT
102   Real eps = FloatingPointPrecision::Epsilon();
103   int n = D.Nrows(); Real* z = Z.Store(); int l;
104   for (l=1; l<n; l++) E.element(l-1) = E.element(l);
105   Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0;
106   for (l=0; l<n; l++)
107   {
108      int i,j;
109      Real& dl = D.element(l); Real& el = E.element(l);
110      Real h = eps * ( fabs(dl) + fabs(el) );
111      if (b < h) { REPORT b = h; }
112      int m;
113      for (m=l; m<n; m++) if (fabs(E.element(m)) <= b) break;
114      bool test = false;
115      for (j=0; j<30; j++)
116      {
117         if (m==l) { REPORT test = true; break; }
118         Real& dl1 = D.element(l+1);
119         Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0);
120         dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h;
121         Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h;
122
123         p = D.element(m); Real c = 1.0; Real s = 0.0;
124         for (i=m-1; i>=l; i--)
125         {
126            Real ei = E.element(i); Real di = D.element(i);
127            Real& ei1 = E.element(i+1);
128            g = c * ei; h = c * p;
129            if ( fabs(p) >= fabs(ei))
130            {
131               REPORT
132               c = ei / p; r = sqrt(c*c + 1.0);
133               ei1 = s*p*r; s = c/r; c = 1.0/r;
134            }
135            else
136            {
137               REPORT
138               c = p / ei; r = sqrt(c*c + 1.0);
139               ei1 = s * ei * r; s = 1.0/r; c /= r;
140            }
141            p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di);
142
143            Real* zki = z + i; Real* zki1 = zki + 1; int k = n;
144            if (k) for (;;)
145            {
146               REPORT
147               h = *zki1; *zki1 = s*(*zki) + c*h; *zki = c*(*zki) - s*h;
148               if (!(--k)) break;
149               zki += n; zki1 += n;
150            }
151         }
152         el = s*p; dl = c*p;
153         if (fabs(el) <= b) { REPORT; test = true; break; }
154      }
155      if (!test) Throw ( ConvergenceException(D) );
156      dl += f;
157   }
158/*
159   for (int i=0; i<n; i++)
160   {
161      int k = i; Real p = D.element(i);
162      for (int j=i+1; j<n; j++)
163         { if (D.element(j) < p) { k = j; p = D.element(j); } }
164      if (k != i)
165      {
166         D.element(k) = D.element(i); D.element(i) = p; int j = n;
167         Real* zji = z + i; Real* zjk = z + k;
168         if (j) for(;;)
169         {
170            p = *zji; *zji = *zjk; *zjk = p;
171            if (!(--j)) break;
172            zji += n; zjk += n;
173         }
174      }
175   }
176*/
177}
178
179static void tred3(const SymmetricMatrix& X, DiagonalMatrix& D,
180   DiagonalMatrix& E, SymmetricMatrix& A)
181{
182   Tracer et("Evalue(tred3)");
183   REPORT
184   Real tol =
185      FloatingPointPrecision::Minimum()/FloatingPointPrecision::Epsilon();
186   int n = X.Nrows(); A = X; D.ReSize(n); E.ReSize(n);
187   Real* ei = E.Store() + n;
188   for (int i = n-1; i >= 0; i--)
189   {
190      Real h = 0.0; Real f = - FloatingPointPrecision::Maximum();
191      Real* d = D.Store(); Real* a = A.Store() + (i*(i+1))/2; int k = i;
192      while (k--) { f = *a++; *d++ = f; h += square(f); }
193      if (h <= tol) { REPORT *(--ei) = 0.0; h = 0.0; }
194      else
195      {
196         REPORT
197         Real g = sign(-sqrt(h), f); *(--ei) = g; h -= f*g;
198         f -= g; *(d-1) = f; *(a-1) = f; f = 0.0;
199         Real* dj = D.Store(); Real* ej = E.Store(); int j;
200         for (j = 0; j < i; j++)
201         {
202            Real* dk = D.Store(); Real* ak = A.Store()+(j*(j+1))/2;
203            Real g = 0.0; k = j;
204            while (k--)  g += *ak++ * *dk++;
205            k = i-j; int l = j; 
206            if (k) for (;;) { g += *ak * *dk++; if (!(--k)) break; ak += ++l; }
207            g /= h; *ej++ = g; f += g * *dj++;
208         } 
209         Real hh = f / (2 * h); Real* ak = A.Store();
210         dj = D.Store(); ej = E.Store();
211         for (j = 0; j < i; j++)
212         {
213            f = *dj++; g = *ej - hh * f; *ej++ = g;
214            Real* dk = D.Store(); Real* ek = E.Store(); k = j+1;
215            while (k--) { *ak++ -= (f * *ek++ + g * *dk++); }
216         }
217      }
218      *d = *a; *a = h;
219   }
220}
221
222static void tql1(DiagonalMatrix& D, DiagonalMatrix& E)
223{
224   Tracer et("Evalue(tql1)");
225   REPORT
226   Real eps = FloatingPointPrecision::Epsilon();
227   int n = D.Nrows(); int l;
228   for (l=1; l<n; l++) E.element(l-1) = E.element(l);
229   Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0;
230   for (l=0; l<n; l++)
231   {
232      int i,j;
233      Real& dl = D.element(l); Real& el = E.element(l);
234      Real h = eps * ( fabs(dl) + fabs(el) );
235      if (b < h) b = h;
236      int m;
237      for (m=l; m<n; m++) if (fabs(E.element(m)) <= b) break;
238      bool test = false;
239      for (j=0; j<30; j++)
240      {
241         if (m==l) { REPORT test = true; break; }
242         Real& dl1 = D.element(l+1);
243         Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0);
244         dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h;
245         Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h;
246
247         p = D.element(m); Real c = 1.0; Real s = 0.0;
248         for (i=m-1; i>=l; i--)
249         {
250            Real ei = E.element(i); Real di = D.element(i);
251            Real& ei1 = E.element(i+1);
252            g = c * ei; h = c * p;
253            if ( fabs(p) >= fabs(ei))
254            {
255               REPORT
256               c = ei / p; r = sqrt(c*c + 1.0);
257               ei1 = s*p*r; s = c/r; c = 1.0/r;
258            }
259            else
260            {
261               REPORT
262               c = p / ei; r = sqrt(c*c + 1.0);
263               ei1 = s * ei * r; s = 1.0/r; c /= r;
264            }
265            p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di);
266         }
267         el = s*p; dl = c*p;
268         if (fabs(el) <= b) { REPORT test = true; break; }
269      }
270      if (!test) Throw ( ConvergenceException(D) );
271      Real p = dl + f;
272      test = false;
273      for (i=l; i>0; i--)
274      {
275         if (p < D.element(i-1)) { REPORT D.element(i) = D.element(i-1); }
276         else { REPORT test = true; break; }
277      }
278      if (!test) i=0;
279      D.element(i) = p;
280   }
281}
282
283void EigenValues(const SymmetricMatrix& A, DiagonalMatrix& D, Matrix& Z)
284{ REPORT DiagonalMatrix E; tred2(A, D, E, Z); tql2(D, E, Z); SortSV(D,Z,true); }
285
286void EigenValues(const SymmetricMatrix& X, DiagonalMatrix& D)
287{ REPORT DiagonalMatrix E; SymmetricMatrix A; tred3(X,D,E,A); tql1(D,E); }
288
289void EigenValues(const SymmetricMatrix& X, DiagonalMatrix& D,
290   SymmetricMatrix& A)
291{ REPORT DiagonalMatrix E; tred3(X,D,E,A); tql1(D,E); }
292
293
294#ifdef use_namespace
295}
296#endif
297
Note: See TracBrowser for help on using the repository browser.