00001 #include "xTCL.h"
00002 #include "TArrayI.h"
00003 #include <cassert>
00004
00005
00006
00007
00008
00009 double xTCL::vmaxa(const double *a,int na)
00010 {
00011 double r=0;
00012 for (int i=0;i<na;i++){if (r < TMath::Abs(a[i])) r = TMath::Abs(a[i]);}
00013 return r;
00014 }
00015
00016 double xTCL::vmaxa(const TVectorD &a)
00017 {
00018 return vmaxa(a.GetMatrixArray(),a.GetNrows());
00019 }
00020
00021 int xTCL::lvmaxa(const double *a,int na)
00022 {
00023 double r=0;
00024 int l=0;
00025 for (int i=0;i<na;i++){if (r < TMath::Abs(a[i])) {r = TMath::Abs(a[i]);l=i;}}
00026 return l;
00027 }
00028
00029 int xTCL::lvmina(const double *a,int na)
00030 {
00031 double r=TMath::Abs(a[0]);
00032 int l=0;
00033 for (int i=1;i<na;i++){if (r > TMath::Abs(a[i])) {r = TMath::Abs(a[i]);l=i;}}
00034 return l;
00035 }
00036
00037 void xTCL::vfill(double *a,double f,int na)
00038 {
00039 for (int i=0;i<na;i++) {a[i]=f;}
00040 }
00041
00042 void xTCL::eigen2(const double err[3], double lam[2], double eig[2][2])
00043 {
00044
00045 double spur = err[0]+err[2];
00046 double det = err[0]*err[2]-err[1]*err[1];
00047 double dis = spur*spur-4*det;
00048 if (dis<0) dis = 0;
00049 dis = TMath::Sqrt(dis);
00050 lam[0] = 0.5*(spur+dis);
00051 lam[1] = 0.5*(spur-dis);
00052 eig[0][0] = 1; eig[0][1]=0;
00053 if (dis>1e-6*spur) {
00054 if (TMath::Abs(err[0]-lam[0])>TMath::Abs(err[2]-lam[0])) {
00055 eig[0][1] = 1; eig[0][0]= -err[1]/(err[0]-lam[0]);
00056 } else {
00057 eig[0][0] = 1; eig[0][1]= -err[1]/(err[2]-lam[0]);
00058 }
00059 double tmp = TMath::Sqrt(eig[0][0]*eig[0][0]+eig[0][1]*eig[0][1]);
00060 eig[0][0]/=tmp; eig[0][1]/=tmp;
00061 }
00062 eig[1][0]=-eig[0][1]; eig[1][1]= eig[0][0];
00063 }
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084 double xTCL::simpson(const double *F,double A,double B,int NP)
00085 {
00086 int N2=NP-1;
00087 assert(N2>0 && !(N2&1));
00088 double S1=F[N2-1];
00089 double S2=0;
00090
00091 for (int N = 1;N<=N2-3;N+=2) {S1+=F[N];S2+=F[N+1];}
00092 S1=S1+S1+S2;
00093 double H=(F[0]+F[N2]+S1+S1)*(B-A)/(3*N2);
00094 return H;
00095 }
00096
00097 double **xTCL::makeMatrixD(int m,int n)
00098 {
00099 int szp = (m*sizeof(double*)+sizeof(double))/sizeof(double);
00100 int szd =m*n;
00101 double *d = new double[szp+szd];
00102 memset(d,0,(szp+szd)*sizeof(double));
00103 double **p = (double**)d;
00104 d+=szp;
00105 for (int i=0;i<m;i++) { p[i]=d; d+=n;};
00106 return p;
00107 }
00108
00109 void xTCL::mxmlrt(const TMatrixD &A,const TMatrixD &B,TMatrixD &X)
00110 {
00111 int nRowA = A.GetNrows();
00112 int nColA = A.GetNcols();
00113 int nRowB = B.GetNrows();
00114 int nColB = B.GetNcols(); if(nColB){}
00115 assert(nColA ==nRowB);
00116 X.ResizeTo(nRowA,nRowA);
00117 TCL::mxmlrt(A.GetMatrixArray(),B.GetMatrixArray()
00118 ,X.GetMatrixArray(),nRowA,nColA);
00119
00120 }
00121
00122 void xTCL::mxmlrtS(const double *A,const double *B,double *X,int nra,int nca)
00123 {
00124 TCL::vzero(X,nra*nra);
00125 for (int i=0,ii=0;i<nra;i++,ii+=nca) {
00126 for (int j=0,jj=0;j<nca;j++,jj+=nca) {
00127 if(!A[ii+j]) continue;
00128 for (int k=0,kk=0;k<=i;k++,kk+=nca) {
00129 double &Xik =X[i*nra+k];
00130 for (int l=0;l<nca;l++) {
00131 if(!A[kk+l]) continue;
00132 Xik +=A[ii+j]*A[kk+l]*B[jj+l];
00133 } } } }
00134 for (int i=0;i<nra;i++){
00135 for (int k=0;k<i ;k++){X[k*nra+i] = X[i*nra+k];}}
00136 }
00137
00138 void xTCL::mxmlrtS(const TMatrixD &A,const TMatrixD &B,TMatrixD &X)
00139 {
00140 int nRowA = A.GetNrows();
00141 int nColA = A.GetNcols();
00142 int nRowB = B.GetNrows();
00143 int nColB = B.GetNcols(); if(nColB){}
00144 assert(nColA ==nRowB);
00145 X.ResizeTo(nRowA,nRowA);
00146 xTCL::mxmlrtS(A.GetMatrixArray(),B.GetMatrixArray()
00147 ,X.GetMatrixArray(),nRowA,nColA);
00148
00149 }
00150
00151 TMatrixD xTCL::T(const TMatrixD &mx)
00152 {
00153 TMatrixD tmp(mx);
00154 return tmp.T();
00155 }
00156
00157 double xTCL::vasum(const double *a, int na)
00158 {
00159 double sum = 0;
00160 for (int i=0;i<na;i++) { sum += TMath::Abs(a[i]);}
00161 return sum;
00162 }
00163
00164 double xTCL::vasum(const TVectorD &a)
00165 {
00166 return vasum(a.GetMatrixArray(),a.GetNrows());
00167 }
00168
00169 int xTCL::SqProgSimple( TVectorD &x
00170 ,const TVectorD &g,const TMatrixD &G
00171 ,const TVectorD &Min
00172 ,const TVectorD &Max, int iAktp)
00173 {
00174 static const double kSMALL = 1e-9;
00175 static int nCall=0; nCall++;
00176 enum {kINIT=0,kADDCONS,kFREECONS};
00177 int kase = kINIT;
00178 int nPars = g.GetNrows();
00179 TVectorD xx(x),gg(g),add(nPars);
00180 TArrayI Side(nPars);
00181 int nCons=0,addCons = -1,freCons=-1,freSide=0,addSide=0,con=0;
00182 double maxGra=3e33;
00183 int iAkt = iAktp;
00184 while(1946) {
00185
00186 freCons=-1; freSide=0;
00187 if (nCons && kase==kFREECONS ) {
00188 double tryGra=kSMALL; freCons=-1;
00189 for (int ix=0;ix<nPars;ix++) {
00190 if(!Side[ix]) continue;
00191 double gra = gg[ix]*Side[ix];
00192 if (gra< tryGra) continue;
00193 if (gra>=maxGra) continue;
00194 freCons=ix; tryGra=gra;
00195 }
00196 if (freCons>=0) {
00197 maxGra = tryGra;
00198 freSide = Side[freCons];
00199 Side[freCons]=0;
00200 nCons--;
00201 }
00202 }
00203
00204 if(kase==kFREECONS && freCons<0) { break;}
00205
00206
00207 TMatrixD S(G);
00208 TVectorD B(gg);
00209 if (nCons) {
00210 for (int ix=0;ix<nPars;ix++) {
00211 if (Side[ix]==0) continue;
00212 for (int jx=0;jx<nPars;jx++) {S[ix][jx]=0; S[jx][ix]=0;}
00213 S[ix][ix]=1; B[ix]=0;
00214 } }
00215 if (iAkt==0 ) {
00216
00217 double det=12345; S.Invert(&det);
00218
00219
00220 add = (-1.)*(S*B);
00221 double along = B*add;
00222 if (along>0) add*=(-1.);
00223 }
00224
00225 if (iAkt==1 ) {
00226 double bb = (B*B);
00227 double bSb = (B*(S*B));
00228 double tau = -bb/(TMath::Abs(bSb)+3e-33);
00229 add = tau*B;
00230
00231 }
00232 if(kase==kFREECONS && freSide) {
00233 if (add[freCons]*freSide > -kSMALL) {
00234 Side[freCons]=freSide; nCons++; continue;}
00235 }
00236
00237
00238 double fak=1;
00239 addCons = -1; addSide = 0;
00240 con = 0;
00241 for (int ix=0;ix<nPars;ix++) {
00242 if (Side[ix]) {add[ix]=0; con = 100*con+ix+1;continue;}
00243 double xi = xx[ix]+fak*add[ix];
00244 if (xi < Min[ix]){fak = (Min[ix]-xx[ix])/add[ix]; addCons=ix;addSide=-1;}
00245 if (xi > Max[ix]){fak = (Max[ix]-xx[ix])/add[ix]; addCons=ix;addSide= 1;}
00246 assert(fak<=1. && fak>=0.);
00247 }
00248 add*=fak;
00249 xx+= add;
00250 gg += G*add;
00251 maxGra=3e33;
00252 kase = kFREECONS; if (!addSide) continue;
00253 kase = kADDCONS;
00254 xx[addCons] = (addSide<0)? Min[addCons]:Max[addCons];
00255
00256 Side[addCons] = addSide ;nCons++;
00257
00258 }
00259 x = xx;
00260 return abs(con);
00261 }
00262
00263 void xTCL::toEuler(const double TT[3][3],double PhiThePsi[6])
00264 {
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276 double cThe = TT[2][2]; if (cThe>1) cThe=1; if (cThe<-1) cThe=-1;
00277 double sThe = (TT[0][2]*TT[0][2]+TT[1][2]*TT[1][2])
00278 + (TT[2][0]*TT[2][0]+TT[2][1]*TT[2][1]);
00279 sThe = sqrt(0.5*sThe);
00280
00281 double N = 0.5*(cThe*cThe+sThe*sThe+1);
00282 cThe/=N; sThe/=N;
00283
00284 double sPsi = 0,cPsi=1;
00285 if (sThe>1e-6) {
00286 sPsi = TT[0][2]/sThe; cPsi = TT[1][2]/sThe;
00287 N = 0.5*(cPsi*cPsi+sPsi*sPsi+1);
00288 cPsi/=N; sPsi/=N;
00289 }
00290 double cPhi = cPsi*TT[0][0]-sPsi*TT[1][0];
00291 double sPhi = cPsi*TT[0][1]-sPsi*TT[1][1];
00292
00293 PhiThePsi[0] = cPhi ; PhiThePsi[1] = sPhi;
00294 PhiThePsi[2] = cThe ; PhiThePsi[3] = sThe;
00295 PhiThePsi[4] = cPsi ; PhiThePsi[5] = sPsi;
00296
00297
00298 double test = fabs(TT[0][0] -( cPsi*cPhi - sPsi*cThe*sPhi))
00299 + fabs(TT[1][0] -(-sPsi*cPhi - cPsi*cThe*sPhi))
00300 + fabs(TT[2][0] -( sThe*sPhi))
00301 + fabs(TT[0][1] -( cPsi*sPhi + sPsi*cThe*cPhi))
00302 + fabs(TT[1][1] -(-sPsi*sPhi + cPsi*cThe*cPhi))
00303 + fabs(TT[2][1] -(-sThe*cPhi))
00304 + fabs(TT[0][2] -( sPsi*sThe))
00305 + fabs(TT[1][2] -( cPsi*sThe))
00306 + fabs(TT[2][2] -( cThe));
00307 printf("EPS=%g\n",test);
00308
00309 }
