source: ntrip/trunk/BNC/newmat/hholder.cpp@ 9241

Last change on this file since 9241 was 8901, checked in by stuerze, 5 years ago

upgrade to newmat11 library
File size: 14.1 KB
Line 
1/// \ingroup newmat
2///@{
3
4/// \file hholder.cpp
5/// QR related decompositions
6/// QRZ, QRZT decompositions
7/// QR update and extend orthogonal functions
8
9// Copyright (C) 1991,2,3,4: R B Davies
10
11#define WANT_MATH
12//#define WANT_STREAM
13
14#include "include.h"
15
16#include "newmatap.h"
17
18#ifdef use_namespace
19namespace NEWMAT {
20#endif
21
22#ifdef DO_REPORT
23#define REPORT { static ExeCounter ExeCount(__LINE__,16); ++ExeCount; }
24#else
25#define REPORT {}
26#endif
27
28
29/*************************** QR decompositions ***************************/
30
31inline Real square(Real x) { return x*x; }
32
33void QRZT(Matrix& X, LowerTriangularMatrix& L)
34{
35 REPORT
36 Tracer et("QRZT(1)");
37 int n = X.Ncols(); int s = X.Nrows(); L.resize(s);
38 if (n == 0 || s == 0) { L = 0.0; return; }
39 Real* xi = X.Store(); int k;
40 for (int i=0; i<s; i++)
41 {
42 Real sum = 0.0;
43 Real* xi0=xi; k=n; while(k--) { sum += square(*xi++); }
44 sum = sqrt(sum);
45 if (sum == 0.0)
46 {
47 REPORT
48 k=n; while(k--) { *xi0++ = 0.0; }
49 for (int j=i; j<s; j++) L.element(j,i) = 0.0;
50 }
51 else
52 {
53 L.element(i,i) = sum;
54 Real* xj0=xi0; k=n; while(k--) { *xj0++ /= sum; }
55 for (int j=i+1; j<s; j++)
56 {
57 sum=0.0;
58 xi=xi0; Real* xj=xj0; k=n; while(k--) { sum += *xi++ * *xj++; }
59 xi=xi0; k=n; while(k--) { *xj0++ -= sum * *xi++; }
60 L.element(j,i) = sum;
61 }
62 }
63 }
64}
65
66void QRZT(const Matrix& X, Matrix& Y, Matrix& M)
67{
68 REPORT
69 Tracer et("QRZT(2)");
70 int n = X.Ncols(); int s = X.Nrows(); int t = Y.Nrows();
71 if (Y.Ncols() != n)
72 { Throw(ProgramException("Unequal row lengths",X,Y)); }
73 M.resize(t,s);
74 Real* xi = X.Store(); int k;
75 for (int i=0; i<s; i++)
76 {
77 Real* xj0 = Y.Store(); Real* xi0 = xi;
78 for (int j=0; j<t; j++)
79 {
80 Real sum=0.0;
81 xi=xi0; Real* xj=xj0; k=n; while(k--) { sum += *xi++ * *xj++; }
82 xi=xi0; k=n; while(k--) { *xj0++ -= sum * *xi++; }
83 M.element(j,i) = sum;
84 }
85 }
86}
87
88/*
89void QRZ(Matrix& X, UpperTriangularMatrix& U)
90{
91 Tracer et("QRZ(1)");
92 int n = X.Nrows(); int s = X.Ncols(); U.resize(s);
93 Real* xi0 = X.Store(); int k;
94 for (int i=0; i<s; i++)
95 {
96 Real sum = 0.0;
97 Real* xi = xi0; k=n; while(k--) { sum += square(*xi); xi+=s; }
98 sum = sqrt(sum);
99 U.element(i,i) = sum;
100 if (sum==0.0) Throw(SingularException(U));
101 Real* xj0=xi0; k=n; while(k--) { *xj0 /= sum; xj0+=s; }
102 xj0 = xi0;
103 for (int j=i+1; j<s; j++)
104 {
105 sum=0.0;
106 xi=xi0; k=n; xj0++; Real* xj=xj0;
107 while(k--) { sum += *xi * *xj; xi+=s; xj+=s; }
108 xi=xi0; k=n; xj=xj0;
109 while(k--) { *xj -= sum * *xi; xj+=s; xi+=s; }
110 U.element(i,j) = sum;
111 }
112 xi0++;
113 }
114}
115*/
116
117void QRZ(Matrix& X, UpperTriangularMatrix& U)
118{
119 REPORT
120 Tracer et("QRZ(1)");
121 int n = X.Nrows(); int s = X.Ncols(); U.resize(s); U = 0.0;
122 if (n == 0 || s == 0) return;
123 Real* xi0 = X.Store(); Real* u0 = U.Store(); Real* u;
124 int j, k; int J = s; int i = s;
125 while (i--)
126 {
127 Real* xj0 = xi0; Real* xi = xi0; k = n;
128 if (k) for (;;)
129 {
130 u = u0; Real Xi = *xi; Real* xj = xj0;
131 j = J; while(j--) *u++ += Xi * *xj++;
132 if (!(--k)) break;
133 xi += s; xj0 += s;
134 }
135
136 Real sum = sqrt(*u0); *u0 = sum; u = u0+1;
137 if (sum == 0.0)
138 {
139 REPORT
140 j = J - 1; while(j--) *u++ = 0.0;
141
142 xj0 = xi0++; k = n;
143 if (k) for (;;)
144 {
145 *xj0 = 0.0;
146 if (!(--k)) break;
147 xj0 += s;
148 }
149 u0 += J--;
150 }
151 else
152 {
153 int J1 = J-1; j = J1; while(j--) *u++ /= sum;
154
155 xj0 = xi0; xi = xi0++; k = n;
156 if (k) for (;;)
157 {
158 u = u0+1; Real Xi = *xi; Real* xj = xj0;
159 Xi /= sum; *xj++ = Xi;
160 j = J1; while(j--) *xj++ -= *u++ * Xi;
161 if (!(--k)) break;
162 xi += s; xj0 += s;
163 }
164 u0 += J--;
165 }
166 }
167}
168
169void QRZ(const Matrix& X, Matrix& Y, Matrix& M)
170{
171 REPORT
172 Tracer et("QRZ(2)");
173 int n = X.Nrows(); int s = X.Ncols(); int t = Y.Ncols();
174 if (Y.Nrows() != n)
175 { Throw(ProgramException("Unequal column lengths",X,Y)); }
176 M.resize(s,t); M = 0;Real* m0 = M.Store(); Real* m;
177 Real* xi0 = X.Store();
178 int j, k; int i = s;
179 while (i--)
180 {
181 Real* xj0 = Y.Store(); Real* xi = xi0; k = n;
182 if (k) for (;;)
183 {
184 m = m0; Real Xi = *xi; Real* xj = xj0;
185 j = t; while(j--) *m++ += Xi * *xj++;
186 if (!(--k)) break;
187 xi += s; xj0 += t;
188 }
189
190 xj0 = Y.Store(); xi = xi0++; k = n;
191 if (k) for (;;)
192 {
193 m = m0; Real Xi = *xi; Real* xj = xj0;
194 j = t; while(j--) *xj++ -= *m++ * Xi;
195 if (!(--k)) break;
196 xi += s; xj0 += t;
197 }
198 m0 += t;
199 }
200}
201
202/*
203
204void QRZ(const Matrix& X, Matrix& Y, Matrix& M)
205{
206 Tracer et("QRZ(2)");
207 int n = X.Nrows(); int s = X.Ncols(); int t = Y.Ncols();
208 if (Y.Nrows() != n)
209 { Throw(ProgramException("Unequal column lengths",X,Y)); }
210 M.resize(s,t);
211 Real* xi0 = X.Store(); int k;
212 for (int i=0; i<s; i++)
213 {
214 Real* xj0 = Y.Store();
215 for (int j=0; j<t; j++)
216 {
217 Real sum=0.0;
218 Real* xi=xi0; Real* xj=xj0; k=n;
219 while(k--) { sum += *xi * *xj; xi+=s; xj+=t; }
220 xi=xi0; k=n; xj=xj0++;
221 while(k--) { *xj -= sum * *xi; xj+=t; xi+=s; }
222 M.element(i,j) = sum;
223 }
224 xi0++;
225 }
226}
227*/
228
229void updateQRZT(Matrix& X, LowerTriangularMatrix& L)
230{
231 REPORT
232 Tracer et("updateQRZT");
233 int n = X.Ncols(); int s = X.Nrows();
234 if (s != L.Nrows())
235 Throw(ProgramException("Incompatible dimensions",X,L));
236 if (n == 0 || s == 0) return;
237 Real* xi = X.Store(); int k;
238 for (int i=0; i<s; i++)
239 {
240 Real r = L.element(i,i);
241 Real sum = 0.0;
242 Real* xi0=xi; k=n; while(k--) { sum += square(*xi++); }
243 sum = sqrt(sum + square(r));
244 if (sum == 0.0)
245 {
246 REPORT
247 k=n; while(k--) { *xi0++ = 0.0; }
248 for (int j=i; j<s; j++) L.element(j,i) = 0.0;
249 }
250 else
251 {
252 Real frs = fabs(r) + sum;
253 Real a0 = sqrt(frs / sum); Real alpha = a0 / frs;
254 if (r <= 0) { REPORT L.element(i,i) = sum; alpha = -alpha; }
255 else { REPORT L.element(i,i) = -sum; }
256 Real* xj0=xi0; k=n; while(k--) { *xj0++ *= alpha; }
257 for (int j=i+1; j<s; j++)
258 {
259 sum = 0.0;
260 xi=xi0; Real* xj=xj0; k=n; while(k--) { sum += *xi++ * *xj++; }
261 sum += a0 * L.element(j,i);
262 xi=xi0; k=n; while(k--) { *xj0++ -= sum * *xi++; }
263 L.element(j,i) -= sum * a0;
264 }
265 }
266 }
267}
268
269void updateQRZ(Matrix& X, UpperTriangularMatrix& U)
270{
271 REPORT
272 Tracer et("updateQRZ");
273 int n = X.Nrows(); int s = X.Ncols();
274 if (s != U.Ncols())
275 Throw(ProgramException("Incompatible dimensions",X,U));
276 if (n == 0 || s == 0) return;
277 Real* xi0 = X.Store(); Real* u0 = U.Store(); Real* u;
278 RowVector V(s); Real* v0 = V.Store(); Real* v; V = 0.0;
279 int j, k; int J = s; int i = s;
280 while (i--)
281 {
282 Real* xj0 = xi0; Real* xi = xi0; k = n;
283 if (k) for (;;)
284 {
285 v = v0; Real Xi = *xi; Real* xj = xj0;
286 j = J; while(j--) *v++ += Xi * *xj++;
287 if (!(--k)) break;
288 xi += s; xj0 += s;
289 }
290
291 Real r = *u0;
292 Real sum = sqrt(*v0 + square(r));
293
294 if (sum == 0.0)
295 {
296 REPORT
297 u = u0; v = v0;
298 j = J; while(j--) { *u++ = 0.0; *v++ = 0.0; }
299 xj0 = xi0++; k = n;
300 if (k) for (;;)
301 {
302 *xj0 = 0.0;
303 if (!(--k)) break;
304 xj0 += s;
305 }
306 u0 += J--;
307 }
308 else
309 {
310 Real frs = fabs(r) + sum;
311 Real a0 = sqrt(frs / sum); Real alpha = a0 / frs;
312 if (r <= 0) { REPORT alpha = -alpha; *u0 = sum; }
313 else { REPORT *u0 = -sum; }
314
315 j = J - 1; v = v0 + 1; u = u0 + 1;
316 while (j--)
317 { *v = a0 * *u + alpha * *v; *u -= a0 * *v; ++v; ++u; }
318
319 xj0 = xi0; xi = xi0++; k = n;
320 if (k) for (;;)
321 {
322 v = v0 + 1; Real Xi = *xi; Real* xj = xj0;
323 Xi *= alpha; *xj++ = Xi;
324 j = J - 1; while(j--) *xj++ -= *v++ * Xi;
325 if (!(--k)) break;
326 xi += s; xj0 += s;
327 }
328
329 j = J; v = v0;
330 while (j--) *v++ = 0.0;
331
332 u0 += J--;
333 }
334 }
335}
336
337// Following previous transformation,
338// now apply the same orthogonal transformation to (MX & MU)
339// Need the X Matrix but not the U.
340// Not optimised for accessing consecutive memory
341
342void updateQRZ(const Matrix& X, Matrix& MX, Matrix& MU)
343{
344 REPORT
345 Tracer et("updateQRZ(2)");
346 int s = X.Ncols(); int n = X.Nrows();
347 if (n != MX.Nrows())
348 Throw(ProgramException("Incompatible dimensions",X,MX));
349 if (s != MU.Nrows())
350 Throw(ProgramException("Incompatible dimensions",X,MU));
351 int t = MX.Ncols();
352 if (t != MU.Ncols())
353 Throw(ProgramException("Incompatible dimensions",MX,MU));
354
355 if (s == 0) return;
356
357 const Real* xi0 = X.data(); Real* mx = MX.data(); Real* muj = MU.data();
358 for (int i=1; i<=s; ++i)
359 {
360 Real sum = 0.0;
361 {
362 const Real* xi=xi0; int k=n;
363 while(k--) { sum += square(*xi); xi+= s;}
364 }
365 Real a0 = sqrt(2.0 - sum); Real* mxj0 = mx;
366 for (int j=1; j<=t; ++j)
367 {
368 Real sum = 0.0;
369 const Real* xi=xi0; Real* mxj=mxj0; int k=n;
370 while(--k) { sum += *xi * *mxj; xi += s; mxj += t; }
371 sum += *xi * *mxj; // last line of loop
372 sum += a0 * *muj;
373 xi=xi0; mxj=mxj0; k=n;
374 while(--k) { *mxj -= sum * *xi; xi += s; mxj += t; }
375 *mxj -= sum * *xi; // last line of loop
376 *muj -= sum * a0; ++mxj0; ++muj;
377 }
378 ++xi0;
379 }
380}
381
382
383
384// same thing as updateQRZ(Matrix& X, UpperTriangularMatrix& U)
385// except that X is upper triangular
386// contents of X are destroyed - results are in U
387// assume we can access efficiently by columns
388// e.g. X and U will fit in cache memory
389
390void updateQRZ(UpperTriangularMatrix& X, UpperTriangularMatrix& U)
391{
392 REPORT
393 Tracer et("updateQRZ(3)");
394 int s = X.Ncols();
395 if (s != U.Ncols())
396 Throw(ProgramException("Incompatible dimensions",X,U));
397 if (s == 0) return;
398 Real* xi0 = X.data(); Real* u = U.data();
399 for (int i=1; i<=s; ++i)
400 {
401 Real r = *u; Real sum = 0.0;
402 {
403 Real* xi=xi0; int k=i; int l=s;
404 while(k--) { sum += square(*xi); xi+= --l;}
405 }
406 sum = sqrt(sum + square(r));
407 if (sum == 0.0) { REPORT X.column(i) = 0.0; *u = 0.0; }
408 else
409 {
410 Real frs = fabs(r) + sum;
411 Real a0 = sqrt(frs / sum); Real alpha = a0 / frs;
412 if (r <= 0) { REPORT *u = sum; alpha = -alpha; }
413 else { REPORT *u = -sum; }
414 {
415 Real* xj0=xi0; int k=i; int l=s;
416 while(k--) { *xj0 *= alpha; --l; xj0 += l;}
417 }
418 Real* xj0=xi0; Real* uj=u;
419 for (int j=i+1; j<=s; ++j)
420 {
421 Real sum = 0.0; ++xj0; ++uj;
422 Real* xi=xi0; Real* xj=xj0; int k=i; int l=s;
423 while(k--) { sum += *xi * *xj; --l; xi += l; xj += l; }
424 sum += a0 * *uj;
425 xi=xi0; xj=xj0; k=i; l=s;
426 while(k--) { *xj -= sum * *xi; --l; xi += l; xj += l; }
427 *uj -= sum * a0;
428 }
429 }
430 ++xi0; u += s-i+1;
431 }
432}
433
434// Following previous transformation,
435// now apply the same orthogonal transformation to (MX & MU)
436// Need the X UpperTriangularMatrix but not the U.
437
438void updateQRZ(const UpperTriangularMatrix& X, Matrix& MX, Matrix& MU)
439{
440 REPORT
441 Tracer et("updateQRZ(4)");
442 int s = X.Ncols();
443 if (s != MX.Nrows())
444 Throw(ProgramException("Incompatible dimensions",X,MX));
445 if (s != MU.Nrows())
446 Throw(ProgramException("Incompatible dimensions",X,MU));
447 int t = MX.Ncols();
448 if (t != MU.Ncols())
449 Throw(ProgramException("Incompatible dimensions",MX,MU));
450 if (s == 0) return;
451
452 const Real* xi0 = X.data(); Real* mx = MX.data(); Real* muj = MU.data();
453 for (int i=1; i<=s; ++i)
454 {
455 Real sum = 0.0;
456 {
457 const Real* xi=xi0; int k=i; int l=s;
458 while(k--) { sum += square(*xi); xi+= --l;}
459 }
460 Real a0 = sqrt(2.0 - sum); Real* mxj0 = mx;
461 for (int j=1; j<=t; ++j)
462 {
463 Real sum = 0.0;
464 const Real* xi=xi0; Real* mxj=mxj0; int k=i; int l=s;
465 while(--k) { sum += *xi * *mxj; --l; xi += l; mxj += t; }
466 sum += *xi * *mxj; // last line of loop
467 sum += a0 * *muj;
468 xi=xi0; mxj=mxj0; k=i; l=s;
469 while(--k) { *mxj -= sum * *xi; --l; xi += l; mxj += t; }
470 *mxj -= sum * *xi; // last line of loop
471 *muj -= sum * a0; ++mxj0; ++muj;
472 }
473 ++xi0;
474 }
475}
476
477
478
479
480
481// Matrix A's first n columns are orthonormal
482// so A.Columns(1,n).t() * A.Columns(1,n) is the identity matrix.
483// Fill out the remaining columns of A to make them orthonormal
484// so A.t() * A is the identity matrix
485void extend_orthonormal(Matrix& A, int n)
486{
487 REPORT
488 Tracer et("extend_orthonormal");
489 int nr = A.nrows(); int nc = A.ncols();
490 if (nc > nr) Throw(IncompatibleDimensionsException(A));
491 if (n > nc) Throw(IncompatibleDimensionsException(A));
492 ColumnVector SSR;
493 { Matrix A1 = A.Columns(1,n); SSR = A1.sum_square_rows(); }
494 for (int i = n; i < nc; ++i)
495 {
496 // pick row with smallest SSQ
497 int k; SSR.minimum1(k);
498 // orthogonalise column with 1 at element k, 0 elsewhere
499 // next line is rather inefficient
500 ColumnVector X = - A.Columns(1, i) * A.SubMatrix(k, k, 1, i).t();
501 X(k) += 1.0;
502 // normalise
503 X /= sqrt(X.SumSquare());
504 // update row sums of squares
505 for (k = 1; k <= nr; ++k) SSR(k) += square(X(k));
506 // load new column into matrix
507 A.Column(i+1) = X;
508 }
509}
510
511
512
513
514
515#ifdef use_namespace
516}
517#endif
518
519
520///@}
Note: See TracBrowser for help on using the repository browser.