CLHEP VERSION Reference Documentation
CLHEP Home Page CLHEP Documentation CLHEP Bug Reports |
00001 // -*- C++ -*- 00002 // --------------------------------------------------------------------------- 00003 // 00004 // This file is a part of the CLHEP - a Class Library for High Energy Physics. 00005 // 00006 // This is the definition of special linear algebra functions for the 00007 // HepMatrix class. 00008 // 00009 // Many of these algorithms are taken from Golub and Van Loan. 00010 // 00011 00012 #include "CLHEP/Matrix/Matrix.h" 00013 #include "CLHEP/Matrix/Vector.h" 00014 #include "CLHEP/Matrix/SymMatrix.h" 00015 00016 namespace CLHEP { 00017 00018 static inline int sign(double x) { return (x>0 ? 1: -1);} 00019 00020 /* ----------------------------------------------------------------------- 00021 00022 The following contains stuff to try to do basic things with matrixes. 00023 The functions are: 00024 00025 back_solve - Solves R*x = b where R is upper triangular. 00026 Also has a variation that solves a number of equations 00027 of this form in one step, where b is a matrix with 00028 each column a different vector. See also solve. 00029 col_givens - Does a column Givens update. 00030 col_house - Does a column Householder update. 00031 condition - Find the conditon number of a symmetric matrix. 00032 diag_step - Implicit symmetric QR step with Wilkinson Shift 00033 diagonalize - Diagonalize a symmetric matrix. 00034 givens - algorithm 5.1.5 in Golub and Van Loan 00035 house - Returns a Householder vector to zero elements. 00036 house_with_update - Finds and does Householder reflection on matrix. 00037 qr_inverse - Finds the inverse of a matrix. Note, often what you 00038 really want is solve or backsolve, they can be much 00039 quicker than inverse in many calculations. 00040 min_line_dist - Takes a set of lines and returns the point nearest to 00041 all the lines in the least squares sense. 00042 qr_decomp - Does a QR decomposition of a matrix. 00043 row_givens - Does a row Givens update. 00044 row_house - Does a row Householder update. 00045 qr_solve - Works like backsolve, except matrix does not need 00046 to be upper triangular. For nonsquare matrix, it 00047 solves in the least square sense. 00048 tridiagonal - Does a Householder tridiagonalization of a symmetric 00049 matrix. 00050 ----------------------------------------------------------------------- */ 00051 00052 /* ----------------------------------------------------------------------- 00053 back_solve Version: 1.00 Date Last Changed: 2/9/93 00054 00055 This solves the system R*x=b where R is upper triangular. Since b 00056 is most likely not interesting after this step, x is returned in b. 00057 This is algorithm 3.1.2 in Golub & Van Loan 00058 ----------------------------------------------------------------------- */ 00059 00060 void back_solve(const HepMatrix &R, HepVector *b) 00061 { 00062 (*b)(b->num_row()) /= R(b->num_row(),b->num_row()); 00063 int n = R.num_col(); 00064 int nb = b->num_row(); 00065 HepMatrix::mIter br = b->m.begin() + b->num_row() - 2; 00066 HepMatrix::mcIter Rrr = R.m.begin() + (nb-2) * (n+1); 00067 for (int r=b->num_row()-1;r>=1;--r) { 00068 HepMatrix::mIter bc = br+1; 00069 HepMatrix::mcIter Rrc = Rrr+1; 00070 for (int c=r+1;c<=b->num_row();c++) { 00071 (*br)-=(*(Rrc++))*(*(bc++)); 00072 } 00073 (*br)/=(*Rrr); 00074 if(r>1) { 00075 br--; 00076 Rrr -= n+1; 00077 } 00078 } 00079 } 00080 00081 /* ----------------------------------------------------------------------- 00082 Variation that solves a set of equations R*x=b in one step, where b 00083 is a Matrix with columns each a different vector. Solution is again 00084 returned in b. 00085 ----------------------------------------------------------------------- */ 00086 00087 void back_solve(const HepMatrix &R, HepMatrix *b) 00088 { 00089 int n = R.num_col(); 00090 int nb = b->num_row(); 00091 int nc = b->num_col(); 00092 HepMatrix::mIter bbi = b->m.begin() + (nb - 2) * nc; 00093 for (int i=1;i<=b->num_col();i++) { 00094 (*b)(b->num_row(),i) /= R(b->num_row(),b->num_row()); 00095 HepMatrix::mcIter Rrr = R.m.begin() + (nb-2) * (n+1); 00096 HepMatrix::mIter bri = bbi; 00097 for (int r=b->num_row()-1;r>=1;--r) { 00098 HepMatrix::mIter bci = bri + nc; 00099 HepMatrix::mcIter Rrc = Rrr+1; 00100 for (int c=r+1;c<=b->num_row();c++) { 00101 (*bri)-= (*(Rrc++)) * (*bci); 00102 if(c<b->num_row()) bci += nc; 00103 } 00104 (*bri)/= (*Rrr); 00105 if(r>1) { 00106 Rrr -= (n+1); 00107 bri -= nc; 00108 } 00109 } 00110 bbi++; 00111 } 00112 } 00113 00114 /* ----------------------------------------------------------------------- 00115 col_givens Version: 1.00 Date Last Changed: 1/28/93 00116 00117 This does the same thing as row_givens, except it works for columns. 00118 It replaces A with A*G. 00119 ----------------------------------------------------------------------- */ 00120 00121 void col_givens(HepMatrix *A, double c,double s, 00122 int k1, int k2, int row_min, int row_max) { 00123 if (row_max<=0) row_max = A->num_row(); 00124 int n = A->num_col(); 00125 HepMatrix::mIter Ajk1 = A->m.begin() + (row_min - 1) * n + k1 - 1; 00126 HepMatrix::mIter Ajk2 = A->m.begin() + (row_min - 1) * n + k2 - 1; 00127 for (int j=row_min;j<=row_max;j++) { 00128 double tau1=(*Ajk1); double tau2=(*Ajk2); 00129 (*Ajk1)=c*tau1-s*tau2;(*Ajk2)=s*tau1+c*tau2; 00130 if(j<row_max) { 00131 Ajk1 += n; 00132 Ajk2 += n; 00133 } 00134 } 00135 } 00136 00137 /* ----------------------------------------------------------------------- 00138 col_house Version: 1.00 Date Last Changed: 1/28/93 00139 00140 This replaces A with A*P where P=I-2v*v.T/(v.T*v). If row and col are 00141 not one, then it only acts on the subpart of A, from A(row,col) to 00142 A(A.num_row(),A.num_row()). For use with house, can pass V as a matrix. 00143 Then row_start and col_start describe where the vector lies. It is 00144 assumed to run from V(row_start,col_start) to 00145 V(row_start+A.num_row()-row,col_start). 00146 Since typically row_house comes after house, and v.normsq is calculated 00147 in house, it is passed as a arguement. Also supplied without vnormsq. 00148 This does a column Householder update. 00149 ----------------------------------------------------------------------- */ 00150 00151 void col_house(HepMatrix *a,const HepMatrix &v,double vnormsq, 00152 int row, int col, int row_start,int col_start) { 00153 double beta=-2/vnormsq; 00154 00155 // This is a fast way of calculating w=beta*A.sub(row,n,col,n)*v 00156 00157 HepVector w(a->num_col()-col+1,0); 00158 /* not tested */ 00159 HepMatrix::mIter wptr = w.m.begin(); 00160 int n = a->num_col(); 00161 int nv = v.num_col(); 00162 HepMatrix::mIter acrb = a->m.begin() + (col-1) * n + (row-1); 00163 int c; 00164 for (c=col;c<=a->num_col();c++) { 00165 HepMatrix::mcIter vp = v.m.begin() + (row_start-1) * nv + (col_start-1); 00166 HepMatrix::mcIter acr = acrb; 00167 for (int r=row;r<=a->num_row();r++) { 00168 (*wptr)+=(*(acr++))*(*vp); 00169 vp += nv; 00170 } 00171 wptr++; 00172 if(c<a->num_col()) acrb += n; 00173 } 00174 w*=beta; 00175 00176 // Fast way of calculating A.sub=A.sub+w*v.T() 00177 00178 HepMatrix::mIter arcb = a->m.begin() + (row-1) * n + col-1; 00179 wptr = w.m.begin(); 00180 for (int r=row; r<=a->num_row();r++) { 00181 HepMatrix::mIter arc = arcb; 00182 HepMatrix::mcIter vp = v.m.begin() + (row_start-1) * nv + col_start; 00183 for (c=col;c<=a->num_col();c++) { 00184 (*(arc++))+=(*vp)*(*wptr); 00185 vp += nv; 00186 } 00187 wptr++; 00188 if(r<a->num_row()) arcb += n; 00189 } 00190 } 00191 00192 /* ----------------------------------------------------------------------- 00193 condition Version: 1.00 Date Last Changed: 1/28/93 00194 00195 This finds the condition number of SymMatrix. 00196 ----------------------------------------------------------------------- */ 00197 00198 double condition(const HepSymMatrix &m) 00199 { 00200 HepSymMatrix mcopy=m; 00201 diagonalize(&mcopy); 00202 double max,min; 00203 max=min=fabs(mcopy(1,1)); 00204 00205 int n = mcopy.num_row(); 00206 HepMatrix::mIter mii = mcopy.m.begin() + 2; 00207 for (int i=2; i<=n; i++) { 00208 if (max<fabs(*mii)) max=fabs(*mii); 00209 if (min>fabs(*mii)) min=fabs(*mii); 00210 if(i<n) mii += i+1; 00211 } 00212 return max/min; 00213 } 00214 00215 /* ----------------------------------------------------------------------- 00216 diag_step Version: 1.00 Date Last Changed: 1/28/93 00217 00218 This does a Implicit symmetric QR step with Wilkinson Shift. See 00219 algorithm 8.2.2 in Golub and Van Loan. begin and end mark the submatrix 00220 of t to do the QR step on, the matrix runs from t(begin,begin) to 00221 t(end,end). Can include Matrix *U to be updated so that told = U*t*U.T(); 00222 ----------------------------------------------------------------------- */ 00223 00224 void diag_step(HepSymMatrix *t,int begin,int end) 00225 { 00226 double d=(t->fast(end-1,end-1)-t->fast(end,end))/2; 00227 double mu=t->fast(end,end)-t->fast(end,end-1)*t->fast(end,end-1)/ 00228 (d+sign(d)*sqrt(d*d+ t->fast(end,end-1)*t->fast(end,end-1))); 00229 double x=t->fast(begin,begin)-mu; 00230 double z=t->fast(begin+1,begin); 00231 HepMatrix::mIter tkk = t->m.begin() + (begin+2)*(begin-1)/2; 00232 HepMatrix::mIter tkp1k = tkk + begin; 00233 HepMatrix::mIter tkp2k = tkk + 2 * begin + 1; 00234 for (int k=begin;k<=end-1;k++) { 00235 double c,s; 00236 givens(x,z,&c,&s); 00237 00238 // This is the result of G.T*t*G, making use of the special structure 00239 // of t and G. Note that since t is symmetric, only the lower half 00240 // needs to be updated. Equations were gotten from maple. 00241 00242 if (k!=begin) 00243 { 00244 *(tkk-1)= *(tkk-1)*c-(*(tkp1k-1))*s; 00245 *(tkp1k-1)=0; 00246 } 00247 double ap=(*tkk); 00248 double bp=(*tkp1k); 00249 double aq=(*tkp1k+1); 00250 (*tkk)=ap*c*c-2*c*bp*s+aq*s*s; 00251 (*tkp1k)=c*ap*s+bp*c*c-bp*s*s-s*aq*c; 00252 (*(tkp1k+1))=ap*s*s+2*c*bp*s+aq*c*c; 00253 if (k<end-1) 00254 { 00255 double bq=(*(tkp2k+1)); 00256 (*tkp2k)=-bq*s; 00257 (*(tkp2k+1))=bq*c; 00258 x=(*tkp1k); 00259 z=(*tkp2k); 00260 tkk += k+1; 00261 tkp1k += k+2; 00262 } 00263 if(k<end-2) tkp2k += k+3; 00264 } 00265 } 00266 00267 void diag_step(HepSymMatrix *t,HepMatrix *u,int begin,int end) 00268 { 00269 double d=(t->fast(end-1,end-1)-t->fast(end,end))/2; 00270 double mu=t->fast(end,end)-t->fast(end,end-1)*t->fast(end,end-1)/ 00271 (d+sign(d)*sqrt(d*d+ t->fast(end,end-1)*t->fast(end,end-1))); 00272 double x=t->fast(begin,begin)-mu; 00273 double z=t->fast(begin+1,begin); 00274 HepMatrix::mIter tkk = t->m.begin() + (begin+2)*(begin-1)/2; 00275 HepMatrix::mIter tkp1k = tkk + begin; 00276 HepMatrix::mIter tkp2k = tkk + 2 * begin + 1; 00277 for (int k=begin;k<=end-1;k++) { 00278 double c,s; 00279 givens(x,z,&c,&s); 00280 col_givens(u,c,s,k,k+1); 00281 00282 // This is the result of G.T*t*G, making use of the special structure 00283 // of t and G. Note that since t is symmetric, only the lower half 00284 // needs to be updated. Equations were gotten from maple. 00285 00286 if (k!=begin) { 00287 *(tkk-1)= (*(tkk-1))*c-(*(tkp1k-1))*s; 00288 *(tkp1k-1)=0; 00289 } 00290 double ap=(*tkk); 00291 double bp=(*tkp1k); 00292 double aq=(*(tkp1k+1)); 00293 (*tkk)=ap*c*c-2*c*bp*s+aq*s*s; 00294 (*tkp1k)=c*ap*s+bp*c*c-bp*s*s-s*aq*c; 00295 (*(tkp1k+1))=ap*s*s+2*c*bp*s+aq*c*c; 00296 if (k<end-1) { 00297 double bq=(*(tkp2k+1)); 00298 (*tkp2k)=-bq*s; 00299 (*(tkp2k+1))=bq*c; 00300 x=(*tkp1k); 00301 z=(*(tkp2k)); 00302 tkk += k+1; 00303 tkp1k += k+2; 00304 } 00305 if(k<end-2) tkp2k += k+3; 00306 } 00307 } 00308 00309 /* ----------------------------------------------------------------------- 00310 diagonalize Version: 1.00 Date Last Changed: 1/28/93 00311 00312 This subroutine diagonalizes a symmatrix using algorithm 8.2.3 in Golub & 00313 Van Loan. It returns the matrix U so that sold = U*sdiag*U.T 00314 ----------------------------------------------------------------------- */ 00315 HepMatrix diagonalize(HepSymMatrix *s) 00316 { 00317 const double tolerance = 1e-12; 00318 HepMatrix u= tridiagonal(s); 00319 int begin=1; 00320 int end=s->num_row(); 00321 while(begin!=end) 00322 { 00323 HepMatrix::mIter sii = s->m.begin() + (begin+2)*(begin-1)/2; 00324 HepMatrix::mIter sip1i = sii + begin; 00325 for (int i=begin;i<=end-1;i++) { 00326 if (fabs(*sip1i)<= 00327 tolerance*(fabs(*sii)+fabs(*(sip1i+1)))) { 00328 (*sip1i)=0; 00329 } 00330 if(i<end-1) { 00331 sii += i+1; 00332 sip1i += i+2; 00333 } 00334 } 00335 while(begin<end && s->fast(begin+1,begin) ==0) begin++; 00336 while(end>begin && s->fast(end,end-1) ==0) end--; 00337 if (begin!=end) 00338 diag_step(s,&u,begin,end); 00339 } 00340 return u; 00341 } 00342 00343 /* ----------------------------------------------------------------------- 00344 house Version: 1.00 Date Last Changed: 1/28/93 00345 00346 This return a Householder Vector to zero the elements in column col, 00347 from row+1 to a.num_row(). 00348 ----------------------------------------------------------------------- */ 00349 00350 HepVector house(const HepSymMatrix &a,int row,int col) 00351 { 00352 HepVector v(a.num_row()-row+1); 00353 /* not tested */ 00354 HepMatrix::mIter vp = v.m.begin(); 00355 HepMatrix::mcIter aci = a.m.begin() + col * (col - 1) / 2 + row - 1; 00356 int i; 00357 for (i=row;i<=col;i++) { 00358 (*(vp++))=(*(aci++)); 00359 } 00360 for (;i<=a.num_row();i++) { 00361 (*(vp++))=(*aci); 00362 aci += i; 00363 } 00364 v(1)+=sign(a(row,col))*v.norm(); 00365 return v; 00366 } 00367 00368 HepVector house(const HepMatrix &a,int row,int col) 00369 { 00370 HepVector v(a.num_row()-row+1); 00371 /* not tested */ 00372 int n = a.num_col(); 00373 HepMatrix::mcIter aic = a.m.begin() + (row - 1) * n + (col - 1) ; 00374 HepMatrix::mIter vp = v.m.begin(); 00375 for (int i=row;i<=a.num_row();i++) { 00376 (*(vp++))=(*aic); 00377 aic += n; 00378 } 00379 v(1)+=sign(a(row,col))*v.norm(); 00380 return v; 00381 } 00382 00383 /* ----------------------------------------------------------------------- 00384 house_with_update Version: 1.00 Date Last Changed: 1/28/93 00385 00386 This returns the householder vector as in house, and it also changes 00387 A to be PA. (It is slightly more efficient than calling house, followed 00388 by calling row_house). If you include the optional Matrix *house_vector, 00389 then the householder vector is stored in house_vector(row,col) to 00390 house_vector(a->num_row(),col). 00391 ----------------------------------------------------------------------- */ 00392 00393 void house_with_update(HepMatrix *a,int row,int col) 00394 { 00395 HepVector v(a->num_row()-row+1); 00396 /* not tested */ 00397 HepMatrix::mIter vp = v.m.begin(); 00398 int n = a->num_col(); 00399 HepMatrix::mIter arc = a->m.begin() + (row-1) * n + (col-1); 00400 int r; 00401 for (r=row;r<=a->num_row();r++) { 00402 (*(vp++))=(*arc); 00403 if(r<a->num_row()) arc += n; 00404 } 00405 double normsq=v.normsq(); 00406 double norm=sqrt(normsq); 00407 normsq-=v(1)*v(1); 00408 v(1)+=sign((*a)(row,col))*norm; 00409 normsq+=v(1)*v(1); 00410 (*a)(row,col)=-sign((*a)(row,col))*norm; 00411 if (row<a->num_row()) { 00412 arc = a->m.begin() + row * n + (col-1); 00413 for (r=row+1;r<=a->num_row();r++) { 00414 (*arc)=0; 00415 if(r<a->num_row()) arc += n; 00416 } 00417 row_house(a,v,normsq,row,col+1); 00418 } 00419 } 00420 00421 void house_with_update(HepMatrix *a,HepMatrix *v,int row,int col) 00422 { 00423 double normsq=0; 00424 int nv = v->num_col(); 00425 int na = a->num_col(); 00426 HepMatrix::mIter vrc = v->m.begin() + (row-1) * nv + (col-1); 00427 HepMatrix::mIter arc = a->m.begin() + (row-1) * na + (col-1); 00428 int r; 00429 for (r=row;r<=a->num_row();r++) { 00430 (*vrc)=(*arc); 00431 normsq+=(*vrc)*(*vrc); 00432 if(r<a->num_row()) { 00433 vrc += nv; 00434 arc += na; 00435 } 00436 } 00437 double norm=sqrt(normsq); 00438 vrc = v->m.begin() + (row-1) * nv + (col-1); 00439 normsq-=(*vrc)*(*vrc); 00440 (*vrc)+=sign((*a)(row,col))*norm; 00441 normsq+=(*vrc)*(*vrc); 00442 (*a)(row,col)=-sign((*a)(row,col))*norm; 00443 if (row<a->num_row()) { 00444 arc = a->m.begin() + row * na + (col-1); 00445 for (r=row+1;r<=a->num_row();r++) { 00446 (*arc)=0; 00447 if(r<a->num_row()) arc += na; 00448 } 00449 row_house(a,*v,normsq,row,col+1,row,col); 00450 } 00451 } 00452 /* ----------------------------------------------------------------------- 00453 house_with_update2 Version: 1.00 Date Last Changed: 1/28/93 00454 00455 This is a variation of house_with_update for use with tridiagonalization. 00456 It only updates column number col in a SymMatrix. 00457 ----------------------------------------------------------------------- */ 00458 00459 void house_with_update2(HepSymMatrix *a,HepMatrix *v,int row,int col) 00460 { 00461 double normsq=0; 00462 int nv = v->num_col(); 00463 HepMatrix::mIter vrc = v->m.begin() + (row-1) * nv + (col-1); 00464 HepMatrix::mIter arc = a->m.begin() + (row-1) * row / 2 + (col-1); 00465 int r; 00466 for (r=row;r<=a->num_row();r++) 00467 { 00468 (*vrc)=(*arc); 00469 normsq+=(*vrc)*(*vrc); 00470 if(r<a->num_row()) { 00471 arc += r; 00472 vrc += nv; 00473 } 00474 } 00475 double norm=sqrt(normsq); 00476 vrc = v->m.begin() + (row-1) * nv + (col-1); 00477 arc = a->m.begin() + (row-1) * row / 2 + (col-1); 00478 (*vrc)+=sign(*arc)*norm; 00479 (*arc)=-sign(*arc)*norm; 00480 arc += row; 00481 for (r=row+1;r<=a->num_row();r++) { 00482 (*arc)=0; 00483 if(r<a->num_row()) arc += r; 00484 } 00485 } 00486 00487 /* ----------------------------------------------------------------------- 00488 inverse Version: 1.00 Date Last Changed: 2/9/93 00489 00490 This calculates the inverse of a square matrix. Note that this is 00491 often not what you really want to do. Often, you really want solve or 00492 backsolve, which can be faster at calculating (e.g. you want the inverse 00493 to calculate A^-1*c where c is a vector. Then this is just solve(A,c)) 00494 00495 If A is not need after the calculation, you can call this with *A. A will 00496 be destroyed, but the inverse will be calculated quicker. 00497 ----------------------------------------------------------------------- */ 00498 00499 HepMatrix qr_inverse(const HepMatrix &A) 00500 { 00501 HepMatrix Atemp=A; 00502 return qr_inverse(&Atemp); 00503 } 00504 00505 HepMatrix qr_inverse(HepMatrix *A) 00506 { 00507 if (A->num_row()!=A->num_col()) { 00508 HepGenMatrix::error("qr_inverse: The matrix is not square."); 00509 } 00510 HepMatrix QT=qr_decomp(A).T(); 00511 back_solve(*A,&QT); 00512 return QT; 00513 } 00514 00515 /* ----------------------------------------------------------------------- 00516 Version: 1.00 Date Last Changed: 5/26/93 00517 00518 This takes a set of lines described by Xi=Ai*t+Bi and finds the point P 00519 that is closest to the lines in the least squares sense. n is the 00520 number of lines, and A and B are pointers to arrays of the Vectors Ai 00521 and Bi. The array runs from 0 to n-1. 00522 ----------------------------------------------------------------------- */ 00523 00524 HepVector min_line_dist(const HepVector *const A, const HepVector *const B, 00525 int n) 00526 { 00527 // For (P-(A t +B))^2 minimum, we have tmin=dot(A,P-B)/A.normsq(). So 00528 // To minimize distance, we want sum_i (P-(Ai tmini +Bi))^2 minimum. This 00529 // is solved by having 00530 // (sum_i k Ai*Ai.T +1) P - (sum_i k dot(Ai,Bi) Ai + Bi) = 0 00531 // where k=1-2/Ai.normsq 00532 const double small = 1e-10; // Small number 00533 HepSymMatrix C(3,0),I(3,1); 00534 HepVector D(3,0); 00535 double t; 00536 for (int i=0;i<n;i++) 00537 { 00538 if (fabs(t=A[i].normsq())<small) { 00539 C += I; 00540 D += B[i]; 00541 } else { 00542 C += vT_times_v(A[i])*(1-2/t)+I; 00543 D += dot(A[i],B[i])*(1-2/t)*A[i]+B[i]; 00544 } 00545 } 00546 return qr_solve(C,D); 00547 } 00548 00549 /* ----------------------------------------------------------------------- 00550 qr_decomp Version: 1.00 Date Last Changed: 1/28/93 00551 00552 This does a Householder QR decomposition of the passed matrix. The 00553 matrix does not have to be square, but the number of rows needs to be 00554 >= number of columns. If A is a mxn matrix, Q is mxn and R is nxn. 00555 R is returned in the upper left part of A. qr_decomp with a second 00556 matrix changes A to R, and returns a set of householder vectors. 00557 00558 Algorithm is from Golub and Van Loan 5.15. 00559 ----------------------------------------------------------------------- */ 00560 00561 HepMatrix qr_decomp(HepMatrix *A) 00562 { 00563 HepMatrix hsm(A->num_row(),A->num_col()); 00564 qr_decomp(A,&hsm); 00565 // Try to make Q diagonal 00566 // HepMatrix Q(A->num_row(),A->num_col(),1); 00567 HepMatrix Q(A->num_row(),A->num_row(),1); 00568 for (int j=hsm.num_col();j>=1;--j) 00569 row_house(&Q,hsm,j,j,j,j); 00570 return Q; 00571 } 00572 00573 /* ----------------------------------------------------------------------- 00574 row_givens Version: 1.00 Date Last Changed: 1/28/93 00575 00576 This algorithm performs a Givens rotation on the left. Given c and s 00577 and k1 and k2, this is like forming G= identity except for row k1 and 00578 k2 where G(k1,k1)=c, G(k1,k2)=s, G(k2,k1)=-s, G(k2,k2)=c. It replaces 00579 A with G.T()*A. A variation allows you to express col_min and col_max, 00580 and then only the submatrix of A from (1,col_min) to (num_row(),col_max) 00581 are updated. This is algorithm 5.1.6 in Golub and Van Loan. 00582 ----------------------------------------------------------------------- */ 00583 00584 void row_givens(HepMatrix *A, double c,double s, 00585 int k1, int k2, int col_min, int col_max) { 00586 /* not tested */ 00587 if (col_max==0) col_max = A->num_col(); 00588 int n = A->num_col(); 00589 HepMatrix::mIter Ak1j = A->m.begin() + (k1-1) * n + (col_min-1); 00590 HepMatrix::mIter Ak2j = A->m.begin() + (k2-1) * n + (col_min-1); 00591 for (int j=col_min;j<=col_max;j++) { 00592 double tau1=(*Ak1j); double tau2=(*Ak2j); 00593 (*(Ak1j++))=c*tau1-s*tau2;(*(Ak2j++))=s*tau1+c*tau2; 00594 } 00595 } 00596 00597 /* ----------------------------------------------------------------------- 00598 row_house Version: 1.00 Date Last Changed: 1/28/93 00599 00600 This replaces A with P*A where P=I-2v*v.T/(v.T*v). If row and col are 00601 not one, then it only acts on the subpart of A, from A(row,col) to 00602 A(A.num_row(),A.num_row()). For use with house, can pass V as a matrix. 00603 Then row_start and col_start describe where the vector lies. It is 00604 assumed to run from V(row_start,col_start) to 00605 V(row_start+A.num_row()-row,col_start). 00606 Since typically row_house comes after house, and v.normsq is calculated 00607 in house, it is passed as a arguement. Also supplied without vnormsq. 00608 ----------------------------------------------------------------------- */ 00609 00610 void row_house(HepMatrix *a,const HepVector &v,double vnormsq, 00611 int row, int col) { 00612 double beta=-2/vnormsq; 00613 00614 // This is a fast way of calculating w=beta*A.sub(row,n,col,n).T()*v 00615 00616 HepVector w(a->num_col()-col+1,0); 00617 /* not tested */ 00618 int na = a->num_col(); 00619 HepMatrix::mIter wptr = w.m.begin(); 00620 HepMatrix::mIter arcb = a->m.begin() + (row-1) * na + (col-1); 00621 int c; 00622 for (c=col;c<=a->num_col();c++) { 00623 HepMatrix::mcIter vp = v.m.begin(); 00624 HepMatrix::mIter arc = arcb; 00625 for (int r=row;r<=a->num_row();r++) { 00626 (*wptr)+=(*arc)*(*(vp++)); 00627 if(r<a->num_row()) arc += na; 00628 } 00629 wptr++; 00630 arcb++; 00631 } 00632 w*=beta; 00633 00634 // Fast way of calculating A.sub=A.sub+v*w.T() 00635 00636 arcb = a->m.begin() + (row-1) * na + (col-1); 00637 HepMatrix::mcIter vp = v.m.begin(); 00638 for (int r=row; r<=a->num_row();r++) { 00639 HepMatrix::mIter wptr2 = w.m.begin(); 00640 HepMatrix::mIter arc = arcb; 00641 for (c=col;c<=a->num_col();c++) { 00642 (*(arc++))+=(*vp)*(*(wptr2++)); 00643 } 00644 vp++; 00645 if(r<a->num_row()) arcb += na; 00646 } 00647 } 00648 00649 void row_house(HepMatrix *a,const HepMatrix &v,double vnormsq, 00650 int row, int col, int row_start, int col_start) { 00651 double beta=-2/vnormsq; 00652 00653 // This is a fast way of calculating w=beta*A.sub(row,n,col,n).T()*v 00654 HepVector w(a->num_col()-col+1,0); 00655 int na = a->num_col(); 00656 int nv = v.num_col(); 00657 HepMatrix::mIter wptr = w.m.begin(); 00658 HepMatrix::mIter arcb = a->m.begin() + (row-1) * na + (col-1); 00659 HepMatrix::mcIter vpcb = v.m.begin() + (row_start-1) * nv + (col_start-1); 00660 int c; 00661 for (c=col;c<=a->num_col();c++) { 00662 HepMatrix::mIter arc = arcb; 00663 HepMatrix::mcIter vpc = vpcb; 00664 for (int r=row;r<=a->num_row();r++) { 00665 (*wptr)+=(*arc)*(*vpc); 00666 if(r<a->num_row()) { 00667 arc += na; 00668 vpc += nv; 00669 } 00670 } 00671 wptr++; 00672 arcb++; 00673 } 00674 w*=beta; 00675 00676 arcb = a->m.begin() + (row-1) * na + (col-1); 00677 HepMatrix::mcIter vpc = v.m.begin() + (row_start-1) * nv + (col_start-1); 00678 for (int r=row; r<=a->num_row();r++) { 00679 HepMatrix::mIter arc = arcb; 00680 HepMatrix::mIter wptr2 = w.m.begin(); 00681 for (c=col;c<=a->num_col();c++) { 00682 (*(arc++))+=(*vpc)*(*(wptr2++)); 00683 } 00684 if(r<a->num_row()) { 00685 arcb += na; 00686 vpc += nv; 00687 } 00688 } 00689 } 00690 00691 /* ----------------------------------------------------------------------- 00692 solve Version: 1.00 Date Last Changed: 2/9/93 00693 00694 This solves the system A*x=b where A is not upper triangular, but it 00695 must have full rank. If A is not square, then this is solved in the least 00696 squares sense. Has a variation that works for b a matrix with each column 00697 being a different vector. If A is not needed after this call, you can 00698 call solve with *A. This will destroy A, but it will run faster. 00699 ----------------------------------------------------------------------- */ 00700 00701 HepVector qr_solve(const HepMatrix &A, const HepVector &b) 00702 { 00703 HepMatrix temp = A; 00704 return qr_solve(&temp,b); 00705 } 00706 00707 HepVector qr_solve(HepMatrix *A,const HepVector &b) 00708 { 00709 HepMatrix Q=qr_decomp(A); 00710 // Quick way to to Q.T*b. 00711 HepVector b2(Q.num_col(),0); 00712 HepMatrix::mIter b2r = b2.m.begin(); 00713 HepMatrix::mIter Qr = Q.m.begin(); 00714 int n = Q.num_col(); 00715 for (int r=1;r<=b2.num_row();r++) { 00716 HepMatrix::mcIter bc = b.m.begin(); 00717 HepMatrix::mIter Qcr = Qr; 00718 for (int c=1;c<=b.num_row();c++) { 00719 *b2r += (*Qcr) * (*(bc++)); 00720 if(c<b.num_row()) Qcr += n; 00721 } 00722 b2r++; 00723 Qr++; 00724 } 00725 back_solve(*A,&b2); 00726 return b2; 00727 } 00728 00729 HepMatrix qr_solve(const HepMatrix &A, const HepMatrix &b) 00730 { 00731 HepMatrix temp = A; 00732 return qr_solve(&temp,b); 00733 } 00734 00735 HepMatrix qr_solve(HepMatrix *A,const HepMatrix &b) 00736 { 00737 HepMatrix Q=qr_decomp(A); 00738 // Quick way to to Q.T*b. 00739 HepMatrix b2(Q.num_col(),b.num_col(),0); 00740 int nb = b.num_col(); 00741 int nq = Q.num_col(); 00742 HepMatrix::mcIter b1i = b.m.begin(); 00743 HepMatrix::mIter b21i = b2.m.begin(); 00744 for (int i=1;i<=b.num_col();i++) { 00745 HepMatrix::mIter Q1r = Q.m.begin(); 00746 HepMatrix::mIter b2ri = b21i; 00747 for (int r=1;r<=b2.num_row();r++) { 00748 HepMatrix::mIter Qcr = Q1r; 00749 HepMatrix::mcIter bci = b1i; 00750 for (int c=1;c<=b.num_row();c++) { 00751 *b2ri += (*Qcr) * (*bci); 00752 if(c<b.num_row()) { 00753 Qcr += nq; 00754 bci += nb; 00755 } 00756 } 00757 Q1r++; 00758 if(r<b2.num_row()) b2ri += nb; 00759 } 00760 b1i++; 00761 b21i++; 00762 } 00763 back_solve(*A,&b2); 00764 return b2; 00765 } 00766 00767 /* ----------------------------------------------------------------------- 00768 tridiagonal Version: 1.00 Date Last Changed: 1/28/93 00769 00770 This does a Householder tridiagonalization. It takes a symmetric matrix 00771 A and changes it to A=U*T*U.T. 00772 ----------------------------------------------------------------------- */ 00773 00774 void tridiagonal(HepSymMatrix *a,HepMatrix *hsm) 00775 { 00776 int nh = hsm->num_col(); 00777 for (int k=1;k<=a->num_col()-2;k++) { 00778 00779 // If this row is already in tridiagonal form, we can skip the 00780 // transformation. 00781 00782 double scale=0; 00783 HepMatrix::mIter ajk = a->m.begin() + k * (k+5) / 2; 00784 int j; 00785 for (j=k+2;j<=a->num_row();j++) { 00786 scale+=fabs(*ajk); 00787 if(j<a->num_row()) ajk += j; 00788 } 00789 if (scale ==0) { 00790 HepMatrix::mIter hsmjkp = hsm->m.begin() + k * (nh+1) - 1; 00791 for (j=k+1;j<=hsm->num_row();j++) { 00792 *hsmjkp = 0; 00793 if(j<hsm->num_row()) hsmjkp += nh; 00794 } 00795 } else { 00796 house_with_update2(a,hsm,k+1,k); 00797 double normsq=0; 00798 HepMatrix::mIter hsmrptrkp = hsm->m.begin() + k * (nh+1) - 1; 00799 int rptr; 00800 for (rptr=k+1;rptr<=hsm->num_row();rptr++) { 00801 normsq+=(*hsmrptrkp)*(*hsmrptrkp); 00802 if(rptr<hsm->num_row()) hsmrptrkp += nh; 00803 } 00804 HepVector p(a->num_row()-k,0); 00805 rptr=k+1; 00806 HepMatrix::mIter pr = p.m.begin(); 00807 int r; 00808 for (r=1;r<=p.num_row();r++) { 00809 HepMatrix::mIter hsmcptrkp = hsm->m.begin() + k * (nh+1) - 1; 00810 int cptr; 00811 for (cptr=k+1;cptr<=rptr;cptr++) { 00812 (*pr)+=a->fast(rptr,cptr)*(*hsmcptrkp); 00813 if(cptr<a->num_col()) hsmcptrkp += nh; 00814 } 00815 for (;cptr<=a->num_col();cptr++) { 00816 (*pr)+=a->fast(cptr,rptr)*(*hsmcptrkp); 00817 if(cptr<a->num_col()) hsmcptrkp += nh; 00818 } 00819 (*pr)*=2/normsq; 00820 rptr++; 00821 pr++; 00822 } 00823 double pdotv=0; 00824 pr = p.m.begin(); 00825 hsmrptrkp = hsm->m.begin() + k * (nh+1) - 1; 00826 for (r=1;r<=p.num_row();r++) { 00827 pdotv+=(*(pr++))*(*hsmrptrkp); 00828 if(r<p.num_row()) hsmrptrkp += nh; 00829 } 00830 pr = p.m.begin(); 00831 hsmrptrkp = hsm->m.begin() + k * (nh+1) - 1; 00832 for (r=1;r<=p.num_row();r++) { 00833 (*(pr++))-=pdotv*(*hsmrptrkp)/normsq; 00834 if(r<p.num_row()) hsmrptrkp += nh; 00835 } 00836 rptr=k+1; 00837 pr = p.m.begin(); 00838 hsmrptrkp = hsm->m.begin() + k * (nh+1) - 1; 00839 for (r=1;r<=p.num_row();r++) { 00840 int cptr=k+1; 00841 HepMatrix::mIter pc = p.m.begin(); 00842 HepMatrix::mIter hsmcptrkp = hsm->m.begin() + k * (nh+1) - 1; 00843 for (int c=1;c<=r;c++) { 00844 a->fast(rptr,cptr)-= (*hsmrptrkp)*(*(pc++))+(*pr)*(*hsmcptrkp); 00845 cptr++; 00846 if(c<r) hsmcptrkp += nh; 00847 } 00848 pr++; 00849 rptr++; 00850 if(r<p.num_row()) hsmrptrkp += nh; 00851 } 00852 } 00853 } 00854 } 00855 00856 HepMatrix tridiagonal(HepSymMatrix *a) 00857 { 00858 HepMatrix U(a->num_row(),a->num_col(),1); 00859 if (a->num_col()>2) 00860 { 00861 HepMatrix hsm(a->num_col(),a->num_col()-2,0); 00862 tridiagonal(a,&hsm); 00863 for (int j=hsm.num_col();j>=1;--j) { 00864 row_house(&U,hsm,j,j,j,j); 00865 } 00866 } 00867 return U; 00868 } 00869 00870 void col_house(HepMatrix *a,const HepMatrix &v,int row,int col, 00871 int row_start,int col_start) 00872 { 00873 double normsq=0; 00874 for (int i=row_start;i<=row_start+a->num_row()-row;i++) 00875 normsq+=v(i,col)*v(i,col); 00876 col_house(a,v,normsq,row,col,row_start,col_start); 00877 } 00878 00879 void givens(double a, double b, double *c, double *s) 00880 { 00881 if (b ==0) { *c=1; *s=0; } 00882 else { 00883 if (fabs(b)>fabs(a)) { 00884 double tau=-a/b; 00885 *s=1/sqrt(1+tau*tau); 00886 *c=(*s)*tau; 00887 } else { 00888 double tau=-b/a; 00889 *c=1/sqrt(1+tau*tau); 00890 *s=(*c)*tau; 00891 } 00892 } 00893 } 00894 00895 void qr_decomp(HepMatrix *A,HepMatrix *hsm) 00896 { 00897 for (int i=1;i<=A->num_col();i++) 00898 house_with_update(A,hsm,i,i); 00899 } 00900 00901 void row_house(HepMatrix *a,const HepMatrix &v,int row,int col, 00902 int row_start,int col_start) 00903 { 00904 double normsq=0; 00905 int end = row_start+a->num_row()-row; 00906 for (int i=row_start; i<=end; i++) 00907 normsq += v(i,col)*v(i,col); 00908 // If v is 0, then we can skip doing row_house. 00909 if (normsq !=0) 00910 row_house(a,v,normsq,row,col,row_start,col_start); 00911 } 00912 00913 } // namespace CLHEP