14#include "CLHEP/Matrix/defs.h"
15#include "CLHEP/Random/Random.h"
16#include "CLHEP/Matrix/SymMatrix.h"
17#include "CLHEP/Matrix/Matrix.h"
18#include "CLHEP/Matrix/DiagMatrix.h"
19#include "CLHEP/Matrix/Vector.h"
21#ifdef HEP_DEBUG_INLINE
22#include "CLHEP/Matrix/SymMatrix.icc"
29#define SIMPLE_UOP(OPER) \
30 HepMatrix::mIter a=m.begin(); \
31 HepMatrix::mIter e=m.begin()+num_size(); \
32 for(;a<e; a++) (*a) OPER t;
34#define SIMPLE_BOP(OPER) \
35 HepMatrix::mIter a=m.begin(); \
36 HepMatrix::mcIter b=hm2.m.begin(); \
37 HepMatrix::mcIter e=m.begin()+num_size(); \
38 for(;a<e; a++, b++) (*a) OPER (*b);
40#define SIMPLE_TOP(OPER) \
41 HepMatrix::mcIter a=hm1.m.begin(); \
42 HepMatrix::mcIter b=hm2.m.begin(); \
43 HepMatrix::mIter t=mret.m.begin(); \
44 HepMatrix::mcIter e=hm1.m.begin()+hm1.num_size(); \
45 for( ;a<e; a++, b++, t++) (*t) = (*a) OPER (*b);
47#define CHK_DIM_2(r1,r2,c1,c2,fun) \
48 if (r1!=r2 || c1!=c2) { \
49 HepGenMatrix::error("Range error in SymMatrix function " #fun "(1)."); \
52#define CHK_DIM_1(c1,r2,fun) \
54 HepGenMatrix::error("Range error in SymMatrix function " #fun "(2)."); \
60 : m(p*(p+1)/2), nrow(p)
62 size_ = nrow * (nrow+1) / 2;
67 : m(p*(p+1)/2), nrow(p)
69 size_ = nrow * (nrow+1) / 2;
80 for(
int i=0;i<nrow;++i) {
81 a = m.begin() + (i+1)*i/2 + i;
87 error(
"SymMatrix: initialization must be either 0 or 1.");
92 : m(p*(p+1)/2), nrow(p)
94 size_ = nrow * (nrow+1) / 2;
97 for(;
a<
b;
a++) *
a = r();
107 :
HepGenMatrix(hm1), m(hm1.size_), nrow(hm1.nrow), size_(hm1.size_)
113 : m(hm1.nrow*(hm1.nrow+1)/2), nrow(hm1.nrow)
115 size_ = nrow * (nrow+1) / 2;
122 for(
int r=1;r<=
n;r++) {
124 if(r<
n) mrr += (r+1);
135#ifdef HEP_GNU_OPTIMIZED_RETURN
136return mret(max_row-min_row+1);
142 if(max_row > num_row())
143 error(
"HepSymMatrix::sub: Index out of range");
146 int rowsize=mret.num_row();
147 for(
int irow=1; irow<=rowsize; irow++) {
149 for(
int icol=0; icol<irow; ++icol) {
152 if(irow<rowsize) b1 += irow+min_row-1;
161 error(
"HepSymMatrix::sub: Index out of range");
165 for(
int irow=1; irow<=rowsize; irow++) {
167 for(
int icol=0; icol<irow; ++icol) {
170 if(irow<rowsize) b1 += irow+min_row-1;
178 error(
"HepSymMatrix::sub: Index out of range");
182 for(
int irow=1; irow<=rowsize; ++irow) {
184 for(
int icol=0; icol<irow; ++icol) {
187 if(irow<rowsize) b1 += irow+row-1;
197#ifdef HEP_GNU_OPTIMIZED_RETURN
215#ifdef HEP_GNU_OPTIMIZED_RETURN
225 for(;
a<e;
a++,
b++) (*
b) = -(*a);
232#ifdef HEP_GNU_OPTIMIZED_RETURN
244#ifdef HEP_GNU_OPTIMIZED_RETURN
257#ifdef HEP_GNU_OPTIMIZED_RETURN
258 return mret(hm1.nrow);
274#ifdef HEP_GNU_OPTIMIZED_RETURN
282 hm1.num_col(),hm2.
num_col(),-);
287#ifdef HEP_GNU_OPTIMIZED_RETURN
301#ifdef HEP_GNU_OPTIMIZED_RETURN
320#ifdef HEP_GNU_OPTIMIZED_RETURN
332#ifdef HEP_GNU_OPTIMIZED_RETURN
344#ifdef HEP_GNU_OPTIMIZED_RETURN
357#ifdef HEP_GNU_OPTIMIZED_RETURN
368 for(mit1=hm1.m.begin();
373 for(
int step=1;step<=hm2.
num_row();++step)
380 temp+=*(sp++)*(*(mit2++));
383 for(
int stept=step+1;stept<=hm2.
num_row();stept++)
385 temp+=*sp*(*(mit2++));
386 if(stept<hm2.
num_row()) sp+=stept;
396#ifdef HEP_GNU_OPTIMIZED_RETURN
408 for(step=1,snp=hm1.m.begin();step<=hm1.
num_row();snp+=step++)
409 for(mit1=hm2.m.begin();mit1<hm2.m.begin()+hm2.
num_col();mit1++)
416 temp+=*mit2*(*(sp++));
423 for(stept=step+1;stept<=hm1.
num_row();stept++)
438#ifdef HEP_GNU_OPTIMIZED_RETURN
446 int step1,stept1,step2,stept2;
451 for(step1=1;step1<=hm1.
num_row();++step1) {
453 for(step2=1;step2<=hm2.
num_row();++step2)
461 while(sp1<snp1+step1) {
462 temp+=(*(sp1++))*(*(sp2++));
465 for(stept1=step1+1;stept1!=step2+1;++stept1) {
466 temp+=(*sp1)*(*(sp2++));
467 if(stept1<hm2.
num_row()) sp1+=stept1;
471 for(stept2=step2+1;stept2<=hm2.
num_row();stept1++,stept2++) {
483 temp+=(*(sp1++))*(*(sp2++));
487 for(stept2=step2+1;stept2!=step1+1;stept2++) {
488 temp+=(*(sp1++))*(*sp2);
489 if(stept2<hm1.
num_row()) sp2+=stept2;
493 for(stept1=step1+1;stept1<=hm1.
num_row();stept1++,stept2++) {
505 if(step1<hm1.
num_row()) snp1+=step1;
511#ifdef HEP_GNU_OPTIMIZED_RETURN
523 for(step=1,snp=hm1.m.begin();step<=hm1.
num_row();++step)
530 temp+=*(sp++)*(*(vpt++));
531 if(step<hm1.
num_row()) sp+=step-1;
532 for(stept=step+1;stept<=hm1.
num_row();stept++)
534 temp+=*sp*(*(vpt++));
535 if(stept<hm1.
num_row()) sp+=stept;
543#ifdef HEP_GNU_OPTIMIZED_RETURN
552 for(vt1=v.m.begin();vt1<v.m.begin()+v.
num_row();vt1++)
553 for(vt2=v.m.begin();vt2<=vt1;vt2++)
554 *(mr++)=(*vt1)*(*vt2);
567 for(
int j=0; j!=nrow; ++j) {
568 for(
int k=0; k<=j; ++k) {
571 if(k!=j) m[k*nrow+j] += *sjk;
590 for(
int j=0; j!=nrow; ++j) {
591 for(
int k=0; k<=j; ++k) {
594 if(k!=j) m[k*nrow+j] -= *sjk;
623 nrow = ncol = hm1.nrow;
624 if(nrow*ncol != size_)
630 mcIter sjk = hm1.m.begin();
632 for(
int j=0; j!=nrow; ++j) {
633 for(
int k=0; k<=j; ++k) {
638 if(k!=j) m[k*nrow+j] = *sjk;
662 size_ = nrow * (nrow+1) / 2;
669 for(
int r=1; r<=nrow; r++) {
671 if(r<nrow) mrr += (r+1);
683 if(os.flags() & std::ios::fixed)
684 width = os.precision()+3;
686 width = os.precision()+7;
687 for(
int irow = 1; irow<= q.
num_row(); irow++)
689 for(
int icol = 1; icol <= q.
num_col(); icol++)
692 os << q(irow,icol) <<
" ";
700apply(
double (*
f)(
double,
int,
int))
const
701#ifdef HEP_GNU_OPTIMIZED_RETURN
702return mret(num_row());
710 for(
int ir=1;ir<=num_row();ir++) {
711 for(
int ic=1;ic<=ir;ic++) {
712 *(
b++) = (*
f)(*(
a++), ir, ic);
723 size_ = nrow * (nrow+1) / 2;
728 for(
int r=1;r<=nrow;r++) {
730 for(
int c=1;c<=r;c++) {
733 if(r<nrow)
a += nrow;
738#ifdef HEP_GNU_OPTIMIZED_RETURN
751 for(
int r=1;r<=mret.num_row();r++) {
753 for(
int c=1;c<=r;c++) {
754 register double tmp = 0.0;
757 for(
int i=1;i<=hm1.
num_col();i++) {
758 tmp+=(*(tempri++))*(*(hm1ci++));
769#ifdef HEP_GNU_OPTIMIZED_RETURN
780 for(
int r=1;r<=mret.num_row();r++) {
784 register double tmp = 0.0;
789 tmp+=(*(tempri++))*(*(hm1ci++));
791 for(i=c;i<=hm1.
num_col();i++) {
792 tmp+=(*(tempri++))*(*(hm1ci));
793 if(i<hm1.
num_col()) hm1ci += i;
805 register double mret = 0.0;
812 for(;
a<e;) mret += (*(
a++)) * (*(
b++));
817#ifdef HEP_GNU_OPTIMIZED_RETURN
825 int n = hm1.num_col();
828 for(
int r=1;r<=mret.num_row();r++) {
830 for(
int c=1;c<=r;c++) {
831 register double tmp = 0.0;
832 for(
int i=1;i<=hm1.num_row();i++) {
835 tmp+=(*(tempir))*(*(hm1ic));
854 double c11,c12,c13,c22,c23,c33;
855 c11 = (*(m.begin()+2)) * (*(m.begin()+5)) - (*(m.begin()+4)) * (*(m.begin()+4));
856 c12 = (*(m.begin()+4)) * (*(m.begin()+3)) - (*(m.begin()+1)) * (*(m.begin()+5));
857 c13 = (*(m.begin()+1)) * (*(m.begin()+4)) - (*(m.begin()+2)) * (*(m.begin()+3));
858 c22 = (*(m.begin()+5)) * (*m.begin()) - (*(m.begin()+3)) * (*(m.begin()+3));
859 c23 = (*(m.begin()+3)) * (*(m.begin()+1)) - (*(m.begin()+4)) * (*m.begin());
860 c33 = (*m.begin()) * (*(m.begin()+2)) - (*(m.begin()+1)) * (*(m.begin()+1));
861 t1 = fabs(*m.begin());
862 t2 = fabs(*(m.begin()+1));
863 t3 = fabs(*(m.begin()+3));
866 temp = *(m.begin()+3);
867 det = c23*c12-c22*c13;
870 det = c22*c33-c23*c23;
872 }
else if (t3 >= t2) {
873 temp = *(m.begin()+3);
874 det = c23*c12-c22*c13;
876 temp = *(m.begin()+1);
877 det = c13*c23-c12*c33;
884 double ds = temp/det;
897 double det, temp, ds;
898 det = (*m.begin())*(*(m.begin()+2)) - (*(m.begin()+1))*(*(m.begin()+1));
904 *(m.begin()+1) *= -ds;
905 temp = ds*(*(m.begin()+2));
906 *(m.begin()+2) = ds*(*m.begin());
912 if ((*m.begin())==0) {
916 *m.begin() = 1.0/(*m.begin());
944 static const int max_array = 20;
946 static std::vector<int> ir_vec (max_array+1);
947 if (ir_vec.size() <=
static_cast<unsigned int>(nrow)) ir_vec.resize(nrow+1);
948 int * ir = &ir_vec[0];
952 int i = mt.dfact_matrix(det, ir);
959 for (
int i=0; i<nrow; i++)
960 t += *(m.begin() + (i+3)*i/2);
982 static const int max_array = 25;
984 static std::vector<double> xvec (max_array);
985 static std::vector<int> pivv (max_array);
986 typedef std::vector<int>::iterator pivIter;
988 static std::vector<double,Alloc<double,25> > xvec (max_array);
989 static std::vector<int, Alloc<int, 25> > pivv (max_array);
990 typedef std::vector<int,Alloc<int,25> >::iterator pivIter;
992 if (xvec.size() <
static_cast<unsigned int>(nrow)) xvec.resize(nrow);
993 if (pivv.size() <
static_cast<unsigned int>(nrow)) pivv.resize(nrow);
998 mIter x = xvec.begin();
1000 pivIter piv = pivv.begin();
1003 double temp1, temp2;
1005 double lambda, sigma;
1006 const double alpha = .6404;
1007 const double epsilon = 32*DBL_EPSILON;
1013 for (i = 0; i < nrow; ++i) piv[i] = i+1;
1021 for (j=1; j < nrow; j+=is)
1023 mjj = m.begin() + j*(j-1)/2 + j-1;
1027 for (i=j+1; i <= nrow ; ++i) {
1029 ip = m.begin() + (i-1)*i/2 + j-1;
1030 if (fabs(*ip) > lambda) {
1043 if (fabs(*mjj) >= lambda*alpha) {
1048 ip = m.begin() + pivrow*(pivrow-1)/2+j-1;
1049 for (k=j; k < pivrow; k++) {
1050 if (fabs(*ip) > sigma) sigma = fabs(*ip);
1053 if (sigma * fabs(*mjj) >= alpha * lambda * lambda) {
1056 }
else if (fabs(*(m.begin()+pivrow*(pivrow-1)/2+pivrow-1))
1069 temp2 = *mjj = 1./ *mjj;
1072 for (i=j+1; i <= nrow; i++) {
1073 temp1 = *(m.begin() + i*(i-1)/2 + j-1) * temp2;
1074 ip = m.begin()+i*(i-1)/2+j;
1075 for (k=j+1; k<=i; k++) {
1076 *ip -= temp1 * *(m.begin() + k*(k-1)/2 + j-1);
1077 if (fabs(*ip) <= epsilon)
1084 for (i=j+1; i <= nrow; ++i) {
1086 ip = m.begin() + (i-1)*i/2 + j-1;
1094 ip = m.begin() + pivrow*(pivrow-1)/2 + j;
1095 for (i=j+1; i < pivrow; i++, ip++) {
1096 temp1 = *(m.begin() + i*(i-1)/2 + j-1);
1097 *(m.begin() + i*(i-1)/2 + j-1)= *ip;
1101 *mjj = *(m.begin()+pivrow*(pivrow-1)/2+pivrow-1);
1102 *(m.begin()+pivrow*(pivrow-1)/2+pivrow-1) = temp1;
1103 ip = m.begin() + (pivrow+1)*pivrow/2 + j-1;
1105 for (i = pivrow+1; i <= nrow; ip += i, iq += i++) {
1115 temp2 = *mjj = 1./ *mjj;
1118 for (i = j+1; i <= nrow; i++) {
1119 temp1 = *(m.begin() + i*(i-1)/2 + j-1) * temp2;
1120 ip = m.begin()+i*(i-1)/2+j;
1121 for (k=j+1; k<=i; k++) {
1122 *ip -= temp1 * *(m.begin() + k*(k-1)/2 + j-1);
1123 if (fabs(*ip) <= epsilon)
1130 for (i=j+1; i <= nrow; ++i) {
1132 ip = m.begin() + (i-1)*i/2 + j-1;
1139 if (j+1 != pivrow) {
1142 ip = m.begin() + pivrow*(pivrow-1)/2 + j+1;
1143 for (i=j+2; i < pivrow; i++, ip++) {
1144 temp1 = *(m.begin() + i*(i-1)/2 + j);
1145 *(m.begin() + i*(i-1)/2 + j) = *ip;
1148 temp1 = *(mjj + j + 1);
1150 *(m.begin() + pivrow*(pivrow-1)/2 + pivrow-1);
1151 *(m.begin() + pivrow*(pivrow-1)/2 + pivrow-1) = temp1;
1153 *(mjj + j) = *(m.begin() + pivrow*(pivrow-1)/2 + j-1);
1154 *(m.begin() + pivrow*(pivrow-1)/2 + j-1) = temp1;
1155 ip = m.begin() + (pivrow+1)*pivrow/2 + j;
1156 iq = ip + pivrow-(j+1);
1157 for (i = pivrow+1; i <= nrow; ip += i, iq += i++) {
1164 temp2 = *mjj * *(mjj + j + 1) - *(mjj + j) * *(mjj + j);
1167 <<
"SymMatrix::bunch_invert: error in pivot choice"
1174 *mjj = *(mjj + j + 1) * temp2;
1175 *(mjj + j + 1) = temp1 * temp2;
1176 *(mjj + j) = - *(mjj + j) * temp2;
1180 for (i=j+2; i <= nrow ; i++) {
1181 ip = m.begin() + i*(i-1)/2 + j-1;
1182 temp1 = *ip * *mjj + *(ip + 1) * *(mjj + j);
1183 if (fabs(temp1 ) <= epsilon) temp1 = 0;
1184 temp2 = *ip * *(mjj + j) + *(ip + 1) * *(mjj + j + 1);
1185 if (fabs(temp2 ) <= epsilon) temp2 = 0;
1186 for (k = j+2; k <= i ; k++) {
1187 ip = m.begin() + i*(i-1)/2 + k-1;
1188 iq = m.begin() + k*(k-1)/2 + j-1;
1189 *ip -= temp1 * *iq + temp2 * *(iq+1);
1190 if (fabs(*ip) <= epsilon)
1195 for (i=j+2; i <= nrow ; i++) {
1196 ip = m.begin() + i*(i-1)/2 + j-1;
1197 temp1 = *ip * *mjj + *(ip+1) * *(mjj + j);
1198 if (fabs(temp1) <= epsilon) temp1 = 0;
1199 *(ip+1) = *ip * *(mjj + j)
1200 + *(ip+1) * *(mjj + j + 1);
1201 if (fabs(*(ip+1)) <= epsilon) *(ip+1) = 0;
1210 mjj = m.begin() + j*(j-1)/2 + j-1;
1214 }
else { *mjj = 1. / *mjj; }
1219 for (j = nrow ; j >= 1 ; j -= is)
1221 mjj = m.begin() + j*(j-1)/2 + j-1;
1227 ip = m.begin() + (j+1)*j/2 - 1;
1228 for (i=0; i < nrow-j; ++i) {
1232 for (i=j+1; i<=nrow ; i++) {
1234 ip = m.begin() + i*(i-1)/2 + j;
1235 for (k=0; k <= i-j-1; k++) temp2 += *ip++ * x[k];
1239 for ( ; k < nrow-j; ++k) {
1241 temp2 += *ip * x[k];
1243 *(m.begin()+ i*(i-1)/2 + j-1) = -temp2;
1249 ip = m.begin() + (j+1)*j/2 - 1;
1250 for (k=0; k < nrow-j; ++k) {
1252 temp2 += x[k] * *ip;
1258 std::cerr <<
"error in piv" << piv[j-1] << std::endl;
1263 ip = m.begin() + (j+1)*j/2 - 1;
1264 for (i=0; i < nrow-j; ++i) {
1268 for (i=j+1; i<=nrow ; i++) {
1270 ip = m.begin() + i*(i-1)/2 + j;
1271 for (k=0; k <= i-j-1; k++)
1272 temp2 += *ip++ * x[k];
1273 for (ip += i-1; k < nrow-j; ip += 1+j+k++)
1274 temp2 += *ip * x[k];
1275 *(m.begin()+ i*(i-1)/2 + j-1) = -temp2;
1280 ip = m.begin() + (j+1)*j/2 - 1;
1281 for (k=0; k < nrow-j; ++k) {
1283 temp2 += x[k] * *ip;
1289 ip = m.begin() + (j+1)*j/2 - 2;
1290 for (i=j+1; i <= nrow; ++i) {
1292 temp2 += *ip * *(ip+1);
1297 ip = m.begin() + (j+1)*j/2 - 2;
1298 for (i=0; i < nrow-j; ++i) {
1302 for (i=j+1; i <= nrow ; i++) {
1304 ip = m.begin() + i*(i-1)/2 + j;
1305 for (k=0; k <= i-j-1; k++)
1306 temp2 += *ip++ * x[k];
1307 for (ip += i-1; k < nrow-j; ip += 1+j+k++)
1308 temp2 += *ip * x[k];
1309 *(m.begin()+ i*(i-1)/2 + j-2)= -temp2;
1315 ip = m.begin() + (j+1)*j/2 - 2;
1316 for (k=0; k < nrow-j; ++k) {
1318 temp2 += x[k] * *ip;
1327 pivrow = (piv[j-1]==0)? -piv[j-2] : piv[j-1];
1328 ip = m.begin() + pivrow*(pivrow-1)/2 + j;
1329 for (i=j+1;i < pivrow; i++, ip++) {
1330 temp1 = *(m.begin() + i*(i-1)/2 + j-1);
1331 *(m.begin() + i*(i-1)/2 + j-1) = *ip;
1335 *mjj = *(m.begin() + pivrow*(pivrow-1)/2 + pivrow-1);
1336 *(m.begin() + pivrow*(pivrow-1)/2 + pivrow-1) = temp1;
1339 *(mjj-1) = *( m.begin() + pivrow*(pivrow-1)/2 + j-2);
1340 *( m.begin() + pivrow*(pivrow-1)/2 + j-2) = temp1;
1344 if( pivrow < nrow ) {
1345 ip = m.begin() + (pivrow+1)*pivrow/2 + j-1;
1347 iq = ip + (pivrow-j);
1348 for (i = pivrow+1; i <= nrow; i++) {
#define CHK_DIM_2(r1, r2, c1, c2, fun)
#define CHK_DIM_1(c1, r2, fun)
std::vector< double, Alloc< double, 25 > >::const_iterator mcIter
std::vector< double, Alloc< double, 25 > >::iterator mIter
static void error(const char *s)
HepMatrix & operator=(const HepMatrix &)
virtual int num_col() const
virtual int num_row() const
HepMatrix & operator+=(const HepMatrix &)
HepMatrix & operator-=(const HepMatrix &)
virtual int num_size() const
HepSymMatrix & operator*=(double t)
double determinant() const
HepSymMatrix & operator+=(const HepSymMatrix &hm2)
void assign(const HepMatrix &hm2)
HepSymMatrix apply(double(*f)(double, int, int)) const
HepSymMatrix similarityT(const HepMatrix &hm1) const
HepSymMatrix & operator=(const HepSymMatrix &hm2)
HepSymMatrix sub(int min_row, int max_row) const
HepSymMatrix operator-() const
HepSymMatrix & operator-=(const HepSymMatrix &hm2)
HepSymMatrix similarity(const HepMatrix &hm1) const
void invertBunchKaufman(int &ifail)
HepSymMatrix & operator/=(double t)
virtual int num_row() const
HepSymMatrix vT_times_v(const HepVector &v)
Hep3Vector operator-(const Hep3Vector &, const Hep3Vector &)
Hep3Vector operator+(const Hep3Vector &, const Hep3Vector &)
HepLorentzRotation operator*(const HepRotation &r, const HepLorentzRotation <)
std::ostream & operator<<(std::ostream &os, const HepAxisAngle &aa)
HepDiagMatrix dsum(const HepDiagMatrix &s1, const HepDiagMatrix &s2)