CLHEP VERSION Reference Documentation
   
CLHEP Home Page     CLHEP Documentation     CLHEP Bug Reports

MatrixLinear.cc
Go to the documentation of this file.
1// -*- C++ -*-
2// ---------------------------------------------------------------------------
3//
4// This file is a part of the CLHEP - a Class Library for High Energy Physics.
5//
6// This is the definition of special linear algebra functions for the
7// HepMatrix class.
8//
9// Many of these algorithms are taken from Golub and Van Loan.
10//
11
12#include "CLHEP/Matrix/Matrix.h"
13#include "CLHEP/Matrix/Vector.h"
14#include "CLHEP/Matrix/SymMatrix.h"
15
16namespace CLHEP {
17
18static inline int sign(double x) { return (x>0 ? 1: -1);}
19
20/* -----------------------------------------------------------------------
21
22 The following contains stuff to try to do basic things with matrixes.
23 The functions are:
24
25 back_solve - Solves R*x = b where R is upper triangular.
26 Also has a variation that solves a number of equations
27 of this form in one step, where b is a matrix with
28 each column a different vector. See also solve.
29 col_givens - Does a column Givens update.
30 col_house - Does a column Householder update.
31 condition - Find the conditon number of a symmetric matrix.
32 diag_step - Implicit symmetric QR step with Wilkinson Shift
33 diagonalize - Diagonalize a symmetric matrix.
34 givens - algorithm 5.1.5 in Golub and Van Loan
35 house - Returns a Householder vector to zero elements.
36 house_with_update - Finds and does Householder reflection on matrix.
37 qr_inverse - Finds the inverse of a matrix. Note, often what you
38 really want is solve or backsolve, they can be much
39 quicker than inverse in many calculations.
40 min_line_dist - Takes a set of lines and returns the point nearest to
41 all the lines in the least squares sense.
42 qr_decomp - Does a QR decomposition of a matrix.
43 row_givens - Does a row Givens update.
44 row_house - Does a row Householder update.
45 qr_solve - Works like backsolve, except matrix does not need
46 to be upper triangular. For nonsquare matrix, it
47 solves in the least square sense.
48 tridiagonal - Does a Householder tridiagonalization of a symmetric
49 matrix.
50 ----------------------------------------------------------------------- */
51
52/* -----------------------------------------------------------------------
53 back_solve Version: 1.00 Date Last Changed: 2/9/93
54
55 This solves the system R*x=b where R is upper triangular. Since b
56 is most likely not interesting after this step, x is returned in b.
57 This is algorithm 3.1.2 in Golub & Van Loan
58 ----------------------------------------------------------------------- */
59
61{
62 (*b)(b->num_row()) /= R(b->num_row(),b->num_row());
63 int n = R.num_col();
64 int nb = b->num_row();
65 HepMatrix::mIter br = b->m.begin() + b->num_row() - 2;
66 HepMatrix::mcIter Rrr = R.m.begin() + (nb-2) * (n+1);
67 for (int r=b->num_row()-1;r>=1;--r) {
68 HepMatrix::mIter bc = br+1;
69 HepMatrix::mcIter Rrc = Rrr+1;
70 for (int c=r+1;c<=b->num_row();c++) {
71 (*br)-=(*(Rrc++))*(*(bc++));
72 }
73 (*br)/=(*Rrr);
74 if(r>1) {
75 br--;
76 Rrr -= n+1;
77 }
78 }
79}
80
81/* -----------------------------------------------------------------------
82 Variation that solves a set of equations R*x=b in one step, where b
83 is a Matrix with columns each a different vector. Solution is again
84 returned in b.
85 ----------------------------------------------------------------------- */
86
88{
89 int n = R.num_col();
90 int nb = b->num_row();
91 int nc = b->num_col();
92 HepMatrix::mIter bbi = b->m.begin() + (nb - 2) * nc;
93 for (int i=1;i<=b->num_col();i++) {
94 (*b)(b->num_row(),i) /= R(b->num_row(),b->num_row());
95 HepMatrix::mcIter Rrr = R.m.begin() + (nb-2) * (n+1);
96 HepMatrix::mIter bri = bbi;
97 for (int r=b->num_row()-1;r>=1;--r) {
98 HepMatrix::mIter bci = bri + nc;
99 HepMatrix::mcIter Rrc = Rrr+1;
100 for (int c=r+1;c<=b->num_row();c++) {
101 (*bri)-= (*(Rrc++)) * (*bci);
102 if(c<b->num_row()) bci += nc;
103 }
104 (*bri)/= (*Rrr);
105 if(r>1) {
106 Rrr -= (n+1);
107 bri -= nc;
108 }
109 }
110 bbi++;
111 }
112}
113
114/* -----------------------------------------------------------------------
115 col_givens Version: 1.00 Date Last Changed: 1/28/93
116
117 This does the same thing as row_givens, except it works for columns.
118 It replaces A with A*G.
119 ----------------------------------------------------------------------- */
120
121void col_givens(HepMatrix *A, double c,double ds,
122 int k1, int k2, int row_min, int row_max) {
123 if (row_max<=0) row_max = A->num_row();
124 int n = A->num_col();
125 HepMatrix::mIter Ajk1 = A->m.begin() + (row_min - 1) * n + k1 - 1;
126 HepMatrix::mIter Ajk2 = A->m.begin() + (row_min - 1) * n + k2 - 1;
127 for (int j=row_min;j<=row_max;j++) {
128 double tau1=(*Ajk1); double tau2=(*Ajk2);
129 (*Ajk1)=c*tau1-ds*tau2;(*Ajk2)=ds*tau1+c*tau2;
130 if(j<row_max) {
131 Ajk1 += n;
132 Ajk2 += n;
133 }
134 }
135}
136
137/* -----------------------------------------------------------------------
138 col_house Version: 1.00 Date Last Changed: 1/28/93
139
140 This replaces A with A*P where P=I-2v*v.T/(v.T*v). If row and col are
141 not one, then it only acts on the subpart of A, from A(row,col) to
142 A(A.num_row(),A.num_row()). For use with house, can pass V as a matrix.
143 Then row_start and col_start describe where the vector lies. It is
144 assumed to run from V(row_start,col_start) to
145 V(row_start+A.num_row()-row,col_start).
146 Since typically row_house comes after house, and v.normsq is calculated
147 in house, it is passed as a arguement. Also supplied without vnormsq.
148 This does a column Householder update.
149 ----------------------------------------------------------------------- */
150
151void col_house(HepMatrix *a,const HepMatrix &v,double vnormsq,
152 int row, int col, int row_start,int col_start) {
153 double beta=-2/vnormsq;
154
155 // This is a fast way of calculating w=beta*A.sub(row,n,col,n)*v
156
157 HepVector w(a->num_col()-col+1,0);
158/* not tested */
159 HepMatrix::mIter wptr = w.m.begin();
160 int n = a->num_col();
161 int nv = v.num_col();
162 HepMatrix::mIter acrb = a->m.begin() + (col-1) * n + (row-1);
163 int c;
164 for (c=col;c<=a->num_col();c++) {
165 HepMatrix::mcIter vp = v.m.begin() + (row_start-1) * nv + (col_start-1);
166 HepMatrix::mcIter acr = acrb;
167 for (int r=row;r<=a->num_row();r++) {
168 (*wptr)+=(*(acr++))*(*vp);
169 vp += nv;
170 }
171 wptr++;
172 if(c<a->num_col()) acrb += n;
173 }
174 w*=beta;
175
176 // Fast way of calculating A.sub=A.sub+w*v.T()
177
178 HepMatrix::mIter arcb = a->m.begin() + (row-1) * n + col-1;
179 wptr = w.m.begin();
180 for (int r=row; r<=a->num_row();r++) {
181 HepMatrix::mIter arc = arcb;
182 HepMatrix::mcIter vp = v.m.begin() + (row_start-1) * nv + col_start;
183 for (c=col;c<=a->num_col();c++) {
184 (*(arc++))+=(*vp)*(*wptr);
185 vp += nv;
186 }
187 wptr++;
188 if(r<a->num_row()) arcb += n;
189 }
190}
191
192/* -----------------------------------------------------------------------
193 condition Version: 1.00 Date Last Changed: 1/28/93
194
195 This finds the condition number of SymMatrix.
196 ----------------------------------------------------------------------- */
197
198double condition(const HepSymMatrix &hm)
199{
200 HepSymMatrix mcopy=hm;
201 diagonalize(&mcopy);
202 double max,min;
203 max=min=fabs(mcopy(1,1));
204
205 int n = mcopy.num_row();
206 HepMatrix::mIter mii = mcopy.m.begin() + 2;
207 for (int i=2; i<=n; i++) {
208 if (max<fabs(*mii)) max=fabs(*mii);
209 if (min>fabs(*mii)) min=fabs(*mii);
210 if(i<n) mii += i+1;
211 }
212 return max/min;
213}
214
215/* -----------------------------------------------------------------------
216 diag_step Version: 1.00 Date Last Changed: 1/28/93
217
218 This does a Implicit symmetric QR step with Wilkinson Shift. See
219 algorithm 8.2.2 in Golub and Van Loan. begin and end mark the submatrix
220 of t to do the QR step on, the matrix runs from t(begin,begin) to
221 t(end,end). Can include Matrix *U to be updated so that told = U*t*U.T();
222 ----------------------------------------------------------------------- */
223
224void diag_step(HepSymMatrix *t,int begin,int end)
225{
226 double d=(t->fast(end-1,end-1)-t->fast(end,end))/2;
227 double mu=t->fast(end,end)-t->fast(end,end-1)*t->fast(end,end-1)/
228 (d+sign(d)*sqrt(d*d+ t->fast(end,end-1)*t->fast(end,end-1)));
229 double x=t->fast(begin,begin)-mu;
230 double z=t->fast(begin+1,begin);
231 HepMatrix::mIter tkk = t->m.begin() + (begin+2)*(begin-1)/2;
232 HepMatrix::mIter tkp1k = tkk + begin;
233 HepMatrix::mIter tkp2k = tkk + 2 * begin + 1;
234 for (int k=begin;k<=end-1;k++) {
235 double c,ds;
236 givens(x,z,&c,&ds);
237
238 // This is the result of G.T*t*G, making use of the special structure
239 // of t and G. Note that since t is symmetric, only the lower half
240 // needs to be updated. Equations were gotten from maple.
241
242 if (k!=begin)
243 {
244 *(tkk-1)= *(tkk-1)*c-(*(tkp1k-1))*ds;
245 *(tkp1k-1)=0;
246 }
247 double ap=(*tkk);
248 double bp=(*tkp1k);
249 double aq=(*tkp1k+1);
250 (*tkk)=ap*c*c-2*c*bp*ds+aq*ds*ds;
251 (*tkp1k)=c*ap*ds+bp*c*c-bp*ds*ds-ds*aq*c;
252 (*(tkp1k+1))=ap*ds*ds+2*c*bp*ds+aq*c*c;
253 if (k<end-1)
254 {
255 double bq=(*(tkp2k+1));
256 (*tkp2k)=-bq*ds;
257 (*(tkp2k+1))=bq*c;
258 x=(*tkp1k);
259 z=(*tkp2k);
260 tkk += k+1;
261 tkp1k += k+2;
262 }
263 if(k<end-2) tkp2k += k+3;
264 }
265}
266
267void diag_step(HepSymMatrix *t,HepMatrix *u,int begin,int end)
268{
269 double d=(t->fast(end-1,end-1)-t->fast(end,end))/2;
270 double mu=t->fast(end,end)-t->fast(end,end-1)*t->fast(end,end-1)/
271 (d+sign(d)*sqrt(d*d+ t->fast(end,end-1)*t->fast(end,end-1)));
272 double x=t->fast(begin,begin)-mu;
273 double z=t->fast(begin+1,begin);
274 HepMatrix::mIter tkk = t->m.begin() + (begin+2)*(begin-1)/2;
275 HepMatrix::mIter tkp1k = tkk + begin;
276 HepMatrix::mIter tkp2k = tkk + 2 * begin + 1;
277 for (int k=begin;k<=end-1;k++) {
278 double c,ds;
279 givens(x,z,&c,&ds);
280 col_givens(u,c,ds,k,k+1);
281
282 // This is the result of G.T*t*G, making use of the special structure
283 // of t and G. Note that since t is symmetric, only the lower half
284 // needs to be updated. Equations were gotten from maple.
285
286 if (k!=begin) {
287 *(tkk-1)= (*(tkk-1))*c-(*(tkp1k-1))*ds;
288 *(tkp1k-1)=0;
289 }
290 double ap=(*tkk);
291 double bp=(*tkp1k);
292 double aq=(*(tkp1k+1));
293 (*tkk)=ap*c*c-2*c*bp*ds+aq*ds*ds;
294 (*tkp1k)=c*ap*ds+bp*c*c-bp*ds*ds-ds*aq*c;
295 (*(tkp1k+1))=ap*ds*ds+2*c*bp*ds+aq*c*c;
296 if (k<end-1) {
297 double bq=(*(tkp2k+1));
298 (*tkp2k)=-bq*ds;
299 (*(tkp2k+1))=bq*c;
300 x=(*tkp1k);
301 z=(*(tkp2k));
302 tkk += k+1;
303 tkp1k += k+2;
304 }
305 if(k<end-2) tkp2k += k+3;
306 }
307}
308
309/* -----------------------------------------------------------------------
310 diagonalize Version: 1.00 Date Last Changed: 1/28/93
311
312 This subroutine diagonalizes a symmatrix using algorithm 8.2.3 in Golub &
313 Van Loan. It returns the matrix U so that sold = U*sdiag*U.T
314 ----------------------------------------------------------------------- */
316{
317 const double tolerance = 1e-12;
318 HepMatrix u= tridiagonal(hms);
319 int begin=1;
320 int end=hms->num_row();
321 while(begin!=end)
322 {
323 HepMatrix::mIter sii = hms->m.begin() + (begin+2)*(begin-1)/2;
324 HepMatrix::mIter sip1i = sii + begin;
325 for (int i=begin;i<=end-1;i++) {
326 if (fabs(*sip1i)<=
327 tolerance*(fabs(*sii)+fabs(*(sip1i+1)))) {
328 (*sip1i)=0;
329 }
330 if(i<end-1) {
331 sii += i+1;
332 sip1i += i+2;
333 }
334 }
335 while(begin<end && hms->fast(begin+1,begin) ==0) begin++;
336 while(end>begin && hms->fast(end,end-1) ==0) end--;
337 if (begin!=end)
338 diag_step(hms,&u,begin,end);
339 }
340 return u;
341}
342
343/* -----------------------------------------------------------------------
344 house Version: 1.00 Date Last Changed: 1/28/93
345
346 This return a Householder Vector to zero the elements in column col,
347 from row+1 to a.num_row().
348 ----------------------------------------------------------------------- */
349
350HepVector house(const HepSymMatrix &a,int row,int col)
351{
352 HepVector v(a.num_row()-row+1);
353/* not tested */
354 HepMatrix::mIter vp = v.m.begin();
355 HepMatrix::mcIter aci = a.m.begin() + col * (col - 1) / 2 + row - 1;
356 int i;
357 for (i=row;i<=col;i++) {
358 (*(vp++))=(*(aci++));
359 }
360 for (;i<=a.num_row();i++) {
361 (*(vp++))=(*aci);
362 aci += i;
363 }
364 v(1)+=sign(a(row,col))*v.norm();
365 return v;
366}
367
368HepVector house(const HepMatrix &a,int row,int col)
369{
370 HepVector v(a.num_row()-row+1);
371/* not tested */
372 int n = a.num_col();
373 HepMatrix::mcIter aic = a.m.begin() + (row - 1) * n + (col - 1) ;
374 HepMatrix::mIter vp = v.m.begin();
375 for (int i=row;i<=a.num_row();i++) {
376 (*(vp++))=(*aic);
377 aic += n;
378 }
379 v(1)+=sign(a(row,col))*v.norm();
380 return v;
381}
382
383/* -----------------------------------------------------------------------
384 house_with_update Version: 1.00 Date Last Changed: 1/28/93
385
386 This returns the householder vector as in house, and it also changes
387 A to be PA. (It is slightly more efficient than calling house, followed
388 by calling row_house). If you include the optional Matrix *house_vector,
389 then the householder vector is stored in house_vector(row,col) to
390 house_vector(a->num_row(),col).
391 ----------------------------------------------------------------------- */
392
393void house_with_update(HepMatrix *a,int row,int col)
394{
395 HepVector v(a->num_row()-row+1);
396/* not tested */
397 HepMatrix::mIter vp = v.m.begin();
398 int n = a->num_col();
399 HepMatrix::mIter arc = a->m.begin() + (row-1) * n + (col-1);
400 int r;
401 for (r=row;r<=a->num_row();r++) {
402 (*(vp++))=(*arc);
403 if(r<a->num_row()) arc += n;
404 }
405 double normsq=v.normsq();
406 double norm=sqrt(normsq);
407 normsq-=v(1)*v(1);
408 v(1)+=sign((*a)(row,col))*norm;
409 normsq+=v(1)*v(1);
410 (*a)(row,col)=-sign((*a)(row,col))*norm;
411 if (row<a->num_row()) {
412 arc = a->m.begin() + row * n + (col-1);
413 for (r=row+1;r<=a->num_row();r++) {
414 (*arc)=0;
415 if(r<a->num_row()) arc += n;
416 }
417 row_house(a,v,normsq,row,col+1);
418 }
419}
420
421void house_with_update(HepMatrix *a,HepMatrix *v,int row,int col)
422{
423 double normsq=0;
424 int nv = v->num_col();
425 int na = a->num_col();
426 HepMatrix::mIter vrc = v->m.begin() + (row-1) * nv + (col-1);
427 HepMatrix::mIter arc = a->m.begin() + (row-1) * na + (col-1);
428 int r;
429 for (r=row;r<=a->num_row();r++) {
430 (*vrc)=(*arc);
431 normsq+=(*vrc)*(*vrc);
432 if(r<a->num_row()) {
433 vrc += nv;
434 arc += na;
435 }
436 }
437 double norm=sqrt(normsq);
438 vrc = v->m.begin() + (row-1) * nv + (col-1);
439 normsq-=(*vrc)*(*vrc);
440 (*vrc)+=sign((*a)(row,col))*norm;
441 normsq+=(*vrc)*(*vrc);
442 (*a)(row,col)=-sign((*a)(row,col))*norm;
443 if (row<a->num_row()) {
444 arc = a->m.begin() + row * na + (col-1);
445 for (r=row+1;r<=a->num_row();r++) {
446 (*arc)=0;
447 if(r<a->num_row()) arc += na;
448 }
449 row_house(a,*v,normsq,row,col+1,row,col);
450 }
451}
452/* -----------------------------------------------------------------------
453 house_with_update2 Version: 1.00 Date Last Changed: 1/28/93
454
455 This is a variation of house_with_update for use with tridiagonalization.
456 It only updates column number col in a SymMatrix.
457 ----------------------------------------------------------------------- */
458
460{
461 double normsq=0;
462 int nv = v->num_col();
463 HepMatrix::mIter vrc = v->m.begin() + (row-1) * nv + (col-1);
464 HepMatrix::mIter arc = a->m.begin() + (row-1) * row / 2 + (col-1);
465 int r;
466 for (r=row;r<=a->num_row();r++)
467 {
468 (*vrc)=(*arc);
469 normsq+=(*vrc)*(*vrc);
470 if(r<a->num_row()) {
471 arc += r;
472 vrc += nv;
473 }
474 }
475 double norm=sqrt(normsq);
476 vrc = v->m.begin() + (row-1) * nv + (col-1);
477 arc = a->m.begin() + (row-1) * row / 2 + (col-1);
478 (*vrc)+=sign(*arc)*norm;
479 (*arc)=-sign(*arc)*norm;
480 arc += row;
481 for (r=row+1;r<=a->num_row();r++) {
482 (*arc)=0;
483 if(r<a->num_row()) arc += r;
484 }
485}
486
487/* -----------------------------------------------------------------------
488 inverse Version: 1.00 Date Last Changed: 2/9/93
489
490 This calculates the inverse of a square matrix. Note that this is
491 often not what you really want to do. Often, you really want solve or
492 backsolve, which can be faster at calculating (e.g. you want the inverse
493 to calculate A^-1*c where c is a vector. Then this is just solve(A,c))
494
495 If A is not need after the calculation, you can call this with *A. A will
496 be destroyed, but the inverse will be calculated quicker.
497 ----------------------------------------------------------------------- */
498
500{
501 HepMatrix Atemp=A;
502 return qr_inverse(&Atemp);
503}
504
506{
507 if (A->num_row()!=A->num_col()) {
508 HepGenMatrix::error("qr_inverse: The matrix is not square.");
509 }
510 HepMatrix QT=qr_decomp(A).T();
511 back_solve(*A,&QT);
512 return QT;
513}
514
515/* -----------------------------------------------------------------------
516 Version: 1.00 Date Last Changed: 5/26/93
517
518 This takes a set of lines described by Xi=Ai*t+Bi and finds the point P
519 that is closest to the lines in the least squares sense. n is the
520 number of lines, and A and B are pointers to arrays of the Vectors Ai
521 and Bi. The array runs from 0 to n-1.
522 ----------------------------------------------------------------------- */
523
524HepVector min_line_dist(const HepVector *const A, const HepVector *const B,
525 int n)
526{
527 // For (P-(A t +B))^2 minimum, we have tmin=dot(A,P-B)/A.normsq(). So
528 // To minimize distance, we want sum_i (P-(Ai tmini +Bi))^2 minimum. This
529 // is solved by having
530 // (sum_i k Ai*Ai.T +1) P - (sum_i k dot(Ai,Bi) Ai + Bi) = 0
531 // where k=1-2/Ai.normsq
532 const double small = 1e-10; // Small number
533 HepSymMatrix C(3,0),I(3,1);
534 HepVector D(3,0);
535 double t;
536 for (int i=0;i<n;i++)
537 {
538 if (fabs(t=A[i].normsq())<small) {
539 C += I;
540 D += B[i];
541 } else {
542 C += vT_times_v(A[i])*(1-2/t)+I;
543 D += dot(A[i],B[i])*(1-2/t)*A[i]+B[i];
544 }
545 }
546 return qr_solve(C,D);
547}
548
549/* -----------------------------------------------------------------------
550 qr_decomp Version: 1.00 Date Last Changed: 1/28/93
551
552 This does a Householder QR decomposition of the passed matrix. The
553 matrix does not have to be square, but the number of rows needs to be
554 >= number of columns. If A is a mxn matrix, Q is mxn and R is nxn.
555 R is returned in the upper left part of A. qr_decomp with a second
556 matrix changes A to R, and returns a set of householder vectors.
557
558 Algorithm is from Golub and Van Loan 5.15.
559 ----------------------------------------------------------------------- */
560
562{
563 HepMatrix hsm(A->num_row(),A->num_col());
564 qr_decomp(A,&hsm);
565 // Try to make Q diagonal
566 // HepMatrix Q(A->num_row(),A->num_col(),1);
567 HepMatrix Q(A->num_row(),A->num_row(),1);
568 for (int j=hsm.num_col();j>=1;--j)
569 row_house(&Q,hsm,j,j,j,j);
570 return Q;
571}
572
573/* -----------------------------------------------------------------------
574 row_givens Version: 1.00 Date Last Changed: 1/28/93
575
576 This algorithm performs a Givens rotation on the left. Given c and s
577 and k1 and k2, this is like forming G= identity except for row k1 and
578 k2 where G(k1,k1)=c, G(k1,k2)=s, G(k2,k1)=-s, G(k2,k2)=c. It replaces
579 A with G.T()*A. A variation allows you to express col_min and col_max,
580 and then only the submatrix of A from (1,col_min) to (num_row(),col_max)
581 are updated. This is algorithm 5.1.6 in Golub and Van Loan.
582 ----------------------------------------------------------------------- */
583
584void row_givens(HepMatrix *A, double c,double ds,
585 int k1, int k2, int col_min, int col_max) {
586 /* not tested */
587 if (col_max==0) col_max = A->num_col();
588 int n = A->num_col();
589 HepMatrix::mIter Ak1j = A->m.begin() + (k1-1) * n + (col_min-1);
590 HepMatrix::mIter Ak2j = A->m.begin() + (k2-1) * n + (col_min-1);
591 for (int j=col_min;j<=col_max;j++) {
592 double tau1=(*Ak1j); double tau2=(*Ak2j);
593 (*(Ak1j++))=c*tau1-ds*tau2;(*(Ak2j++))=ds*tau1+c*tau2;
594 }
595}
596
597/* -----------------------------------------------------------------------
598 row_house Version: 1.00 Date Last Changed: 1/28/93
599
600 This replaces A with P*A where P=I-2v*v.T/(v.T*v). If row and col are
601 not one, then it only acts on the subpart of A, from A(row,col) to
602 A(A.num_row(),A.num_row()). For use with house, can pass V as a matrix.
603 Then row_start and col_start describe where the vector lies. It is
604 assumed to run from V(row_start,col_start) to
605 V(row_start+A.num_row()-row,col_start).
606 Since typically row_house comes after house, and v.normsq is calculated
607 in house, it is passed as a arguement. Also supplied without vnormsq.
608 ----------------------------------------------------------------------- */
609
610void row_house(HepMatrix *a,const HepVector &v,double vnormsq,
611 int row, int col) {
612 double beta=-2/vnormsq;
613
614 // This is a fast way of calculating w=beta*A.sub(row,n,col,n).T()*v
615
616 HepVector w(a->num_col()-col+1,0);
617/* not tested */
618 int na = a->num_col();
619 HepMatrix::mIter wptr = w.m.begin();
620 HepMatrix::mIter arcb = a->m.begin() + (row-1) * na + (col-1);
621 int c;
622 for (c=col;c<=a->num_col();c++) {
623 HepMatrix::mcIter vp = v.m.begin();
624 HepMatrix::mIter arc = arcb;
625 for (int r=row;r<=a->num_row();r++) {
626 (*wptr)+=(*arc)*(*(vp++));
627 if(r<a->num_row()) arc += na;
628 }
629 wptr++;
630 arcb++;
631 }
632 w*=beta;
633
634 // Fast way of calculating A.sub=A.sub+v*w.T()
635
636 arcb = a->m.begin() + (row-1) * na + (col-1);
637 HepMatrix::mcIter vp = v.m.begin();
638 for (int r=row; r<=a->num_row();r++) {
639 HepMatrix::mIter wptr2 = w.m.begin();
640 HepMatrix::mIter arc = arcb;
641 for (c=col;c<=a->num_col();c++) {
642 (*(arc++))+=(*vp)*(*(wptr2++));
643 }
644 vp++;
645 if(r<a->num_row()) arcb += na;
646 }
647}
648
649void row_house(HepMatrix *a,const HepMatrix &v,double vnormsq,
650 int row, int col, int row_start, int col_start) {
651 double beta=-2/vnormsq;
652
653 // This is a fast way of calculating w=beta*A.sub(row,n,col,n).T()*v
654 HepVector w(a->num_col()-col+1,0);
655 int na = a->num_col();
656 int nv = v.num_col();
657 HepMatrix::mIter wptr = w.m.begin();
658 HepMatrix::mIter arcb = a->m.begin() + (row-1) * na + (col-1);
659 HepMatrix::mcIter vpcb = v.m.begin() + (row_start-1) * nv + (col_start-1);
660 int c;
661 for (c=col;c<=a->num_col();c++) {
662 HepMatrix::mIter arc = arcb;
663 HepMatrix::mcIter vpc = vpcb;
664 for (int r=row;r<=a->num_row();r++) {
665 (*wptr)+=(*arc)*(*vpc);
666 if(r<a->num_row()) {
667 arc += na;
668 vpc += nv;
669 }
670 }
671 wptr++;
672 arcb++;
673 }
674 w*=beta;
675
676 arcb = a->m.begin() + (row-1) * na + (col-1);
677 HepMatrix::mcIter vpc = v.m.begin() + (row_start-1) * nv + (col_start-1);
678 for (int r=row; r<=a->num_row();r++) {
679 HepMatrix::mIter arc = arcb;
680 HepMatrix::mIter wptr2 = w.m.begin();
681 for (c=col;c<=a->num_col();c++) {
682 (*(arc++))+=(*vpc)*(*(wptr2++));
683 }
684 if(r<a->num_row()) {
685 arcb += na;
686 vpc += nv;
687 }
688 }
689}
690
691/* -----------------------------------------------------------------------
692 solve Version: 1.00 Date Last Changed: 2/9/93
693
694 This solves the system A*x=b where A is not upper triangular, but it
695 must have full rank. If A is not square, then this is solved in the least
696 squares sense. Has a variation that works for b a matrix with each column
697 being a different vector. If A is not needed after this call, you can
698 call solve with *A. This will destroy A, but it will run faster.
699 ----------------------------------------------------------------------- */
700
702{
703 HepMatrix temp = A;
704 return qr_solve(&temp,b);
705}
706
708{
709 HepMatrix Q=qr_decomp(A);
710 // Quick way to to Q.T*b.
711 HepVector b2(Q.num_col(),0);
712 HepMatrix::mIter b2r = b2.m.begin();
713 HepMatrix::mIter Qr = Q.m.begin();
714 int n = Q.num_col();
715 for (int r=1;r<=b2.num_row();r++) {
716 HepMatrix::mcIter bc = b.m.begin();
717 HepMatrix::mIter Qcr = Qr;
718 for (int c=1;c<=b.num_row();c++) {
719 *b2r += (*Qcr) * (*(bc++));
720 if(c<b.num_row()) Qcr += n;
721 }
722 b2r++;
723 Qr++;
724 }
725 back_solve(*A,&b2);
726 return b2;
727}
728
730{
731 HepMatrix temp = A;
732 return qr_solve(&temp,b);
733}
734
736{
737 HepMatrix Q=qr_decomp(A);
738 // Quick way to to Q.T*b.
739 HepMatrix b2(Q.num_col(),b.num_col(),0);
740 int nb = b.num_col();
741 int nq = Q.num_col();
742 HepMatrix::mcIter b1i = b.m.begin();
743 HepMatrix::mIter b21i = b2.m.begin();
744 for (int i=1;i<=b.num_col();i++) {
745 HepMatrix::mIter Q1r = Q.m.begin();
746 HepMatrix::mIter b2ri = b21i;
747 for (int r=1;r<=b2.num_row();r++) {
748 HepMatrix::mIter Qcr = Q1r;
749 HepMatrix::mcIter bci = b1i;
750 for (int c=1;c<=b.num_row();c++) {
751 *b2ri += (*Qcr) * (*bci);
752 if(c<b.num_row()) {
753 Qcr += nq;
754 bci += nb;
755 }
756 }
757 Q1r++;
758 if(r<b2.num_row()) b2ri += nb;
759 }
760 b1i++;
761 b21i++;
762 }
763 back_solve(*A,&b2);
764 return b2;
765}
766
767/* -----------------------------------------------------------------------
768 tridiagonal Version: 1.00 Date Last Changed: 1/28/93
769
770 This does a Householder tridiagonalization. It takes a symmetric matrix
771 A and changes it to A=U*T*U.T.
772 ----------------------------------------------------------------------- */
773
775{
776 int nh = hsm->num_col();
777 for (int k=1;k<=a->num_col()-2;k++) {
778
779 // If this row is already in tridiagonal form, we can skip the
780 // transformation.
781
782 double scale=0;
783 HepMatrix::mIter ajk = a->m.begin() + k * (k+5) / 2;
784 int j;
785 for (j=k+2;j<=a->num_row();j++) {
786 scale+=fabs(*ajk);
787 if(j<a->num_row()) ajk += j;
788 }
789 if (scale ==0) {
790 HepMatrix::mIter hsmjkp = hsm->m.begin() + k * (nh+1) - 1;
791 for (j=k+1;j<=hsm->num_row();j++) {
792 *hsmjkp = 0;
793 if(j<hsm->num_row()) hsmjkp += nh;
794 }
795 } else {
796 house_with_update2(a,hsm,k+1,k);
797 double normsq=0;
798 HepMatrix::mIter hsmrptrkp = hsm->m.begin() + k * (nh+1) - 1;
799 int rptr;
800 for (rptr=k+1;rptr<=hsm->num_row();rptr++) {
801 normsq+=(*hsmrptrkp)*(*hsmrptrkp);
802 if(rptr<hsm->num_row()) hsmrptrkp += nh;
803 }
804 HepVector p(a->num_row()-k,0);
805 rptr=k+1;
806 HepMatrix::mIter pr = p.m.begin();
807 int r;
808 for (r=1;r<=p.num_row();r++) {
809 HepMatrix::mIter hsmcptrkp = hsm->m.begin() + k * (nh+1) - 1;
810 int cptr;
811 for (cptr=k+1;cptr<=rptr;cptr++) {
812 (*pr)+=a->fast(rptr,cptr)*(*hsmcptrkp);
813 if(cptr<a->num_col()) hsmcptrkp += nh;
814 }
815 for (;cptr<=a->num_col();cptr++) {
816 (*pr)+=a->fast(cptr,rptr)*(*hsmcptrkp);
817 if(cptr<a->num_col()) hsmcptrkp += nh;
818 }
819 (*pr)*=2/normsq;
820 rptr++;
821 pr++;
822 }
823 double pdotv=0;
824 pr = p.m.begin();
825 hsmrptrkp = hsm->m.begin() + k * (nh+1) - 1;
826 for (r=1;r<=p.num_row();r++) {
827 pdotv+=(*(pr++))*(*hsmrptrkp);
828 if(r<p.num_row()) hsmrptrkp += nh;
829 }
830 pr = p.m.begin();
831 hsmrptrkp = hsm->m.begin() + k * (nh+1) - 1;
832 for (r=1;r<=p.num_row();r++) {
833 (*(pr++))-=pdotv*(*hsmrptrkp)/normsq;
834 if(r<p.num_row()) hsmrptrkp += nh;
835 }
836 rptr=k+1;
837 pr = p.m.begin();
838 hsmrptrkp = hsm->m.begin() + k * (nh+1) - 1;
839 for (r=1;r<=p.num_row();r++) {
840 int cptr=k+1;
841 HepMatrix::mIter mpc = p.m.begin();
842 HepMatrix::mIter hsmcptrkp = hsm->m.begin() + k * (nh+1) - 1;
843 for (int c=1;c<=r;c++) {
844 a->fast(rptr,cptr)-= (*hsmrptrkp)*(*(mpc++))+(*pr)*(*hsmcptrkp);
845 cptr++;
846 if(c<r) hsmcptrkp += nh;
847 }
848 pr++;
849 rptr++;
850 if(r<p.num_row()) hsmrptrkp += nh;
851 }
852 }
853 }
854}
855
857{
858 HepMatrix U(a->num_row(),a->num_col(),1);
859 if (a->num_col()>2)
860 {
861 HepMatrix hsm(a->num_col(),a->num_col()-2,0);
862 tridiagonal(a,&hsm);
863 for (int j=hsm.num_col();j>=1;--j) {
864 row_house(&U,hsm,j,j,j,j);
865 }
866 }
867 return U;
868}
869
870void col_house(HepMatrix *a,const HepMatrix &v,int row,int col,
871 int row_start,int col_start)
872{
873 double normsq=0;
874 for (int i=row_start;i<=row_start+a->num_row()-row;i++)
875 normsq+=v(i,col)*v(i,col);
876 col_house(a,v,normsq,row,col,row_start,col_start);
877}
878
879void givens(double a, double b, double *c, double *ds)
880{
881 if (b ==0) { *c=1; *ds=0; }
882 else {
883 if (fabs(b)>fabs(a)) {
884 double tau=-a/b;
885 *ds=1/sqrt(1+tau*tau);
886 *c=(*ds)*tau;
887 } else {
888 double tau=-b/a;
889 *c=1/sqrt(1+tau*tau);
890 *ds=(*c)*tau;
891 }
892 }
893}
894
896{
897 for (int i=1;i<=A->num_col();i++)
898 house_with_update(A,hsm,i,i);
899}
900
901void row_house(HepMatrix *a,const HepMatrix &v,int row,int col,
902 int row_start,int col_start)
903{
904 double normsq=0;
905 int end = row_start+a->num_row()-row;
906 for (int i=row_start; i<=end; i++)
907 normsq += v(i,col)*v(i,col);
908 // If v is 0, then we can skip doing row_house.
909 if (normsq !=0)
910 row_house(a,v,normsq,row,col,row_start,col_start);
911}
912
913} // namespace CLHEP
std::vector< double, Alloc< double, 25 > >::iterator mIter
std::vector< double, Alloc< double, 25 > >::const_iterator mcIter
static void error(const char *s)
Definition GenMatrix.cc:73
virtual int num_col() const
Definition Matrix.cc:122
virtual int num_row() const
Definition Matrix.cc:120
HepMatrix T() const
Definition Matrix.cc:456
int num_row() const
const double & fast(int row, int col) const
double norm() const
virtual int num_row() const
Definition Vector.cc:117
double normsq() const
void col_house(HepMatrix *a, const HepMatrix &v, double vnormsq, int row, int col, int row_start, int col_start)
void tridiagonal(HepSymMatrix *a, HepMatrix *hsm)
HepVector qr_solve(const HepMatrix &A, const HepVector &b)
HepSymMatrix vT_times_v(const HepVector &v)
Definition SymMatrix.cc:542
void house_with_update2(HepSymMatrix *a, HepMatrix *v, int row=1, int col=1)
double norm(const HepGenMatrix &m)
Definition GenMatrix.cc:57
HepMatrix qr_inverse(const HepMatrix &A)
void house_with_update(HepMatrix *a, int row=1, int col=1)
void back_solve(const HepMatrix &R, HepVector *b)
void qr_decomp(HepMatrix *A, HepMatrix *hsm)
double dot(const HepVector &v1, const HepVector &v2)
Definition Vector.cc:543
HepVector house(const HepMatrix &a, int row=1, int col=1)
HepVector min_line_dist(const HepVector *const A, const HepVector *const B, int n)
void row_givens(HepMatrix *A, double c, double s, int k1, int k2, int col_min=1, int col_max=0)
void givens(double a, double b, double *c, double *s)
void col_givens(HepMatrix *A, double c, double s, int k1, int k2, int row_min=1, int row_max=0)
void row_house(HepMatrix *a, const HepVector &v, double vnormsq, int row=1, int col=1)
double condition(const HepSymMatrix &m)
HepMatrix diagonalize(HepSymMatrix *s)
void diag_step(HepSymMatrix *t, int begin, int end)