source: ntrip/trunk/BNC/newmat/evalue.cpp@ 7506

Last change on this file since 7506 was 2013, checked in by mervart, 15 years ago

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