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