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 
16 namespace CLHEP {
17 
18 static 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 
60 void back_solve(const HepMatrix &R, HepVector *b)
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 
87 void back_solve(const HepMatrix &R, HepMatrix *b)
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 
121 void 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 
151 void 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 
198 double 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 
224 void 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 
267 void 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 
350 HepVector 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 
368 HepVector 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 
393 void 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 
421 void 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 
459 void house_with_update2(HepSymMatrix *a,HepMatrix *v,int row,int col)
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 
524 HepVector 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 
584 void 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 
610 void 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 
649 void 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 
870 void 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 
879 void 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 
901 void 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
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)
Definition: MatrixLinear.cc:60
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)
Definition: excDblThrow.cc:8
Definition: excDblThrow.cc:17
@ b
@ a