StRoot: Sti/StiTrackNode.cxx Source File

00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include "StiTrackNode.h"
00004 #include "TMath.h"
00005 #include "TRMatrix.h"
00006 #include "TRVector.h"
00007 #include "StMessMgr.h"
00008 int StiTrackNode::mgFlag=0;
00009   static const int idx66[6][6] =
00010   {{ 0, 1, 3, 6,10,15},{ 1, 2, 4, 7,11,16},{ 3, 4, 5, 8,12,17}
00011   ,{ 6, 7, 8, 9,13,18},{10,11,12,13,14,19},{15,16,17,18,19,20}};
00012 static const double MAX1ERR[]={3,3,3,0.1,3.,0.1};
00013 static const double MAX2ERR[]={MAX1ERR[0]*MAX1ERR[0]
00014                               ,MAX1ERR[1]*MAX1ERR[1]
00015                               ,MAX1ERR[2]*MAX1ERR[2]
00016                               ,MAX1ERR[3]*MAX1ERR[3]
00017                               ,MAX1ERR[4]*MAX1ERR[4]
00018                               ,MAX1ERR[5]*MAX1ERR[5]};
00019 
00020 static const double MIN1ERR[]={1e-5,1e-5,1e-5,1e-7,0,1e-7};
00021 static const double MIN2ERR[]={MIN1ERR[0]*MIN1ERR[0]
00022                               ,MIN1ERR[1]*MIN1ERR[1]
00023                               ,MIN1ERR[2]*MIN1ERR[2]
00024                               ,MIN1ERR[3]*MIN1ERR[3]
00025                               ,MIN1ERR[4]*MIN1ERR[4]
00026                               ,MIN1ERR[5]*MIN1ERR[5]};
00027 static const double recvCORRMAX  = 0.99;
00028 static const double chekCORRMAX  = 0.9999;
00029 static double MAXPARS[]={250,250,250,1.5,100,100};
00030 
00031 //______________________________________________________________________________
00032 void StiTrackNode::errPropag6( double G[21],const double F[6][6],int nF )
00033 {
00034   enum {NP=6,NE=21};
00035 
00036   double g[NE];      memcpy(g,    G,sizeof( g));
00037   double fg[NP][NP]; memset(fg[0],0,sizeof(fg));
00038 //   double myF[6][6];  memcpy(myF[0],F[0],sizeof(fg));
00039 //   for (int i=0;i<NP;i++) {myF[i][i]+=1.;}
00040 //   double myG[NE];
00041 //   TCL::trasat(myF[0],G,myG,NP,NP);
00042 //#define TEST_errPropag6
00043 #ifdef TEST_errPropag6
00044   TRSymMatrix rG(nF,G);    cout << "rG\t" << rG << endl;
00045   TRMatrix    rU(TRArray::kUnit,nF);
00046   TRMatrix    rF(nF,nF,&F[0][0]); 
00047   rF += rU;   cout << "rF\t" << rF << endl;
00048   TRSymMatrix rFGFT(rF,TRArray::kAxSxAT,rG); cout << "rFGFT\t" << rFGFT << endl;
00049 #endif
00050   for (int i=0;i<nF;i++) {
00051   for (int j=0;j<nF;j++) {
00052     if (!F[i][j])       continue;
00053     for (int k=0;k<NP;k++) {
00054       int jk = idx66[j][k];
00055       if (!g[jk])       continue;
00056       fg[i][k] += F[i][j]*g[jk]; 
00057   }}}
00058 
00059   for (int i=0;i<NP;i++) {        
00060   for (int k=i;k<NP;k++) {        
00061     int ik = idx66[i][k];
00062     double s = 0; 
00063     for (int j=0;j<NP;j++) {
00064       if (!F[k][j])     continue;
00065       s += fg[i][j]*F[k][j];
00066     }
00067     G[ik] += (s + fg[i][k] + fg[k][i]);
00068   }}
00069 //  for (int i=0;i<NE;i++) {assert(fabs(G[i]-myG[i]) <= 1e-6*fabs(G[i]+myG[i]));}
00070 #ifdef TEST_errPropag6
00071   TRSymMatrix rGnew(nF,G); cout << "rGnew\t" << rGnew << endl;
00072 #undef TEST_errPropag6
00073 #endif
00074 
00075 }       
00076 
00077 //______________________________________________________________________________
00078 void StiHitContino::reset()
00079 {
00080    memset(this,0,sizeof(StiHitContino));
00081    mChi2[0]=1e61;
00082 }
00083 //______________________________________________________________________________
00084 void StiHitContino::add(StiHit *hit,double chi2,double detr)
00085 {
00086    int i=0;
00087    for (;i<kMaxSize;i++) {
00088      if (!mHits[i])             break;
00089      if (chi2 > mChi2[i])       continue;
00090      for (int jr = kMaxSize-2;jr>=i;jr--) 
00091        {mHits[jr+1]=mHits[jr]; mChi2[jr+1]=mChi2[jr];mDetr[jr+1]=mDetr[jr];}
00092                                 break;
00093    }
00094    if (i>=kMaxSize)             return;
00095    mHits[i]=hit; mChi2[i]=chi2;mDetr[i]=detr;
00096 }
00097 //______________________________________________________________________________
00098 void StiHitContino::print(const char* tit) const
00099 {
00100   if (!tit || !*tit) tit ="print()";
00101   int n=getNHits();     
00102   LOG_DEBUG << Form(" ****      StiHitContino::%s nHits=%d",tit,n)<< endm;
00103   for (int i=0;i<n;i++) {
00104     LOG_DEBUG << Form("%3d - hit=%p chi2 = %g",i,(void*)mHits[i],mChi2[i]);}
00105     LOG_DEBUG << endm;
00106 }
00107 //______________________________________________________________________________
00108 int StiHitContino::getNHits() const
00109 { int n=0; for(int i=0;i<kMaxSize;i++) {if (mHits[i]) n++;}; return n;} 
00110 //______________________________________________________________________________
00111 
00112 #include "TComplex.h"
00113 const TComplex Im(0,1);
00114 //______________________________________________________________________________
00115 int StiTrackNode::cylCross(double r, const double dx[4],double Rho,double out[4])
00116 {
00117 //  dx[0] == cosCA; dx[1] == sinCA;dx[2] == _x; dx[3]==_y
00118 
00119   TComplex d(dx[0],dx[1]),n(-dx[1],dx[0]),x(dx[2],dx[3]);
00120   TComplex res[2];
00121   TComplex xd = x/d;
00122   double R2 = x.Re()*x.Re()+x.Im()*x.Im();
00123 
00124   if (R2*Rho*Rho< 1e-4) {// Low curvature approx
00125     double a = (1.+xd.Im()*Rho);
00126     double b = xd.Re();
00127     double c = (R2-r*r);
00128     double dis = b*b - a*c;
00129     if (dis<0.)                 return 1;
00130     dis = sqrt(dis);
00131     double L[2];
00132     if (b<0) {a=-a;b=-b;c=-c;}
00133     L[0] = -c/(dis+b);
00134     L[1] = -(b+(dis))/a;
00135     res[0] = x+L[0]*(1.+Im*(0.5*L[0]*Rho))*d;
00136     res[1] = x+L[1]*(1.+Im*(0.5*L[1]*Rho))*d;
00137 
00138   } else {      //General case
00139 
00140     TComplex Q = Rho*x+n;
00141     double a1Q = ((x.Re()*n.Re()+x.Im()*n.Im())*2 + R2*Rho);
00142     double aQ = a1Q*Rho+1;
00143     double q = TComplex::Log(Q).Im();
00144  //       cos(Al-q) = ((Rho*r)**2 +Q**2-1)/(2*Q*r*Rho)
00145     double mycos = (r*r*Rho+a1Q)/(2*aQ*r);
00146     if (fabs(mycos)>1)          return 1;
00147     double ang = acos(mycos);
00148     res[0] = r*TComplex::Exp(Im*(q+ang));
00149     res[1] = r*TComplex::Exp(Im*(q-ang));
00150   }
00151   if (res[0].Re() < res[1].Re()) {//swap
00152     xd = res[0]; res[0]=res[1]; res[1]=xd;}
00153   out[0] = res[0].Re();
00154   out[1] = res[0].Im();
00155   out[2] = res[1].Re();
00156   out[3] = res[1].Im();
00157   return 0;
00158 }
00159 
00160 //______________________________________________________________________________
00161  
00162 double StiTrackNode::sinX(double x)
00163 {
00164   double x2 = x*x;
00165   if (x2>0.5) return (sin(x)-x)/x2/x;
00166   double nom = -1./6;
00167   double sum = nom;
00168   for (int it=4;1;it+=2) {
00169     nom = -nom*x2/(it*(it+1));
00170     sum +=nom;
00171     if (fabs(nom) <= 1e-10*fabs(sum)) break;
00172   }
00173   return sum;
00174 } 
00175 //______________________________________________________________________________
00176 void StiTrackNode::mult6(double Rot[kNPars][kNPars],const double Pro[kNPars][kNPars]) 
00177 {
00178   double T[kNPars][kNPars];
00179 
00180   if (!Rot[0][0]) {memcpy(Rot[0],Pro[0],sizeof(T)); return;}
00181 
00182   memcpy(T[0],Pro[0],sizeof(T));
00183 
00184   for (int i=0;i<kNPars;i++) {
00185   for (int j=0;j<kNPars;j++) {
00186     if(!Rot[i][j]) continue;
00187     for (int k=0;k<kNPars;k++) {
00188       if (!Pro[k][i]) continue;
00189       T[k][j] += Pro[k][i]*Rot[i][j];
00190   }}} 
00191   for (int i=0;i<kNPars;i++) {
00192   for (int k=0;k<kNPars;k++) {
00193     Rot[i][k] += T[i][k];
00194 }}
00195 }     
00196 //______________________________________________________________________________
00197 double StiTrackNode::getRefPosition() const
00198 {
00199   if(!_detector) {
00200     return x();
00201   } else {
00202     StiPlacement * place = _detector->getPlacement();
00203     assert(place);
00204     return place->getLayerRadius();
00205   }
00206 }
00207 //______________________________________________________________________________
00208   double StiTrackNode::getLayerAngle()  const
00209 {
00210   assert(_detector);
00211   StiPlacement * place = _detector->getPlacement();
00212   assert(place);
00213   return place->getLayerAngle();
00214 }
00215 
00216 //______________________________________________________________________________
00217 double StiNodeErrs::operator()(int i,int j) const
00218 {
00219   return A[idx66[i][j]];
00220 }
00221 //______________________________________________________________________________
00222 StiNodeErrs &StiNodeErrs::merge(double wt,StiNodeErrs &other)
00223 {
00224    double wt0 = 1.-wt;
00225    for (int i=0;i<kNErrs;i++) {A[i] = wt0*A[i] + wt*other.A[i];}
00226    return *this;
00227 }
00228 //______________________________________________________________________________
00229 void StiNodeErrs::get00(      double *a) const
00230 {
00231    memcpy(a,A,6*sizeof(double));
00232 }
00233 //______________________________________________________________________________
00234 void StiNodeErrs::set00(const double *a) 
00235 {
00236    memcpy(A,a,6*sizeof(double));
00237 }
00238 //______________________________________________________________________________
00239 void StiNodeErrs::get10(double *a) const
00240 {
00241 // 0: 00
00242 // 1: 10 11 
00243 // 3: 20 21 22
00244 // 6: 30 31 32 33
00245 //10: 40 41 42 43 44
00246 //15: 50 51 52 53 54 55
00247   memcpy(a+0,A+ 6,3*sizeof(double));
00248   memcpy(a+3,A+10,3*sizeof(double));
00249   memcpy(a+6,A+15,3*sizeof(double));
00250 }
00251 //______________________________________________________________________________
00252 void StiNodeErrs::set10(const double *a) 
00253 {
00254   memcpy(A+ 6,a+0,3*sizeof(double));
00255   memcpy(A+10,a+3,3*sizeof(double));
00256   memcpy(A+15,a+6,3*sizeof(double));
00257 }
00258 //______________________________________________________________________________
00259 void StiNodeErrs::get11(      double *a) const
00260 {
00261   memcpy(a+0,A+ 9,1*sizeof(double));
00262   memcpy(a+1,A+13,2*sizeof(double));
00263   memcpy(a+3,A+18,3*sizeof(double));
00264 }
00265 //______________________________________________________________________________
00266 void StiNodeErrs::set11(const double *a) 
00267 {
00268   memcpy(A+ 9,a+0,1*sizeof(double));
00269   memcpy(A+13,a+1,2*sizeof(double));
00270   memcpy(A+18,a+3,3*sizeof(double));
00271 }
00272 //______________________________________________________________________________
00273 void StiNodeErrs::zeroX() 
00274 { 
00275   for (int i=0;i<kNPars;i++) {A[idx66[i][0]]=0;}
00276 }
00277 //______________________________________________________________________________
00278 void StiNodeErrs::recov() 
00279 {
00280 static int PRINT_IT=0;
00281 
00282   int i0=0; if (!_cXX) i0 = 1;
00283   for (int i=i0;i<kNPars;i++) {
00284     double maxDia = MAX2ERR[i];
00285     double minDia = MIN2ERR[i];
00286     int ld = idx66[i][i];
00287     if (A[ld]<minDia) {
00288 if(PRINT_IT){
00289       LOG_DEBUG << Form("StiNodeErrs::cut. Negative diagonal %g(%d)",A[ld],i)<<endm;
00290       print();
00291 }
00292        A[ld] = minDia;
00293     }
00294     if (A[ld]>maxDia) {
00295 if(PRINT_IT) {
00296       LOG_DEBUG << Form("StiNodeErrs::cut. Too big  diagonal %g(%d)",A[ld],i)<<endm;
00297       print();
00298 }
00299        for (int j=0;j<kNPars;j++){ A[idx66[i][j]]=0;}
00300        A[ld]=maxDia;
00301     }
00302   }
00303   for (int i=i0;i<kNPars;i++) {
00304     double &aii = A[idx66[i][i]];
00305     for (int j=i+1;j<kNPars;j++) {
00306       double &ajj = A[idx66[j][j]];
00307       double &aij = A[idx66[i][j]];
00308       if (aij*aij <= aii*ajj*recvCORRMAX) continue;
00309       if (aij*aij > aii*ajj){
00310         LOG_DEBUG << Form("StiNodeErrs::recov : Correlation too big %g[%d][%d]>%g"
00311               ,aij,i,j,sqrt(aii*ajj))<< endm;}    
00312       double ab = sqrt(aii*ajj);
00313       double t2 = (fabs(aij)/ab-recvCORRMAX)/(1+recvCORRMAX);
00314       aii += aii*t2; ajj += ajj*t2;
00315       if (aij<0) t2 = -t2; aij -= ab*t2;
00316       
00317     }//end j
00318   }//end i
00319 
00320 }
00321 //______________________________________________________________________________
00322 void StiNodeErrs::print() const
00323 {
00324    const double *d = A;
00325    for (int n=1;n<=6;n++) {
00326      LOG_DEBUG << Form("%d - ",n);
00327      for (int i=0;i<n;i++){LOG_DEBUG << Form("%g\t",*(d++));}; LOG_DEBUG << endm;
00328    }  
00329 }     
00330 
00331 //______________________________________________________________________________
00332 int StiNodeErrs::check(const char *pri) const
00333 {
00334   int i=-2008,j=2009,kase=0;
00335   double aii=-20091005,ajj=-20101005,aij=-20111005;
00336   int i0=0; if (!_cXX) i0 = 1;
00337   for (i=i0;i<kNPars;i++) {
00338     aii = A[idx66[i][i]];
00339     if (aii<MIN2ERR[i]) {kase = 1; break;}
00340   }
00341   if (kase) goto RETN;
00342   for (i=i0;i<kNPars;i++) {
00343     aii = A[idx66[i][i]];
00344     for (j=i+1;j<kNPars ;j++) {
00345       ajj = A[idx66[j][j]];
00346       if (ajj<=0) continue;
00347       aij = A[idx66[i][j]];
00348       if ((aij*aij)> chekCORRMAX*aii*ajj) {kase = 2; break;}
00349     }
00350     if (kase) break;
00351   }  
00352 RETN:
00353 //  if (!kase && sign()<0 ) kase = 3;  //It is rather slow. For debug only
00354   if ((kase == 3) && (!pri || !pri[0])) pri = "QWERTY";
00355 
00356   if (!kase)    return kase;
00357   if (!pri )    return kase;
00358   switch(kase) {
00359   
00360     case 1: LOG_DEBUG << Form("StiNodeErrs::check(%s) FAILED: Negative diagonal %g[%d][%d]",pri,aii,i,i)<< endm;  
00361           break;
00362     case 2: LOG_DEBUG << Form("StiNodeErrs::check(%s) FAILED: Correlation too big %g[%d][%d]>%g"
00363                 ,pri,aij,i,j,sqrt(aii*ajj))<<endm;        
00364           break;
00365     case 3: LOG_DEBUG << Form("StiNodeErrs::check(%s) FAILED: Non Positive matrix",pri)<<endm;  
00366   }    
00367   return kase;
00368 }  
00369 //____________________________________________________________
00370 double StiNodeErrs::sign() const
00371 {
00372    enum {n=kNPars};
00373    double ans=3e33;
00374    const double *a = A;
00375    double *xx = (double *)A;
00376    double save = *xx; if (!save) *xx = 1;
00377    double B[kNErrs];
00378          double *b = B;
00379    // trchlu.F -- translated by f2c (version 19970219).
00380    //
00381    //see original documentation of CERNLIB package F112 
00382 
00383    /* Local variables */
00384    int ipiv, kpiv, i__, j;
00385    double r__, dc;
00386    int id, kd;
00387    double sum;
00388 
00389 
00390    /* CERN PROGLIB# F112    TRCHLU          .VERSION KERNFOR  4.16  870601 */
00391    /* ORIG. 18/12/74 W.HART */
00392 
00393 
00394    /* Parameter adjuTments */
00395    --b;    --a;
00396 
00397    /* Function Body */
00398    ipiv = 0;
00399 
00400    i__ = 0;
00401 
00402    do {
00403       ++i__;
00404       ipiv += i__;
00405       kpiv = ipiv;
00406       r__ = a[ipiv];
00407 
00408       for (j = i__; j <= n; ++j) {
00409          sum = 0.;
00410          if (i__ == 1)       goto L40;
00411          if (r__ == 0.)      goto L42;
00412          id = ipiv - i__ + 1;
00413          kd = kpiv - i__ + 1;
00414 
00415          do {
00416             sum += b[kd] * b[id];
00417             ++kd;   ++id;
00418          } while (id < ipiv);
00419 
00420 L40:
00421          sum = a[kpiv] - sum;
00422 L42:
00423          if (j != i__) b[kpiv] = sum * r__;
00424          else {
00425             if (sum<ans) ans = sum;
00426             if (sum<0.) goto RETN;
00427             dc = sqrt(sum);
00428             b[kpiv] = dc;
00429             if (r__ > 0.)  r__ = (double)1. / dc;
00430          }
00431          kpiv += j;
00432       }
00433 
00434    } while  (i__ < n);
00435 
00436 RETN: *xx=save; 
00437    return ans;
00438 } /* trchlu_ */
00439 
00440 
00441 //______________________________________________________________________________
00442 int StiNodePars::check(const char *pri) const
00443 {
00444 
00445   int ierr=0;
00446 //?? temp test
00447   double tmp = (fabs(curv())<1e-6)? 0: curv()-ptin()*hz();
00448 //              1km for 1GeV is a zero field
00449 //  assert(fabs(_hz)<1e-5 || fabs(tmp)<= 1e-3*fabs(_curv));
00450   if (fabs(hz())>=1e-5 && fabs(tmp)> 1e-3*fabs(curv()))    {ierr=1313; goto FAILED;}
00451   for (int i=0;i<kNPars;i++) {if (fabs(P[i]) > MAXPARS[i]) {ierr = i+1 ; break;}} 
00452   if(ierr) goto FAILED;
00453   for (int i=-2;i<0;i++)     {if (fabs(P[i]) > 1.)         {ierr = i+12; break;}} 
00454 FAILED: 
00455   if (!ierr) return ierr;
00456   if (!pri ) return ierr;
00457   LOG_DEBUG << Form("StiNodePars::check(%s) == FAILED(%d)",pri,ierr)<<endm;
00458   print();
00459   return ierr;
00460 } 
00461 //______________________________________________________________________________
00462 StiNodePars &StiNodePars::merge(double wt,StiNodePars &other)
00463 {
00464    double wt0 = 1.-wt;
00465    for (int i=0;i<kNPars+1;i++) {P[i] = wt0*P[i] + wt*other.P[i];}
00466    ready();
00467    return *this;
00468 }
00469 //______________________________________________________________________________
00470 void StiNodePars::print() const
00471 {
00472 static const char* tit[]={"cosCA","sinCA","X","Y","Z","Eta","Ptin","TanL","Curv",0};
00473   for (int i=-2;i<kNPars+1;i++) {LOG_DEBUG << Form("%s = %g, ",tit[i+2],P[i]);}
00474   LOG_DEBUG << endm;
00475 }   
00476 //______________________________________________________________________________
00477 void StiHitErrs::rotate(double angle)
00478 {
00479   double t[2][2];
00480   t[0][0] = cos(angle); t[0][1] = -sin(angle);
00481   t[1][0] = -t[0][1]  ; t[1][1] = t[0][0];
00482   double r[3];
00483   TCL::trasat(t[0],&hXX,r,2,2);
00484   TCL::ucopy(r,&hXX,3);
00485 }
00486 //______________________________________________________________________________
00487 void StiNode2Pars::set(const StiNodePars &pars,StiNodeErrs &errs)
00488 {
00489   mPar[0]=pars.y(); mPar[1]=pars.z();
00490   mErr[0]=errs._cYY;
00491   mErr[1]=errs._cZY;
00492   mErr[2]=errs._cZZ;
00493 }
00494