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 00007 #ifdef GNUPRAGMA 00008 #pragma implementation 00009 #endif 00010 00011 #include <string.h> 00012 #include <float.h> // for DBL_EPSILON 00013 #include <cmath> 00014 #include <stdlib.h> 00015 00016 #include "CLHEP/Matrix/defs.h" 00017 #include "CLHEP/Random/Random.h" 00018 #include "CLHEP/Matrix/Matrix.h" 00019 #include "CLHEP/Matrix/SymMatrix.h" 00020 #include "CLHEP/Matrix/DiagMatrix.h" 00021 #include "CLHEP/Matrix/Vector.h" 00022 #include "CLHEP/Utility/thread_local.h" 00023 00024 #ifdef HEP_DEBUG_INLINE 00025 #include "CLHEP/Matrix/Matrix.icc" 00026 #endif 00027 00028 namespace CLHEP { 00029 00030 // Simple operation for all elements 00031 00032 #define SIMPLE_UOP(OPER) \ 00033 mIter a=m.begin(); \ 00034 mIter e=m.end(); \ 00035 for(;a!=e; a++) (*a) OPER t; 00036 00037 #define SIMPLE_BOP(OPER) \ 00038 HepMatrix::mIter a=m.begin(); \ 00039 HepMatrix::mcIter b=hm2.m.begin(); \ 00040 HepMatrix::mIter e=m.end(); \ 00041 for(;a!=e; a++, b++) (*a) OPER (*b); 00042 00043 #define SIMPLE_TOP(OPER) \ 00044 HepMatrix::mcIter a=hm1.m.begin(); \ 00045 HepMatrix::mcIter b=hm2.m.begin(); \ 00046 HepMatrix::mIter t=mret.m.begin(); \ 00047 HepMatrix::mcIter e=hm1.m.end(); \ 00048 for(;a!=e; a++, b++, t++) (*t) = (*a) OPER (*b); 00049 00050 // Static functions. 00051 00052 #define CHK_DIM_2(r1,r2,c1,c2,fun) \ 00053 if (r1!=r2 || c1!=c2) { \ 00054 HepGenMatrix::error("Range error in Matrix function " #fun "(1)."); \ 00055 } 00056 00057 #define CHK_DIM_1(c1,r2,fun) \ 00058 if (c1!=r2) { \ 00059 HepGenMatrix::error("Range error in Matrix function " #fun "(2)."); \ 00060 } 00061 00062 // Constructors. (Default constructors are inlined and in .icc file) 00063 00064 HepMatrix::HepMatrix(int p,int q) 00065 : m(p*q), nrow(p), ncol(q) 00066 { 00067 size_ = nrow * ncol; 00068 } 00069 00070 HepMatrix::HepMatrix(int p,int q,int init) 00071 : m(p*q), nrow(p), ncol(q) 00072 { 00073 size_ = nrow * ncol; 00074 00075 if (size_ > 0) { 00076 switch(init) 00077 { 00078 case 0: 00079 break; 00080 00081 case 1: 00082 { 00083 if ( ncol == nrow ) { 00084 mIter a = m.begin(); 00085 for( int step=0; step < size_; step+=(ncol+1) ) *(a+step) = 1.0; 00086 } else { 00087 error("Invalid dimension in HepMatrix(int,int,1)."); 00088 } 00089 break; 00090 } 00091 default: 00092 error("Matrix: initialization must be either 0 or 1."); 00093 } 00094 } 00095 } 00096 00097 HepMatrix::HepMatrix(int p,int q, HepRandom &r) 00098 : m(p*q), nrow(p), ncol(q) 00099 { 00100 size_ = nrow * ncol; 00101 00102 mIter a = m.begin(); 00103 mIter b = m.end(); 00104 for(; a<b; a++) *a = r(); 00105 } 00106 // 00107 // Destructor 00108 // 00109 HepMatrix::~HepMatrix() { 00110 } 00111 00112 HepMatrix::HepMatrix(const HepMatrix &hm1) 00113 : HepGenMatrix(hm1), m(hm1.size_), nrow(hm1.nrow), ncol(hm1.ncol), size_(hm1.size_) 00114 { 00115 m = hm1.m; 00116 00117 } 00118 00119 // trivial 00120 00121 int HepMatrix::num_row() const { return nrow;} 00122 00123 int HepMatrix::num_col() const { return ncol;} 00124 00125 int HepMatrix::num_size() const { return size_;} 00126 00127 // operator() 00128 00129 double & HepMatrix::operator()(int row, int col) 00130 { 00131 #ifdef MATRIX_BOUND_CHECK 00132 if(row<1 || row>num_row() || col<1 || col>num_col()) 00133 error("Range error in HepMatrix::operator()"); 00134 #endif 00135 return *(m.begin()+(row-1)*ncol+col-1); 00136 } 00137 00138 const double & HepMatrix::operator()(int row, int col) const 00139 { 00140 #ifdef MATRIX_BOUND_CHECK 00141 if(row<1 || row>num_row() || col<1 || col>num_col()) 00142 error("Range error in HepMatrix::operator()"); 00143 #endif 00144 return *(m.begin()+(row-1)*ncol+col-1); 00145 } 00146 00147 00148 HepMatrix::HepMatrix(const HepSymMatrix &hm1) 00149 : m(hm1.nrow*hm1.nrow), nrow(hm1.nrow), ncol(hm1.nrow) 00150 { 00151 size_ = nrow * ncol; 00152 00153 mcIter sjk = hm1.m.begin(); 00154 // j >= k 00155 for(int j=0; j!=nrow; ++j) { 00156 for(int k=0; k<=j; ++k) { 00157 m[j*ncol+k] = *sjk; 00158 // we could copy the diagonal element twice or check 00159 // doing the check may be a tiny bit faster, 00160 // so we choose that option for now 00161 if(k!=j) m[k*nrow+j] = *sjk; 00162 ++sjk; 00163 } 00164 } 00165 } 00166 00167 HepMatrix::HepMatrix(const HepDiagMatrix &hm1) 00168 : m(hm1.nrow*hm1.nrow), nrow(hm1.nrow), ncol(hm1.nrow) 00169 { 00170 size_ = nrow * ncol; 00171 00172 int n = num_row(); 00173 mIter mrr; 00174 mcIter mr = hm1.m.begin(); 00175 for(int r=0;r<n;r++) { 00176 mrr = m.begin()+(n+1)*r; 00177 *mrr = *(mr++); 00178 } 00179 } 00180 00181 HepMatrix::HepMatrix(const HepVector &hm1) 00182 : m(hm1.nrow), nrow(hm1.nrow), ncol(1) 00183 { 00184 00185 size_ = nrow; 00186 m = hm1.m; 00187 } 00188 00189 00190 // 00191 // 00192 // Sub matrix 00193 // 00194 // 00195 00196 HepMatrix HepMatrix::sub(int min_row, int max_row, 00197 int min_col,int max_col) const 00198 #ifdef HEP_GNU_OPTIMIZED_RETURN 00199 return mret(max_row-min_row+1,max_col-min_col+1); 00200 { 00201 #else 00202 { 00203 HepMatrix mret(max_row-min_row+1,max_col-min_col+1); 00204 #endif 00205 if(max_row > num_row() || max_col >num_col()) 00206 error("HepMatrix::sub: Index out of range"); 00207 mIter a = mret.m.begin(); 00208 int nc = num_col(); 00209 mcIter b1 = m.begin() + (min_row - 1) * nc + min_col - 1; 00210 int rowsize = mret.num_row(); 00211 for(int irow=1; irow<=rowsize; ++irow) { 00212 mcIter brc = b1; 00213 for(int icol=0; icol<mret.num_col(); ++icol) { 00214 *(a++) = *(brc++); 00215 } 00216 if(irow<rowsize) b1 += nc; 00217 } 00218 return mret; 00219 } 00220 00221 void HepMatrix::sub(int row,int col,const HepMatrix &hm1) 00222 { 00223 if(row <1 || row+hm1.num_row()-1 > num_row() || 00224 col <1 || col+hm1.num_col()-1 > num_col() ) 00225 error("HepMatrix::sub: Index out of range"); 00226 mcIter a = hm1.m.begin(); 00227 int nc = num_col(); 00228 mIter b1 = m.begin() + (row - 1) * nc + col - 1; 00229 int rowsize = hm1.num_row(); 00230 for(int irow=1; irow<=rowsize; ++irow) { 00231 mIter brc = b1; 00232 for(int icol=0; icol<hm1.num_col(); ++icol) { 00233 *(brc++) = *(a++); 00234 } 00235 if(irow<rowsize) b1 += nc; 00236 } 00237 } 00238 00239 // 00240 // Direct sum of two matricies 00241 // 00242 00243 HepMatrix dsum(const HepMatrix &hm1, const HepMatrix &hm2) 00244 #ifdef HEP_GNU_OPTIMIZED_RETURN 00245 return mret(hm1.num_row() + hm2.num_row(), hm1.num_col() + hm2.num_col(), 00246 0); 00247 { 00248 #else 00249 { 00250 HepMatrix mret(hm1.num_row() + hm2.num_row(), hm1.num_col() + hm2.num_col(), 00251 0); 00252 #endif 00253 mret.sub(1,1,hm1); 00254 mret.sub(hm1.num_row()+1,hm1.num_col()+1,hm2); 00255 return mret; 00256 } 00257 00258 /* ----------------------------------------------------------------------- 00259 This section contains support routines for matrix.h. This section contains 00260 The two argument functions +,-. They call the copy constructor and +=,-=. 00261 ----------------------------------------------------------------------- */ 00262 HepMatrix HepMatrix::operator- () const 00263 #ifdef HEP_GNU_OPTIMIZED_RETURN 00264 return hm2(nrow, ncol); 00265 { 00266 #else 00267 { 00268 HepMatrix hm2(nrow, ncol); 00269 #endif 00270 mcIter a=m.begin(); 00271 mIter b=hm2.m.begin(); 00272 mcIter e=m.end(); 00273 for(;a<e; a++, b++) (*b) = -(*a); 00274 return hm2; 00275 } 00276 00277 00278 00279 HepMatrix operator+(const HepMatrix &hm1,const HepMatrix &hm2) 00280 #ifdef HEP_GNU_OPTIMIZED_RETURN 00281 return mret(hm1.nrow, hm1.ncol); 00282 { 00283 #else 00284 { 00285 HepMatrix mret(hm1.nrow, hm1.ncol); 00286 #endif 00287 CHK_DIM_2(hm1.num_row(),hm2.num_row(), hm1.num_col(),hm2.num_col(),+); 00288 SIMPLE_TOP(+) 00289 return mret; 00290 } 00291 00292 // 00293 // operator - 00294 // 00295 00296 HepMatrix operator-(const HepMatrix &hm1,const HepMatrix &hm2) 00297 #ifdef HEP_GNU_OPTIMIZED_RETURN 00298 return mret(hm1.num_row(), hm1.num_col()); 00299 { 00300 #else 00301 { 00302 HepMatrix mret(hm1.num_row(), hm1.num_col()); 00303 #endif 00304 CHK_DIM_2(hm1.num_row(),hm2.num_row(), 00305 hm1.num_col(),hm2.num_col(),-); 00306 SIMPLE_TOP(-) 00307 return mret; 00308 } 00309 00310 /* ----------------------------------------------------------------------- 00311 This section contains support routines for matrix.h. This file contains 00312 The two argument functions *,/. They call copy constructor and then /=,*=. 00313 ----------------------------------------------------------------------- */ 00314 00315 HepMatrix operator/( 00316 const HepMatrix &hm1,double t) 00317 #ifdef HEP_GNU_OPTIMIZED_RETURN 00318 return mret(hm1); 00319 { 00320 #else 00321 { 00322 HepMatrix mret(hm1); 00323 #endif 00324 mret /= t; 00325 return mret; 00326 } 00327 00328 HepMatrix operator*(const HepMatrix &hm1,double t) 00329 #ifdef HEP_GNU_OPTIMIZED_RETURN 00330 return mret(hm1); 00331 { 00332 #else 00333 { 00334 HepMatrix mret(hm1); 00335 #endif 00336 mret *= t; 00337 return mret; 00338 } 00339 00340 HepMatrix operator*(double t,const HepMatrix &hm1) 00341 #ifdef HEP_GNU_OPTIMIZED_RETURN 00342 return mret(hm1); 00343 { 00344 #else 00345 { 00346 HepMatrix mret(hm1); 00347 #endif 00348 mret *= t; 00349 return mret; 00350 } 00351 00352 HepMatrix operator*(const HepMatrix &hm1,const HepMatrix &hm2) 00353 #ifdef HEP_GNU_OPTIMIZED_RETURN 00354 return mret(hm1.nrow,hm2.ncol,0); 00355 { 00356 #else 00357 { 00358 // initialize matrix to 0.0 00359 HepMatrix mret(hm1.nrow,hm2.ncol,0); 00360 #endif 00361 CHK_DIM_1(hm1.ncol,hm2.nrow,*); 00362 00363 int m1cols = hm1.ncol; 00364 int m2cols = hm2.ncol; 00365 00366 for (int i=0; i<hm1.nrow; i++) 00367 { 00368 for (int j=0; j<m1cols; j++) 00369 { 00370 double temp = hm1.m[i*m1cols+j]; 00371 HepMatrix::mIter pt = mret.m.begin() + i*m2cols; 00372 00373 // Loop over k (the column index in matrix hm2) 00374 HepMatrix::mcIter pb = hm2.m.begin() + m2cols*j; 00375 const HepMatrix::mcIter pblast = pb + m2cols; 00376 while (pb < pblast) 00377 { 00378 (*pt) += temp * (*pb); 00379 pb++; 00380 pt++; 00381 } 00382 } 00383 } 00384 00385 return mret; 00386 } 00387 00388 /* ----------------------------------------------------------------------- 00389 This section contains the assignment and inplace operators =,+=,-=,*=,/=. 00390 ----------------------------------------------------------------------- */ 00391 00392 HepMatrix & HepMatrix::operator+=(const HepMatrix &hm2) 00393 { 00394 CHK_DIM_2(num_row(),hm2.num_row(),num_col(),hm2.num_col(),+=); 00395 SIMPLE_BOP(+=) 00396 return (*this); 00397 } 00398 00399 HepMatrix & HepMatrix::operator-=(const HepMatrix &hm2) 00400 { 00401 CHK_DIM_2(num_row(),hm2.num_row(),num_col(),hm2.num_col(),-=); 00402 SIMPLE_BOP(-=) 00403 return (*this); 00404 } 00405 00406 HepMatrix & HepMatrix::operator/=(double t) 00407 { 00408 SIMPLE_UOP(/=) 00409 return (*this); 00410 } 00411 00412 HepMatrix & HepMatrix::operator*=(double t) 00413 { 00414 SIMPLE_UOP(*=) 00415 return (*this); 00416 } 00417 00418 HepMatrix & HepMatrix::operator=(const HepMatrix &hm1) 00419 { 00420 if(hm1.nrow*hm1.ncol != size_) //??fixme?? hm1.size != size 00421 { 00422 size_ = hm1.nrow * hm1.ncol; 00423 m.resize(size_); //??fixme?? if (size < hm1.size) m.resize(hm1.size); 00424 } 00425 nrow = hm1.nrow; 00426 ncol = hm1.ncol; 00427 m = hm1.m; 00428 return (*this); 00429 } 00430 00431 // HepMatrix & HepMatrix::operator=(const HepRotation &hm2) 00432 // is now in Matrix=Rotation.cc 00433 00434 // Print the Matrix. 00435 00436 std::ostream& operator<<(std::ostream &os, const HepMatrix &q) 00437 { 00438 os << "\n"; 00439 /* Fixed format needs 3 extra characters for field, while scientific needs 7 */ 00440 int width; 00441 if(os.flags() & std::ios::fixed) 00442 width = os.precision()+3; 00443 else 00444 width = os.precision()+7; 00445 for(int irow = 1; irow<= q.num_row(); irow++) 00446 { 00447 for(int icol = 1; icol <= q.num_col(); icol++) 00448 { 00449 os.width(width); 00450 os << q(irow,icol) << " "; 00451 } 00452 os << std::endl; 00453 } 00454 return os; 00455 } 00456 00457 HepMatrix HepMatrix::T() const 00458 #ifdef HEP_GNU_OPTIMIZED_RETURN 00459 return mret(ncol,nrow); 00460 { 00461 #else 00462 { 00463 HepMatrix mret(ncol,nrow); 00464 #endif 00465 mcIter pme = m.begin(); 00466 mIter pt = mret.m.begin(); 00467 for( int nr=0; nr<nrow; ++nr) { 00468 for( int nc=0; nc<ncol; ++nc) { 00469 pt = mret.m.begin() + nr + nrow*nc; 00470 (*pt) = (*pme); 00471 ++pme; 00472 } 00473 } 00474 return mret; 00475 } 00476 00477 HepMatrix HepMatrix::apply(double (*f)(double, int, int)) const 00478 #ifdef HEP_GNU_OPTIMIZED_RETURN 00479 return mret(num_row(),num_col()); 00480 { 00481 #else 00482 { 00483 HepMatrix mret(num_row(),num_col()); 00484 #endif 00485 mcIter a = m.begin(); 00486 mIter b = mret.m.begin(); 00487 for(int ir=1;ir<=num_row();ir++) { 00488 for(int ic=1;ic<=num_col();ic++) { 00489 *(b++) = (*f)(*(a++), ir, ic); 00490 } 00491 } 00492 return mret; 00493 } 00494 00495 int HepMatrix::dfinv_matrix(int *ir) { 00496 if (num_col()!=num_row()) 00497 error("dfinv_matrix: Matrix is not NxN"); 00498 int n = num_col(); 00499 if (n==1) return 0; 00500 00501 double s31, s32; 00502 double s33, s34; 00503 00504 mIter hm11 = m.begin(); 00505 mIter hm12 = hm11 + 1; 00506 mIter hm21 = hm11 + n; 00507 mIter hm22 = hm12 + n; 00508 *hm21 = -(*hm22) * (*hm11) * (*hm21); 00509 *hm12 = -(*hm12); 00510 if (n>2) { 00511 mIter mimim = hm11 + n + 1; 00512 for (int i=3;i<=n;i++) { 00513 // calculate these to avoid pointing off the end of the storage array 00514 mIter mi = hm11 + (i-1) * n; 00515 mIter mii= hm11 + (i-1) * n + i - 1; 00516 int ihm2 = i - 2; 00517 mIter mj = hm11; 00518 mIter mji = mj + i - 1; 00519 mIter mij = mi; 00520 for (int j=1;j<=ihm2;j++) { 00521 s31 = 0.0; 00522 s32 = *mji; 00523 mIter mkj = mj + j - 1; 00524 mIter mik = mi + j - 1; 00525 mIter mjkp = mj + j; 00526 mIter mkpi = mj + n + i - 1; 00527 for (int k=j;k<=ihm2;k++) { 00528 s31 += (*mkj) * (*(mik++)); 00529 s32 += (*(mjkp++)) * (*mkpi); 00530 mkj += n; 00531 mkpi += n; 00532 } // for k 00533 *mij = -(*mii) * (((*(mij-n)))*( (*(mii-1)))+(s31)); 00534 *mji = -s32; 00535 mj += n; 00536 mji += n; 00537 mij++; 00538 } // for j 00539 *(mii-1) = -(*mii) * (*mimim) * (*(mii-1)); 00540 *(mimim+1) = -(*(mimim+1)); 00541 mimim += (n+1); 00542 } // for i 00543 } // n>2 00544 mIter mi = hm11; 00545 mIter mii = hm11; 00546 for (int i=1;i<n;i++) { 00547 int ni = n - i; 00548 mIter mij = mi; 00549 int j; 00550 for (j=1; j<=i;j++) { 00551 s33 = *mij; 00552 // change initial definition of mikj to avoid pointing off the end of the storage array 00553 mIter mikj = mi + j - 1; 00554 mIter miik = mii + 1; 00555 mIter min_end = mi + n; 00556 for (;miik<min_end;) { 00557 // iterate by n as we enter the loop to avoid pointing off the end of the storage array 00558 mikj += n; 00559 s33 += (*mikj) * (*(miik++)); 00560 } 00561 *(mij++) = s33; 00562 } 00563 for (j=1;j<=ni;j++) { 00564 s34 = 0.0; 00565 mIter miik = mii + j; 00566 for (int k=j;k<=ni;k++) { 00567 // calculate mikij here to avoid pointing off the end of the storage array 00568 mIter mikij = mii + k * n + j; 00569 s34 += *mikij * (*(miik++)); 00570 } 00571 *(mii+j) = s34; 00572 } 00573 mi += n; 00574 mii += (n+1); 00575 } // for i 00576 int nxch = ir[n]; 00577 if (nxch==0) return 0; 00578 for (int hmm=1;hmm<=nxch;hmm++) { 00579 int k = nxch - hmm + 1; 00580 int ij = ir[k]; 00581 int i = ij >> 12; 00582 int j = ij%4096; 00583 for (k=1; k<=n;k++) { 00584 // avoid setting the iterator beyond the end of the storage vector 00585 mIter mki = hm11 + (k-1)*n + i - 1; 00586 mIter mkj = hm11 + (k-1)*n + j - 1; 00587 // 2/24/05 David Sachs fix of improper swap bug that was present 00588 // for many years: 00589 double ti = *mki; // 2/24/05 00590 *mki = *mkj; 00591 *mkj = ti; // 2/24/05 00592 } 00593 } // for hmm 00594 return 0; 00595 } 00596 00597 int HepMatrix::dfact_matrix(double &det, int *ir) { 00598 if (ncol!=nrow) 00599 error("dfact_matrix: Matrix is not NxN"); 00600 00601 int ifail, jfail; 00602 int n = ncol; 00603 00604 double tf; 00605 double g1 = 1.0e-19, g2 = 1.0e19; 00606 00607 double p, q, t; 00608 double s11, s12; 00609 00610 double epsilon = 8*DBL_EPSILON; 00611 // could be set to zero (like it was before) 00612 // but then the algorithm often doesn't detect 00613 // that a matrix is singular 00614 00615 int normal = 0, imposs = -1; 00616 int jrange = 0, jover = 1, junder = -1; 00617 ifail = normal; 00618 jfail = jrange; 00619 int nxch = 0; 00620 det = 1.0; 00621 mIter mj = m.begin(); 00622 mIter mjj = mj; 00623 for (int j=1;j<=n;j++) { 00624 int k = j; 00625 p = (fabs(*mjj)); 00626 if (j!=n) { 00627 // replace mij with calculation of position 00628 for (int i=j+1;i<n;i++) { 00629 q = (fabs(*(mj + n*(i-j) + j - 1))); 00630 if (q > p) { 00631 k = i; 00632 p = q; 00633 } 00634 } // for i 00635 if (k==j) { 00636 if (p <= epsilon) { 00637 det = 0; 00638 ifail = imposs; 00639 jfail = jrange; 00640 return ifail; 00641 } 00642 det = -det; // in this case the sign of the determinant 00643 // must not change. So I change it twice. 00644 } // k==j 00645 mIter mjl = mj; 00646 mIter mkl = m.begin() + (k-1)*n; 00647 for (int l=1;l<=n;l++) { 00648 tf = *mjl; 00649 *(mjl++) = *mkl; 00650 *(mkl++) = tf; 00651 } 00652 nxch = nxch + 1; // this makes the determinant change its sign 00653 ir[nxch] = (((j)<<12)+(k)); 00654 } else { // j!=n 00655 if (p <= epsilon) { 00656 det = 0.0; 00657 ifail = imposs; 00658 jfail = jrange; 00659 return ifail; 00660 } 00661 } // j!=n 00662 det *= *mjj; 00663 *mjj = 1.0 / *mjj; 00664 t = (fabs(det)); 00665 if (t < g1) { 00666 det = 0.0; 00667 if (jfail == jrange) jfail = junder; 00668 } else if (t > g2) { 00669 det = 1.0; 00670 if (jfail==jrange) jfail = jover; 00671 } 00672 // calculate mk and mkjp so we don't point off the end of the vector 00673 if (j!=n) { 00674 mIter mjk = mj + j; 00675 for (k=j+1;k<=n;k++) { 00676 mIter mk = mj + n*(k-j); 00677 mIter mkjp = mk + j; 00678 s11 = - (*mjk); 00679 s12 = - (*mkjp); 00680 if (j!=1) { 00681 mIter mik = m.begin() + k - 1; 00682 mIter mijp = m.begin() + j; 00683 mIter mki = mk; 00684 mIter mji = mj; 00685 for (int i=1;i<j;i++) { 00686 s11 += (*mik) * (*(mji++)); 00687 s12 += (*mijp) * (*(mki++)); 00688 mik += n; 00689 mijp += n; 00690 } // for i 00691 } // j!=1 00692 *(mjk++) = -s11 * (*mjj); 00693 *(mkjp) = -(((*(mjj+1)))*((*(mkjp-1)))+(s12)); 00694 } // for k 00695 } // j!=n 00696 // avoid setting the iterator beyond the end of the vector 00697 if(j!=n) { 00698 mj += n; 00699 mjj += (n+1); 00700 } 00701 } // for j 00702 if (nxch%2==1) det = -det; 00703 if (jfail !=jrange) det = 0.0; 00704 ir[n] = nxch; 00705 return 0; 00706 } 00707 00708 void HepMatrix::invert(int &ierr) { 00709 if(ncol != nrow) 00710 error("HepMatrix::invert: Matrix is not NxN"); 00711 00712 static CLHEP_THREAD_LOCAL int max_array = 20; 00713 static CLHEP_THREAD_LOCAL int *ir = new int [max_array+1]; 00714 00715 if (ncol > max_array) { 00716 delete [] ir; 00717 max_array = nrow; 00718 ir = new int [max_array+1]; 00719 } 00720 double t1, t2, t3; 00721 double det, temp, sd; 00722 int ifail; 00723 switch(nrow) { 00724 case 3: 00725 double c11,c12,c13,c21,c22,c23,c31,c32,c33; 00726 ifail = 0; 00727 c11 = (*(m.begin()+4)) * (*(m.begin()+8)) - (*(m.begin()+5)) * (*(m.begin()+7)); 00728 c12 = (*(m.begin()+5)) * (*(m.begin()+6)) - (*(m.begin()+3)) * (*(m.begin()+8)); 00729 c13 = (*(m.begin()+3)) * (*(m.begin()+7)) - (*(m.begin()+4)) * (*(m.begin()+6)); 00730 c21 = (*(m.begin()+7)) * (*(m.begin()+2)) - (*(m.begin()+8)) * (*(m.begin()+1)); 00731 c22 = (*(m.begin()+8)) * (*m.begin()) - (*(m.begin()+6)) * (*(m.begin()+2)); 00732 c23 = (*(m.begin()+6)) * (*(m.begin()+1)) - (*(m.begin()+7)) * (*m.begin()); 00733 c31 = (*(m.begin()+1)) * (*(m.begin()+5)) - (*(m.begin()+2)) * (*(m.begin()+4)); 00734 c32 = (*(m.begin()+2)) * (*(m.begin()+3)) - (*m.begin()) * (*(m.begin()+5)); 00735 c33 = (*m.begin()) * (*(m.begin()+4)) - (*(m.begin()+1)) * (*(m.begin()+3)); 00736 t1 = fabs(*m.begin()); 00737 t2 = fabs(*(m.begin()+3)); 00738 t3 = fabs(*(m.begin()+6)); 00739 if (t1 >= t2) { 00740 if (t3 >= t1) { 00741 temp = *(m.begin()+6); 00742 det = c23*c12-c22*c13; 00743 } else { 00744 temp = *(m.begin()); 00745 det = c22*c33-c23*c32; 00746 } 00747 } else if (t3 >= t2) { 00748 temp = *(m.begin()+6); 00749 det = c23*c12-c22*c13; 00750 } else { 00751 temp = *(m.begin()+3); 00752 det = c13*c32-c12*c33; 00753 } 00754 if (det==0) { 00755 ierr = 1; 00756 return; 00757 } 00758 { 00759 double s1 = temp/det; 00760 mIter hmm = m.begin(); 00761 *(hmm++) = s1*c11; 00762 *(hmm++) = s1*c21; 00763 *(hmm++) = s1*c31; 00764 *(hmm++) = s1*c12; 00765 *(hmm++) = s1*c22; 00766 *(hmm++) = s1*c32; 00767 *(hmm++) = s1*c13; 00768 *(hmm++) = s1*c23; 00769 *(hmm) = s1*c33; 00770 } 00771 break; 00772 case 2: 00773 ifail = 0; 00774 det = (*m.begin())*(*(m.begin()+3)) - (*(m.begin()+1))*(*(m.begin()+2)); 00775 if (det==0) { 00776 ierr = 1; 00777 return; 00778 } 00779 sd = 1.0/det; 00780 temp = sd*(*(m.begin()+3)); 00781 *(m.begin()+1) *= -sd; 00782 *(m.begin()+2) *= -sd; 00783 *(m.begin()+3) = sd*(*m.begin()); 00784 *(m.begin()) = temp; 00785 break; 00786 case 1: 00787 ifail = 0; 00788 if ((*(m.begin()))==0) { 00789 ierr = 1; 00790 return; 00791 } 00792 *(m.begin()) = 1.0/(*(m.begin())); 00793 break; 00794 case 4: 00795 invertHaywood4(ierr); 00796 return; 00797 case 5: 00798 invertHaywood5(ierr); 00799 return; 00800 case 6: 00801 invertHaywood6(ierr); 00802 return; 00803 default: 00804 ifail = dfact_matrix(det, ir); 00805 if(ifail) { 00806 ierr = 1; 00807 return; 00808 } 00809 dfinv_matrix(ir); 00810 break; 00811 } 00812 ierr = 0; 00813 return; 00814 } 00815 00816 double HepMatrix::determinant() const { 00817 static CLHEP_THREAD_LOCAL int max_array = 20; 00818 static CLHEP_THREAD_LOCAL int *ir = new int [max_array+1]; 00819 if(ncol != nrow) 00820 error("HepMatrix::determinant: Matrix is not NxN"); 00821 if (ncol > max_array) { 00822 delete [] ir; 00823 max_array = nrow; 00824 ir = new int [max_array+1]; 00825 } 00826 double det; 00827 HepMatrix mt(*this); 00828 int i = mt.dfact_matrix(det, ir); 00829 if(i==0) return det; 00830 return 0; 00831 } 00832 00833 double HepMatrix::trace() const { 00834 double t = 0.0; 00835 for (mcIter d = m.begin(); d < m.end(); d += (ncol+1) ) 00836 t += *d; 00837 return t; 00838 } 00839 00840 } // namespace CLHEP