StRoot: Sti/StiTrackNodeHelper.cxx Source File

00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include "StiUtilities/StiDebug.h"
00004 #include "StiTrackNodeHelper.h"
00005 #include "StiElossCalculator.h"
00006 #include "StDetectorDbMaker/StiHitErrorCalculator.h"
00007 #include "StMessMgr.h"
00008 #include "TArrayD.h"
00009 #if ROOT_VERSION_CODE < 331013
00010 #include "TCL.h"
00011 #else
00012 #include "TCernLib.h"
00013 #endif
00014 
00015 #define NICE(a) ( ((a) <= -M_PI)? ((a)+2*M_PI) :\
00016                   ((a) >   M_PI)? ((a)-2*M_PI) : (a))
00017 
00018 #define sinX(a) StiTrackNode::sinX(a)
00019 static const double kMaxEta = 1.5;
00020 static const double kMaxCur = 0.2;
00021 
00022 static const double DY=0.9,DZ=0.9,DEta=0.1,DPti=3,DTan=0.1;
00023 static const double MAXSTEP[]={0,DY,DZ,DEta,DPti,DTan};
00024 static const double ERROR_FACTOR = 2.;
00025 int StiTrackNodeHelper::_debug = 0;
00026 int StiTrackNodeHelper::mgCutStep=0;
00027 //______________________________________________________________________________
00028 int errTest(StiNodePars &predP,StiNodeErrs &predE,
00029             const StiHit *hit,StiHitErrs &hitErr,
00030             StiNodePars &fitdP,StiNodeErrs &fitdE,double chi2);
00031 
00032 //______________________________________________________________________________
00033 void StiTrackNodeHelper::set(double chi2Max,double chi2Vtx,double errConfidence,int iter)
00034 {
00035   reset();
00036   mChi2Max = chi2Max;
00037   mChi2Vtx = chi2Vtx;
00038   mNodeErrFactor = 10;
00039   mHitsErrFactor = 1.;
00040   if (errConfidence>0.1) {
00041     mNodeErrFactor = (1./(errConfidence));
00042     mHitsErrFactor = (1./(1. - errConfidence));
00043   }
00044   mIter = iter;
00045   if (!mIter) mFlipFlopNode = 0;
00046 }
00047 //______________________________________________________________________________
00048 void StiTrackNodeHelper::reset()
00049 { 
00050   memset(mBeg,'A',mEnd-mBeg+1);
00051   mWorstNode =0;
00052   mVertexNode=0;
00053   mUsed = 0;
00054   mCurvQa.reset();
00055   mTanlQa.reset();
00056 }
00057 
00058 //______________________________________________________________________________
00059 void StiTrackNodeHelper::set(StiKalmanTrackNode *pNode,StiKalmanTrackNode *sNode)               
00060 {
00061   static const double EC = 2.99792458e-4;
00062   if(!pNode) reset();
00063   mParentNode = pNode;
00064   mTargetNode = sNode;
00065   mTargetHz = mTargetNode->getHz();
00066   mParentHz = mTargetHz;
00067   if (mParentNode) {
00068     mParentHz = mParentNode->getHz();
00069     assert(fabs(mParentHz-mParentNode->mFP.hz()) < EC*0.1); // allow the difference in 100 Gauss. TODO check why 10 is not enough
00070     mParentNode->mFP.check("2StiTrackNodeHelper::set");
00071   }
00072   if (mTargetNode->isValid()) {
00073     mTargetNode->mFP.check("1StiTrackNodeHelper::set");
00074     assert(fabs(mTargetHz-mTargetNode->mFP.hz()) < EC*0.1);
00075   }
00076 
00077   mDetector   = mTargetNode->getDetector();
00078   if (!mDetector) mVertexNode = mTargetNode;
00079   mHit        = mTargetNode->getHit();
00080   if (mHit) {
00081     double time = 0;
00082     if (mHit->vy() || mHit->vz()) time = mTargetNode->getTime();
00083     mHitPars[0]=mHit->x();
00084     mHitPars[1]=mHit->y(time);
00085     mHitPars[2]=mHit->z(time);
00086   }
00087 
00088   if (!mIter) mTargetNode->mFlipFlop=0;
00089 }
00090 //______________________________________________________________________________
00091 int StiTrackNodeHelper::propagatePars(const StiNodePars &parPars
00092                                      ,      StiNodePars &rotPars
00093                                      ,      StiNodePars &proPars)
00094 {
00095   int ierr = 0;
00096   alpha = mTargetNode->_alpha - mParentNode->_alpha;
00097   ca=1;sa=0;
00098   parPars.check("1propagatePars");
00099   rotPars = parPars;
00100   if (fabs(alpha) > 1.e-6) { //rotation part
00101 
00102     double xt1=parPars.x(); 
00103     double yt1=parPars.y(); 
00104     double cosCA0 = parPars._cosCA;
00105     double sinCA0 = parPars._sinCA;
00106 
00107     ca = cos(alpha);
00108     sa = sin(alpha);
00109 
00110     rotPars.x() = xt1*ca + yt1*sa;
00111     rotPars.y() = -xt1*sa + yt1*ca;
00112     rotPars._cosCA =  cosCA0*ca+sinCA0*sa;
00113     rotPars._sinCA = -cosCA0*sa+sinCA0*ca;
00114     double nor = 0.5*(rotPars._sinCA*rotPars._sinCA+rotPars._cosCA*rotPars._cosCA +1);
00115     rotPars._cosCA /= nor;
00116     rotPars._sinCA /= nor;
00117     rotPars.eta()= NICE(parPars.eta()-alpha); 
00118   }// end of rotation part
00119   ierr = rotPars.check();
00120   if (ierr) return 1;
00121   
00122 //      Propagation 
00123   x1 = rotPars.x();
00124   x2 = (mDetector)? mDetector->getPlacement()->getNormalRadius():mHitPars[0];
00125   dx = x2-x1;
00126   rho = 0.5*(mTargetHz*rotPars.ptin()+rotPars.curv());
00127   dsin = rho*dx;
00128   sinCA2=rotPars._sinCA + dsin; 
00129   if (sinCA2> 0.95) sinCA2= 0.95;
00130   if (sinCA2<-0.95) sinCA2=-0.95;
00131   cosCA2 = ::sqrt((1.-sinCA2)*(1.+sinCA2));
00132   sumSin   = rotPars._sinCA+sinCA2;
00133   sumCos   = rotPars._cosCA+cosCA2;
00134   dy = dx*(sumSin/sumCos);
00135   y2 = rotPars.y()+dy;
00136   dl0 = rotPars._cosCA*dx+rotPars._sinCA*dy;
00137   sind = dl0*rho;
00138   if (fabs(dsin) < 0.02 && rotPars._cosCA >0) { //tiny angle
00139     dl = dl0*(1.+sind*sind/6);
00140   } else {
00141     double cosd = cosCA2*rotPars._cosCA+sinCA2*rotPars._sinCA;
00142     dl = atan2(sind,cosd)/rho;
00143   }
00144 
00145   proPars.x() = x2;
00146   proPars.y() = y2;
00147   proPars.z() = rotPars.z() + dl*rotPars.tanl();
00148   proPars.eta() = (rotPars.eta()+rho*dl);                                       
00149   proPars.eta() = NICE(proPars.eta());                                          
00150   proPars.ptin() = rotPars.ptin();
00151   proPars.hz()   = mTargetHz;
00152   proPars.curv() = proPars.ptin()*mTargetHz;
00153   proPars.tanl() = rotPars.tanl();
00154   proPars._sinCA   = sinCA2;
00155   proPars._cosCA   = cosCA2;
00156   ierr = proPars.check();
00157   if (ierr) return 2;
00158   return 0;
00159 } 
00160 //______________________________________________________________________________
00161 int StiTrackNodeHelper::propagateFitd()
00162 {
00163 //              account eloss in matrix of transofrmation
00164 //              remember that in matrix diagonal and correction factors
00165 //              1. subtructed.
00166    int ierr = 0;
00167    mBestPars.ptin() *= (1+mMcs._ptinCorr);
00168    mBestPars.curv() *= (1+mMcs._ptinCorr);
00169    mMtx.A[4][4] = (mMtx.A[4][4]+1)*(1+mMcs._ptinCorr) -1;
00170 
00171    StiNodePars rotPars;
00172    ierr = propagatePars(mFitdParentPars,rotPars,mPredPars);
00173    if (ierr) return 1;
00174 //   cutStep(&mBestPars,&mPredPars);
00175    mPredPars.hz() = mTargetHz;
00176    mPredPars.ptin() *= (1+mMcs._ptinCorr);
00177    mPredPars.curv() *= (1+mMcs._ptinCorr);
00178    mPredPars.ready();
00179    ierr = mPredPars.check();
00180    if (ierr) return 2;
00181    return 0;
00182 }
00183 
00184 
00185 
00186 //______________________________________________________________________________
00187 int StiTrackNodeHelper::propagateMtx()
00188 {
00189 //      fYE == dY/dEta
00190   double fYE= dx*(1.+mBestParentRotPars._cosCA*cosCA2+mBestParentRotPars._sinCA*sinCA2)/(sumCos*cosCA2);
00191 //      fZE == dZ/dEta
00192   double dLdEta = dy/cosCA2;
00193   double fZE =  mBestPars.tanl()*dLdEta;
00194 //      fZT == dZ/dTanL; 
00195   double fZT= dl; 
00196 
00197 
00198 //      fEC == dEta/dRho
00199   double fEC = dx/cosCA2;
00200 //      fYC == dY/dRho
00201   double fYC=(dl0)/sumCos*fEC;
00202 //      fZC == dZ/dRho
00203   double dang = dl*rho;
00204   double C2LDX = dl*dl*(
00205                0.5*sinCA2*pow((1+pow(dang/2,2)*sinX(dang/2)),2) +
00206                    cosCA2*dang*sinX(dang));
00207 
00208   double fZC = mBestPars.tanl()*C2LDX/cosCA2;
00209 
00210   fEC*=mTargetHz; fYC*=mTargetHz;fZC*=mTargetHz;
00211     
00212 
00213   mMtx.reset();
00214 //  X related derivatives
00215   mMtx.A[0][0] = -1;
00216   mMtx.A[1][0] = -sinCA2/cosCA2; 
00217   mMtx.A[2][0] = -mBestPars.tanl()/cosCA2 ;
00218   mMtx.A[3][0] = -mBestPars.curv()/cosCA2 ;       ;
00219 
00220   mMtx.A[1][3]=fYE; mMtx.A[1][4]=fYC; mMtx.A[2][3]=fZE;
00221   mMtx.A[2][4]=fZC; mMtx.A[2][5]=fZT; mMtx.A[3][4]=fEC;
00222   double fYX = mMtx.A[1][0]; 
00223   mMtx.A[1][0] = fYX*ca-sa;
00224   mMtx.A[1][1] = fYX*sa+ca-1;
00225   return 0;
00226 }
00227 //______________________________________________________________________________
00228 int StiTrackNodeHelper::propagateError()
00229 {
00230   mPredErrs = mFitdParentErrs;
00231   StiTrackNode::errPropag6(mPredErrs.A,mMtx.A,kNPars);
00232   mPredErrs.recov();
00233   mPredErrs._cEE+=mMcs._cEE;            //add err to <eta*eta> eta crossing angle//add err to <eta*eta> eta crossing angle
00234   mPredErrs._cPP+=mMcs._cPP;            //add err to <curv*curv>                 //add err to <curv*curv>
00235   mPredErrs._cTP+=mMcs._cTP;            //add err to <tanL*curv>                 //add err to <tanL*curv>
00236   mPredErrs._cTT+=mMcs._cTT;            //add err to <tanL*tanL>                 //add err to <tanL*tanL>
00237   int ierr = mPredErrs.check();
00238   if (ierr) return 1;
00239   return 0;
00240 }
00241 
00242 //______________________________________________________________________________
00243 int StiTrackNodeHelper::makeFit(int smooth)
00244 {
00245 static int nCall=0;
00246 nCall++;
00247 StiDebug::Break(nCall);
00248   int ierr=0;
00249   mState = 0;
00250   mChi2 = 1e13;
00251   if (mParentNode) {
00252 //              Select the best params to make matrix
00253 
00254 //              Get best params for derivatives
00255     if (smooth) {//joined pars always the best
00256       mBestParentPars = mJoinPars;
00257       mBestParentErrs = mJoinErrs;
00258       mBestDelta      = mJoinErrs.getDelta();
00259     } else {
00260       double delta = mFitdErrs.getDelta();
00261       double er1 = delta*delta;
00262       double er2 = mSavdDelta*mSavdDelta;
00263       double wt = er1/(er1+er2);
00264       mBestParentPars = mFitdPars;      
00265       mBestParentPars.merge(wt,mSavdParentPars);
00266       mBestDelta = sqrt(er1*er2/(er1+er2));
00267 
00268     }
00269     mBestParentPars.check("1makeFit"); 
00270     mFitdParentPars = mFitdPars;
00271     mFitdParentErrs = mFitdErrs;
00272     mFitdParentPars.check("2makeFit");
00273     mFitdParentErrs.check("3makeFit");
00274 
00275     ierr = propagatePars(mBestParentPars,mBestParentRotPars,mBestPars);
00276     if(ierr) return 1;
00277     ierr = propagateMtx();      if(ierr) return 2;
00278     ierr = propagateMCS();      if(ierr) return 3;
00279     ierr = propagateFitd();     if(ierr) return 4;
00280     ierr = propagateError();    if(ierr) return 5;
00281 
00282   } 
00283   mState = StiTrackNode::kTNReady;
00284 
00285 //              Save parameters for future, 
00286 //              when target node will became parent one
00287   mSavdParentPars = mTargetNode->mFP;
00288   mSavdDelta = (mTargetNode->isValid())? mTargetNode->mFE.getDelta():3e33;
00289 
00290   if (!mParentNode) {
00291     if (!smooth) mgCutStep = 0;
00292     mPredErrs = mTargetNode->mFE;
00293     ierr = mPredErrs.check();   if (ierr) return 11;
00294     mPredPars = mTargetNode->mFP;
00295     ierr = mPredPars.check();   if (ierr) return 12;
00296     mBestPars = mPredPars;
00297     mBestDelta = mPredErrs.getDelta();
00298     mJoinPars = mPredPars;
00299     resetError(mNodeErrFactor);
00300   }
00301 //              Set fitted pars to predicted for the absent hit case
00302   mFitdPars = mPredPars;
00303   mFitdErrs = mPredErrs;
00304 
00305   int ians = 1;
00306   mChi2 =0;
00307   do {//technical (fake) loop
00308     if (!mHit)          break;
00309     setHitErrs();
00310     if (nudge())                        return 13;
00311     mChi2 = 3e33;
00312     double chi2 = evalChi2();
00313     if (mTargetNode == mVertexNode) {
00314       if (chi2>mChi2Vtx)                return 14;
00315     } else {
00316       if (chi2>mChi2Max)                break;
00317     }
00318     mChi2 = chi2; if (mChi2>999) mChi2=999;
00319     ians = updateNode();
00320     if (debug() & 8) {cout << Form("%5d ",ians); StiKalmanTrackNode::PrintStep();}
00321     if (!ians)  break;
00322     if (mTargetNode == mVertexNode)     return 15;
00323     mState = StiTrackNode::kTNReady;
00324     mFitdPars = mPredPars;
00325     mFitdErrs = mPredErrs;
00326     mChi2 = 3e33;
00327   }while(0);
00328 
00329   ierr  = (!smooth)? save():join();
00330   if (ierr)                             return 16;
00331 
00332 
00333   do { //fake loop
00334     if (!smooth)                        break;
00335     if (!mHit)                          break;
00336     if (mDetector && (!mFlipFlopNode || mTargetNode->mFlipFlop > mFlipFlopNode->mFlipFlop))
00337        {mFlipFlopNode=mTargetNode;}
00338     if(mState!=StiTrackNode::kTNFitEnd) break;
00339     mUsed++;
00340     if (mDetector && (!mWorstNode || mChi2>mWorstNode->getChi2())) 
00341        {mWorstNode=mTargetNode;}
00342     if (!mParentNode)                   break;
00343     double accu,errr;
00344     accu = mJoinPars.ptin() - mBestParentPars.ptin()*(1+mMcs._ptinCorr);
00345     errr = sqrt(0.5*(fabs(mJoinErrs._cPP)+fabs(mBestParentErrs._cPP)));
00346     mCurvQa.add(accu/errr);    
00347     accu = mJoinPars.tanl() - mBestParentPars.tanl();
00348     errr = sqrt(0.5*(fabs(mJoinErrs._cTT)+fabs(mBestParentErrs._cTT)));
00349     mTanlQa.add(accu/errr);    
00350   }while(0);
00351     
00352   return 0;
00353 }  
00354   
00355 //______________________________________________________________________________
00356 int StiTrackNodeHelper::join()
00357 {
00358   enum {kOLdValid=1,kNewFitd=2,kJoiUnFit=4};
00359 //  if (!mParentNode) return 0;
00360 
00361 
00362   int ierr = 0;
00363   double chi2;          
00364     
00365   StiDebug::Break(mTargetNode->mId);
00366   int kase = mTargetNode->isValid();
00367   if (mState==StiTrackNode::kTNFitEnd) kase |=kNewFitd;
00368 static int oldJoinPrim = StiDebug::iFlag("StiOldJoinPrim");
00369 if (!oldJoinPrim) {
00370   if (mTargetNode==mVertexNode) kase = kNewFitd; //ignore old info for primVtx
00371                                                  //Hack to accoont specific 
00372                                                  //fit to primVtx
00373 }
00374   do {
00375     switch(kase) {
00376       case 0:                                   // Old invalid & New UnFitd
00377         mChi2 = (mHit)? 3e33:0;
00378         mState = StiTrackNode::kTNReady;
00379 
00380       case kNewFitd:                            // Old invalid & New Fitd
00381         mJoinPars = mFitdPars;
00382         mJoinErrs = mFitdErrs;
00383         kase = -1; break;
00384 
00385       case kOLdValid|kNewFitd|kJoiUnFit:        // Old valid & New Fitd & Join UnFit
00386         mChi2 = 3e33;
00387         mFitdPars = mPredPars;
00388         mFitdErrs = mPredErrs;
00389         mState = StiTrackNode::kTNReady;
00390      
00391       case kOLdValid:;                          // Old valid & New UnFitd
00392       case kOLdValid|kNewFitd:;                 // Old valid & New Fitd
00393 
00394         chi2 = joinTwo(kNPars,mTargetNode->mPP().P,mTargetNode->mPE().A
00395                       ,kNPars,mFitdPars.P         ,mFitdErrs.A
00396                              ,mJoinPars.P         ,mJoinErrs.A);
00397         mJoinPars.hz() = mTargetHz;
00398         mJoinErrs.recov();
00399 
00400 
00401         if (kase == (kOLdValid|kNewFitd)) {     //Check errors improvements
00402 //VP      assert(mHrr.hYY > mJoinErrs._cYY);
00403 //VP      assert(mHrr.hZZ > mJoinErrs._cZZ);
00404           if (mHrr.hYY <= mJoinErrs._cYY) {
00405             LOG_DEBUG << Form("StiTrackNodeHelper::updateNode() WRONG hYY(%g) < nYY(%g)"
00406                    ,mHrr.hYY,mFitdErrs._cYY)<< endm;
00407             return -13;
00408           }
00409           if (mHrr.hZZ <= mJoinErrs._cZZ) {
00410             LOG_DEBUG << Form("StiTrackNodeHelper::updateNode() WRONG hZZ(%g) < nZZ(%g)"
00411                   ,mHrr.hZZ,mFitdErrs._cZZ) << endm;
00412             return -14;
00413           }
00414         } //End  Check errors improvements
00415         mJoinPars.ready();
00416         ierr = mJoinPars.check();       if (ierr) return 2;
00417         if (kase!=(kOLdValid|kNewFitd)) {kase = -1; break;}             
00418 
00419         mChi2 = 3e33;
00420 //      chi2 = joinChi2();              //Test join Chi2
00421         chi2 = recvChi2();              //Test join Chi2
00422         mChi2 = 3e33;
00423         if (chi2>mChi2Max && mTargetNode!=mVertexNode) 
00424                                         { kase |=kJoiUnFit; break;} //join Chi2 too big
00425         mChi2 = (chi2>999)? 999:chi2;
00426         mState = StiTrackNode::kTNFitEnd;
00427         kase = -1; break;
00428         
00429        default: assert(0);
00430      }//end Switch
00431   } while(kase>=0);
00432 
00433    assert(fabs(mJoinPars.hz()-mTargetHz)<=1e-10);
00434    assert(fabs(mTargetNode->getHz()-mTargetHz)<=1e-10);
00435 
00436 
00437   mTargetNode->mFE   = mJoinErrs;
00438   mTargetNode->mPE() = mJoinErrs;
00439   mTargetNode->mFP   = mJoinPars;
00440   mTargetNode->mPP() = mJoinPars;
00441   mTargetNode->mHrr  = mHrr;
00442   if (mTargetNode!=mVertexNode) mTargetNode->mUnTouch = mUnTouch;
00443   mTargetNode->_state = mState;
00444   if (mHit && ((mChi2<1000) != (mTargetNode->getChi2()<1000))) mTargetNode->mFlipFlop++;
00445   if (mTargetNode!=mVertexNode) mTargetNode->setChi2(mChi2);
00446 
00447 //      Sanity check
00448   double myMaxChi2 = (mTargetNode==mVertexNode)? 1000:mChi2Max;
00449   if (mState ==  StiTrackNode::kTNFitEnd) {
00450     assert(mChi2 <myMaxChi2);
00451   } else {
00452     assert(mChi2 <=0 || mChi2 >=myMaxChi2);
00453   }
00454 
00455   return 0;
00456 }
00457 //______________________________________________________________________________
00458 double StiTrackNodeHelper::joinTwo(int nP1,const double *P1,const double *E1
00459                                   ,int nP2,const double *P2,const double *E2
00460                                   ,              double *PJ,      double *EJ)
00461 {
00462 
00463   assert(nP1<=nP2);
00464   int nE1 = nP1*(nP1+1)/2;
00465   int nE2 = nP2*(nP2+1)/2;
00466   TArrayD ard(nE2*6);
00467   double *a = ard.GetArray();  
00468   double *sumE          = (a);
00469   double *sumEI         = (a+=nE2);
00470   double *e1sumEIe1     = (a+=nE2);
00471   double *subP          = (a+=nE2);
00472   double *sumEIsubP     = (a+=nE2);
00473   double chi2=3e33,p,q;
00474 
00475 // Choose the smalest errors
00476   const double *p1 = P1, *p2 = P2, *e1 = E1, *e2 = E2, *t;
00477   double choice = (nP1==nP2)? 0:1;
00478   if (!choice   ) {
00479     for (int i=0,n=1;i<nE2;i+=(++n)) {
00480     p=fabs(e1[i]);q=fabs(e2[i]);choice += (p-q)/(p+q+1e-10);
00481   }}
00482   if ( choice >0) {t = p2; p2 = p1; p1 = t; t = e2; e2 = e1; e1 = t;}
00483 
00484   do {//empty loop
00485 //      Join errors
00486     TCL::vadd(e1,e2,sumE,nE1);
00487     int negati = sumE[2]<0;
00488     if (negati) TCL::vcopyn(sumE,sumE,nE1);
00489     int ign0re = sumE[0]<=0;
00490     if (ign0re) sumE[0] = 1;
00491     TCL::trsinv(sumE,sumEI,nP1);
00492     if (ign0re) {sumE[0]  = 0; sumEI[0] = 0;}
00493     if (negati) {TCL::vcopyn(sumE,sumE,nE1);TCL::vcopyn(sumEI,sumEI,nE1);}
00494     TCL::vsub(p2       ,p1   ,subP       ,nP1);
00495     TCL::trasat(subP,sumEI,&chi2,1,nP1); 
00496     if (!EJ) break;
00497     TCL::trqsq (e1  ,sumEI,e1sumEIe1,nP2); 
00498     TCL::vsub(e1,e1sumEIe1,EJ,nE2);
00499   } while(0);
00500 //      Join params
00501   if (PJ) {
00502     TCL::tras(subP     ,sumEI,sumEIsubP,1,nP1);
00503     TCL::tras(sumEIsubP,e1   ,PJ       ,1,nP2);
00504     TCL::vadd(PJ       ,p1   ,PJ         ,nP2);
00505   }
00506   return chi2;
00507 }
00508 #if 0
00509 //______________________________________________________________________________
00510 double StiTrackNodeHelper::joinVtx(const double *P1,const double *E1
00511                                   ,const double *P2,const double *E2
00512                                   ,      double *PJ,      double *EJ)
00513 {
00514   enum {nP1=3,nE1=6,nP2=kNPars,nE2=kNErrs};
00515   double E1m[nE1],E2m[nE2];
00516 
00517   TCL::vzero(E1m       ,nE1);
00518   TCL::ucopy(E2,E2m    ,nE2);
00519   TCL::vadd (E1,E2m,E2m,nE1);
00520   return joinTwo(nP1,P1,E1m,nP2,P2,E2m,PJ,EJ);
00521 
00522 }
00523 #endif//0
00524 #if 0
00525 //______________________________________________________________________________
00526 double StiTrackNodeHelper::joinVtx(const double *P1,const double *E1
00527                                   ,const double *P2,const double *E2
00528                                   ,      double *PJ,      double *EJ)
00529 {
00530   enum {nP1=3,nE1=6,nP2=kNPars,nE2=kNErrs};
00531 
00532   TArrayD ard(nE2*7);
00533   double *p = ard.GetArray();
00534   double *sumE                  = (p);
00535   double *sumEI                 = (p+=nE2);
00536   double *sumEI_E1_sumEI        = (p+=nE2);
00537   double *E2_sumEI_E2           = (p+=nE2);
00538   double *E2_sumEI_E1_sumEI_E2  = (p+=nE2);
00539   double *sumEIsubP             = (p+=nE2);
00540   double *subP                  = (p+=nE2);
00541 
00542   double chi2=3e33;
00543 
00544 
00545   do {//empty loop
00546 //      Join errors
00547     TCL::ucopy(E2,sumE,nE1);
00548     int ign0re = sumE[0]<=0;
00549     if (ign0re) sumE[0] = 1;
00550     TCL::trsinv(sumE,sumEI,nP1);
00551     if (ign0re) {sumE[0]  = 0; sumEI[0] = 0;}
00552     TCL::vsub(P1       ,P2   ,subP       ,nP1);
00553     TCL::trasat(subP,sumEI,&chi2,1,nP1); 
00554     if (!EJ) break;
00555     TCL::trqsq (E2  ,sumEI,E2_sumEI_E2,nP2); 
00556     TCL::vsub(E2,E2_sumEI_E2,EJ,nE2);
00557 //      Now account errors of vertex itself
00558     TCL::trqsq (sumEI,E1,sumEI_E1_sumEI,nP1); 
00559     TCL::trqsq (E2,sumEI_E1_sumEI,E2_sumEI_E1_sumEI_E2,nP2); 
00560     TCL::vadd(EJ,E2_sumEI_E1_sumEI_E2,EJ,nE2);
00561 
00562   } while(0);
00563 //      Join params
00564   if (PJ) {
00565     TCL::tras(subP     ,sumEI,sumEIsubP,1,nP1);
00566     TCL::tras(sumEIsubP,E2   ,PJ       ,1,nP2);
00567     TCL::vadd(PJ       ,P2   ,PJ         ,nP2);
00568   }
00569   return chi2;
00570 }
00571 #endif//1
00572 //______________________________________________________________________________
00573 double StiTrackNodeHelper::joinVtx(const double      *Y,const StiHitErrs  &B
00574                                   ,const StiNodePars &X,const StiNodeErrs &A
00575                                   ,      StiNodePars *M,      StiNodeErrs *C)
00576 {
00577 // Y : x,y,z of vertex.         B: error matrix of Y
00578 // X : track parameters.        A: error matrix of X
00579 // M : track parameters of X+Y. C: error matrix of M
00580 
00581 
00582   enum {nP1=3,nE1=6,nP2=6,nE2=21};
00583 
00584   StiNodeErrs Ai=A;     //Inverted A
00585   
00586   Ai._cXX=1;
00587   TCL::trsinv(Ai.A,Ai.A,nP2);
00588   Ai._cXX=0;
00589 
00590 
00591   double Ai11i[6],Ai10[3][3],T10[3][3],dif[6],m[6];
00592   Ai.get11(Ai11i);  
00593   Ai.get10(Ai10[0]);  
00594   TCL::trsinv(Ai11i ,Ai11i,3);
00595   TCL::trsa  (Ai11i,Ai10[0],T10[0],3,3);        //Ai11*Ai10
00596   TCL::ucopy(Y,m,3);
00597   TCL::vsub (X.P,Y,dif,3);
00598   TCL::mxmpy(T10[0],dif,m+3,3,3,1);
00599   TCL::vadd(X.P+3,m+3,m+3,3);                   //m:   resulting params
00600   if (M) {TCL::ucopy(m,M->P,nP2); M->ready();}  //fill resulting params
00601 
00602   TCL::vsub(X.P,m,dif,nP2);                     //dif = X - M
00603   double chi2;
00604   TCL::trasat(dif,Ai.A,&chi2,1,nP2);            //calc chi2
00605   if (!C) return chi2;
00606                 // Error matrix calculation
00607   C->reset();
00608 
00609   double TX[nP1][nP2];memset(TX[0],0,sizeof(TX));
00610   for (int i=0;i<3;i++) {TCL::ucopy(T10[i],TX[i],3); TX[i][i+3]=1;}
00611   double C11[nE1];
00612   TCL::trasat(TX[0],A.A,C11,nP1,nP2);
00613   C->set11(C11);
00614   
00615   double TY[nP2][nP1];memset(TY[0],0,sizeof(TX));
00616   for (int i=0;i<3;i++) {TCL::vcopyn(T10[i],TY[i+3],3); TY[i][i]=1;}
00617   TY[0][0] = 0;
00618   double CYY[nE2];
00619   TCL::trasat(TY[0],B.A,CYY,nP2,nP1);
00620   TCL::vadd(CYY,C->A,C->A,nE2);
00621   return chi2;
00622 }
00623 //______________________________________________________________________________
00624 int StiTrackNodeHelper::save()
00625 {
00626    assert(fabs(mPredPars.hz()-mTargetHz)<=1e-10);
00627    assert(fabs(mFitdPars.hz()-mTargetHz)<=1e-10);
00628    assert(fabs(mTargetNode->getHz()-mTargetHz)<=1e-10);
00629    
00630    mTargetNode->mPP() = mPredPars;
00631    mTargetNode->mFP   = mFitdPars;
00632    mTargetNode->mPE() = mPredErrs;
00633    mTargetNode->mFE   = mFitdErrs;
00634    mTargetNode->_state = mState;
00635    if (mHit && ((mChi2<1000) != (mTargetNode->getChi2()<1000))) mTargetNode->mFlipFlop++;
00636    if (mTargetNode!=mVertexNode) mTargetNode->setChi2(mChi2);
00637    return 0;
00638 }
00639 //______________________________________________________________________________
00648 //______________________________________________________________________________
00649 int StiTrackNodeHelper::propagateMCS()
00650 {  
00651   mMcs.reset();
00652   if (!mDetector)                       return 0;
00653   mMcs._ptinCorr =  0;
00654   if (fabs(mBestPars.ptin())<=1e-3)     return 0;
00655   double pt     = 1./fabs(mBestPars.ptin());
00656 
00657   double relRadThickness;
00658   // Half path length in previous node
00659   double pL1,pL2,pL3,d1,d2,d3,dxEloss,dx;
00660   pL1=0.5*pathIn(mParentNode->getDetector(),&mBestParentPars);
00661   // Half path length in this node
00662   pL3=0.5*pathIn(mDetector,&mBestPars);
00663   // Gap path length
00664   pL2= fabs(dl);
00665   double x0p =-1;
00666   double x0Gas=-1;
00667   double x0=-1;
00668   dx = mBestPars.x() - mBestParentRotPars.x();
00669   d1    = mParentNode->getDensity();
00670   x0p   = mParentNode->getX0();
00671   d3    = mDetector->getMaterial()->getDensity();
00672   x0    = mDetector->getMaterial()->getX0();
00673 
00674 
00675   if (pL2> (pL1+pL3)) {
00676 
00677       pL2=pL2-pL1-pL3;
00678       if (dx>0)  {
00679         x0Gas = mDetector->getGas()->getX0();
00680         d2    = mDetector->getGas()->getDensity();
00681       } else {
00682         x0Gas = mParentNode->getGasX0(); 
00683         d2    = mParentNode->getGasDensity();
00684       }
00685 
00686       relRadThickness = 0.;
00687       dxEloss = 0;
00688 
00689       if (x0p>0.) {
00690          relRadThickness += pL1/x0p;
00691          dxEloss += d1*pL1;
00692       }
00693       if (x0Gas>0.) {
00694         relRadThickness += pL2/x0Gas;
00695         dxEloss += d2*pL2;
00696       }
00697       if (x0>0.){ 
00698         relRadThickness += pL3/x0;
00699         dxEloss += d3*pL3;
00700       }
00701     } else { //  pL2<=(pL1+pL3)
00702       relRadThickness = 0.; 
00703       dxEloss = 0;
00704       if (x0p>0.) {
00705         relRadThickness += pL1/x0p;
00706         dxEloss += d1*pL1;
00707       }
00708       if (x0>0.){
00709         relRadThickness += pL3/x0;
00710         dxEloss += d3*pL3;
00711       }
00712     }
00713   double tanl   = mBestPars.tanl();
00714   double pti    = mBestPars.ptin(); 
00715   double p2     = (1.+tanl*tanl)*pt*pt;
00716   double m      = StiKalmanTrackFinderParameters::instance()->getMassHypothesis();
00717   double m2     = m*m;
00718   double e2     = p2+m2;
00719   double beta2  = p2/e2;
00720   double theta2 = StiKalmanTrackNode::mcs2(relRadThickness,beta2,p2);
00721   double cos2Li = (1.+ tanl*tanl);  // 1/cos(lamda)**2
00722   double f = mHitsErrFactor; 
00723   mMcs._cEE = cos2Li            *theta2*f;
00724   mMcs._cPP = tanl*tanl*pti*pti *theta2*f;
00725   mMcs._cTP = pti*tanl*cos2Li   *theta2*f;
00726   mMcs._cTT = cos2Li*cos2Li     *theta2*f;
00727 
00728   double dE=0;
00729   double sign = (dx>0)? 1:-1;
00730 
00731 //  const static double I2Ar = (15.8*18) * (15.8*18) * 1e-18; // GeV**2
00732   StiElossCalculator * calculator = mDetector->getElossCalculator();
00733   double eloss = calculator->calculate(1.,m, beta2);
00734   dE = sign*dxEloss*eloss;
00735 
00736   mMcs._ptinCorr = ::sqrt(e2)*dE/p2;
00737   if (fabs(mMcs._ptinCorr)>0.1) mMcs._ptinCorr = (dE<0)? -0.1:0.1;
00738   return 0;
00739 }
00740 //______________________________________________________________________________
00754 //______________________________________________________________________________
00755 double StiTrackNodeHelper::evalChi2() 
00756 {
00757   double r00, r01,r11,chi2;
00758   //If required, recalculate the errors of the detector hits.
00759   //Do not attempt this calculation for the main vertex.
00760   if (fabs(mPredPars._sinCA)>0.99        )      return 1e41;
00761   if (fabs(mPredPars.eta())       >kMaxEta)     return 1e41;
00762   if (fabs(mPredPars.curv())      >kMaxCur)      return 1e41;
00763   if (!mDetector)       { //Primay vertex
00764     mHitPars[0] = mPredPars.x();
00765 //    chi2 = joinVtx(mHitPars,mHrr.A,mPredPars.P,mPredErrs.A);
00766     chi2 = joinVtx(mHitPars,mHrr,mPredPars,mPredErrs);
00767   } else                { //Normal hit
00768 
00769     r00=mPredErrs._cYY+mHrr.hYY;
00770     r01=mPredErrs._cZY+mHrr.hZY;
00771     r11=mPredErrs._cZZ+mHrr.hZZ;
00772     mDetm = r00*r11 - r01*r01;
00773     if (mDetm<r00*r11*1.e-5) {
00774       LOG_DEBUG << Form("StiTrackNodeHelper::evalChi2 *** zero determinant %g",mDetm)<< endm;
00775       return 1e60;
00776     }
00777     double tmp=r00; r00=r11; r11=tmp; r01=-r01;  
00778 
00779     double dyt=(mPredPars.y()-mHitPars[1]);
00780     double dzt=(mPredPars.z()-mHitPars[2]);
00781     chi2 = (dyt*r00*dyt + 2*r01*dyt*dzt + dzt*r11*dzt)/mDetm;
00782   }
00783   return chi2;
00784 }
00785 //______________________________________________________________________________
00786 /*
00787  * double StiTrackNodeHelper::joinChi2() 
00788  * {
00789  *   double chi2;
00790  *   double mergPars[3],mHitPars[3],mergErrs[6];
00791  *   chi2 = joinTwo(3,mPredPars.P         ,mPredErrs.A
00792  *                 ,3,mTargetNode->mPP().P,mTargetNode->mPE().A
00793  *                   ,mergPars            ,mergErrs);
00794  *   
00795  *   mHitPars[0] = mHit->x();
00796  *   mHitPars[1] = mHit->y();
00797  *   mHitPars[2] = mHit->z();
00798  *   chi2 = joinTwo(3,mergPars,mergErrs,3,mHitPars,mHrr.A);
00799  * //   Save untouched by current hit node's y,z & errors for alignment
00800  *   mUnTouch.mPar[0] = mergPars[1];
00801  *   mUnTouch.mPar[1] = mergPars[2];
00802  *   mUnTouch.mErr[0] = mergErrs[2];
00803  *   mUnTouch.mErr[1] = mergErrs[4];
00804  *   mUnTouch.mErr[2] = mergErrs[5];
00805  *   return chi2;
00806  * }
00807  */
00808 //______________________________________________________________________________
00809 double StiTrackNodeHelper::recvChi2() 
00810 {
00811   if (fabs(mJoinPars._sinCA)>0.99        )      return 1e41;
00812   if (fabs(mJoinPars.eta())       >kMaxEta)     return 1e41;
00813   if (fabs(mJoinPars.curv())      >kMaxCur)      return 1e41;
00814   if (!mDetector) {//Primary vertex
00815 //  double chi2 =joinVtx(mHitPars,mHrr.A,mPredPars.P,mPredErrs.A);
00816     double chi2 =joinVtx(mHitPars,mHrr  ,mPredPars  ,mPredErrs  );
00817     return chi2;
00818   }
00819 
00820   StiHitErrs  myHrr = mHrr;
00821   StiNodeErrs recovErrs;
00822   StiNodePars recovPars;
00823   double f = -(1./mHitsErrFactor);
00824   myHrr*=f;
00825   double r11,r12,r22;
00826   if ((r11=myHrr.hYY+mJoinErrs._cYY) >=0)       return 1e41;
00827   if ((r22=myHrr.hZZ+mJoinErrs._cZZ) >=0)       return 1e41;
00828   r12 =myHrr.hZY+mJoinErrs._cZY;
00829   if (r11*r22-r12*r12<0)                        return 1e41;      
00830 
00831 
00832   double chi2 = joinTwo(3,mHitPars    ,    myHrr.A
00833                        ,3,mJoinPars.P,mJoinErrs.A
00834                          ,recovPars.P,recovErrs.A);
00835   if (fabs(recovPars.y()-mHitPars[1])>10) StiDebug::Break(-1);
00836   if (fabs(recovPars.z()-mHitPars[2])>10) StiDebug::Break(-1);
00837 
00838   mUnTouch.set(recovPars,recovErrs);
00839   return -chi2; //account that result is negative
00840 }
00841 //______________________________________________________________________________
00842 int StiTrackNodeHelper::setHitErrs() 
00843 {
00844   getHitErrors(mHit,&mFitdPars,&mHrr);
00845   mHrr*=mHitsErrFactor;
00846   return 0;
00847 }
00848 //______________________________________________________________________________
00868 //______________________________________________________________________________
00869 int StiTrackNodeHelper::updateNode() 
00870 {
00871 static int nCall=0; nCall++;
00872   mState = StiTrackNode::kTNFitBeg;
00873   assert(mPredErrs._cXX<1e-8);
00874   double r00,r01,r11;
00875   StiDebug::Break(mTargetNode->mId);
00876   if (!mDetector)       { //Primary vertex
00877     mHitPars[0] = mPredPars.x();
00878 //  double chi2 = joinVtx(mHitPars,mHrr.A,mPredPars.P,mPredErrs.A,mFitdPars.P,mFitdErrs.A);
00879     double chi2 = joinVtx(mHitPars,mHrr,mPredPars,mPredErrs,&mFitdPars,&mFitdErrs);
00880     mFitdPars.curv() = mTargetHz*mFitdPars.ptin();
00881     assert(chi2>900 || fabs(mChi2-chi2)<1e-10);
00882     if (debug()) {
00883       StiKalmanTrackNode::ResetComment(Form("Vertex                        "));
00884     }
00885   } else                { //Normal Hit
00886     r00=mHrr.hYY+mPredErrs._cYY;
00887     r01=mHrr.hZY+mPredErrs._cZY;
00888     r11=mHrr.hZZ+mPredErrs._cZZ;
00889     mDetm=r00*r11 - r01*r01;
00890     if (!(mDetm>(r00*r11)*1.e-5)) return 99;
00891     assert(mDetm>(r00*r11)*1.e-5);
00892 
00893     // inverse matrix
00894     double tmp=r00; r00=r11/mDetm; r11=tmp/mDetm; r01=-r01/mDetm;
00895     // update error matrix
00896     double k00=mPredErrs._cYY*r00+mPredErrs._cZY*r01, k01=mPredErrs._cYY*r01+mPredErrs._cZY*r11;
00897     double k10=mPredErrs._cZY*r00+mPredErrs._cZZ*r01, k11=mPredErrs._cZY*r01+mPredErrs._cZZ*r11;
00898     double k20=mPredErrs._cEY*r00+mPredErrs._cEZ*r01, k21=mPredErrs._cEY*r01+mPredErrs._cEZ*r11;
00899     double k30=mPredErrs._cPY*r00+mPredErrs._cPZ*r01, k31=mPredErrs._cPY*r01+mPredErrs._cPZ*r11;
00900     double k40=mPredErrs._cTY*r00+mPredErrs._cTZ*r01, k41=mPredErrs._cTY*r01+mPredErrs._cTZ*r11;
00901 
00902     double myY = mPredPars.y();
00903     double myZ = mPredPars.z();
00904     double dyt  = mHitPars[1] - myY;
00905     double dzt  = mHitPars[2] - myZ;
00906     double dPt  = k30*dyt + k31*dzt;
00907     double dEt  = k20*dyt + k21*dzt;
00908     double dTa  = k40*dyt + k41*dzt;
00909     double eta  = NICE(mPredPars.eta() + dEt);
00910     double pti  = mPredPars.ptin() + dPt;
00911     double tanl = mPredPars.tanl() + dTa;
00912     // Check if any of the quantities required to pursue the update
00913     // are infinite. If so, it means the tracks cannot be update/propagated
00914     // any longer and should therefore be abandoned. Just return. This is 
00915     // not a big but rather a feature of the fact a helicoidal tracks!!!
00916     // update Kalman state
00917     double p0 = myY + k00*dyt + k01*dzt;
00918     double p1 = myZ + k10*dyt + k11*dzt;
00919     //mPredPars._tanl += k40*dyt + k41*dzt;
00920     double sinCA  =  sin(eta);
00921     // The following test introduces a track propagation error but happens
00922     // only when the track should be aborted so we don't care...
00923 
00924     mFitdPars.hz() = mTargetHz;
00925     mFitdPars.x()  = mPredPars.x();
00926     mFitdPars.y()  = p0;
00927     mFitdPars.z()  = p1;
00928     mFitdPars.eta()   = eta;
00929     mFitdPars.ptin() = pti;
00930     mFitdPars.curv()  = mTargetHz*pti;
00931     mFitdPars.tanl()  = tanl;
00932     mFitdPars._sinCA = sinCA;
00933     mFitdPars._cosCA = ::sqrt((1.-mFitdPars._sinCA)*(1.+mFitdPars._sinCA)); 
00934     if (!mDetector) 
00935       assert(fabs(mFitdPars.y()-mHitPars[1])>1e-10 ||  fabs(mHitPars[0])<4);
00936 //??    cutStep(&mFitdPars,&mPredPars);
00937 //??    cutStep(&mFitdPars,&mBestPars);
00938     if (mFitdPars.check()) return -11;
00939   // update error matrix
00940     double c00=mPredErrs._cYY;                       
00941     double c10=mPredErrs._cZY, c11=mPredErrs._cZZ;                 
00942     double c20=mPredErrs._cEY, c21=mPredErrs._cEZ;//, c22=mPredErrs._cEE;           
00943     double c30=mPredErrs._cPY, c31=mPredErrs._cPZ;//, c32=mPredErrs._cPE, c33=mPredErrs._cPP;     
00944     double c40=mPredErrs._cTY, c41=mPredErrs._cTZ;//, c42=mPredErrs._cTE, c43=mPredErrs._cTP, c44=mPredErrs._cTT;
00945     mFitdErrs._cYY-=k00*c00+k01*c10;
00946     mFitdErrs._cZY-=k10*c00+k11*c10;mFitdErrs._cZZ-=k10*c10+k11*c11;
00947     mFitdErrs._cEY-=k20*c00+k21*c10;mFitdErrs._cEZ-=k20*c10+k21*c11;mFitdErrs._cEE-=k20*c20+k21*c21;
00948     mFitdErrs._cPY-=k30*c00+k31*c10;mFitdErrs._cPZ-=k30*c10+k31*c11;mFitdErrs._cPE-=k30*c20+k31*c21;mFitdErrs._cPP-=k30*c30+k31*c31;
00949     mFitdErrs._cTY-=k40*c00+k41*c10;mFitdErrs._cTZ-=k40*c10+k41*c11;mFitdErrs._cTE-=k40*c20+k41*c21;mFitdErrs._cTP-=k40*c30+k41*c31;mFitdErrs._cTT-=k40*c40+k41*c41;
00950     if (debug()) {
00951       StiKalmanTrackNode::ResetComment(Form("%30s ",mDetector->getName().c_str()));
00952     }
00953   }
00954   if (mFitdErrs.check()) return -12;
00955 //  mFitdErrs.recov();
00956   
00957 
00958 static int ERRTEST=0;
00959 if(ERRTEST) errTest(mPredPars,mPredErrs,mHit,mHrr,mFitdPars,mFitdErrs,mChi2);
00960 
00961 //prod  assert(mHrr.hYY > mFitdErrs._cYY);
00962 //prod  assert(mHrr.hZZ > mFitdErrs._cZZ);
00963   if (mDetector) { //Not a primary
00964     if (mHrr.hYY <= mFitdErrs._cYY) {
00965       LOG_DEBUG << Form("StiTrackNodeHelper::updateNode() WRONG hYY(%g) < nYY(%g)"
00966             ,mHrr.hYY,mFitdErrs._cYY)<< endm;
00967       return -13;
00968     }  
00969     if (mHrr.hZZ <= mFitdErrs._cZZ) {
00970       LOG_DEBUG << Form("StiTrackNodeHelper::updateNode() WRONG hZZ(%g) < nZZ(%g)"
00971             ,mHrr.hZZ,mFitdErrs._cZZ)<< endm;
00972       return -14;
00973     }  
00974   } //EndIf Not a primary         
00975   if (debug()) StiKalmanTrackNode::comment += Form(" chi2 = %6.2f",mChi2);
00976   if (mTargetNode && debug()) {
00977     mTargetNode->PrintpT("U");
00978   }
00979   mState = StiTrackNode::kTNFitEnd;
00980   return 0; 
00981 }
00982  
00983 //______________________________________________________________________________
00984 void StiTrackNodeHelper::resetError(double fk)
00985 { 
00986   if (fk) do {//fake loop
00987     if(mPredErrs._cYY>DY*DY)            break;
00988     if(mPredErrs._cZZ>DZ*DZ)            break;
00989     if(mPredErrs._cEE>DEta*DEta)        break;
00990     if(mPredErrs._cPP>DPti*DPti)        break;
00991     if(mPredErrs._cTT>DTan*DTan)        break;
00992     mPredErrs*=fk;
00993     return;
00994   }while(0);
00995 
00996   mPredErrs.reset();
00997   mPredErrs._cYY=DY*DY;
00998   mPredErrs._cZZ=DZ*DZ;
00999   mPredErrs._cEE=DEta*DEta;
01000   mPredErrs._cPP=DPti*DPti;
01001   mPredErrs._cTT=DTan*DTan;
01002 }
01003 //______________________________________________________________________________
01004 int StiTrackNodeHelper::cutStep(StiNodePars *pars,StiNodePars *base)
01005 {
01006   double fact=1,dif,fak;
01007   double cuts[kNPars];
01008   memcpy(cuts,MAXSTEP,sizeof(cuts));
01009   if (mBestDelta<DY) {cuts[1]=mBestDelta;cuts[2]=mBestDelta;} 
01010 
01011   for (int jx=0;jx<kNPars;jx++) {
01012     dif =(fabs((*pars)[jx]-(*base)[jx]));
01013     fak = (dif >cuts[jx]) ? cuts[jx]/dif:1;
01014     if (fak < fact) fact = fak;
01015   }
01016   if (fact>=1) return 0;
01017   mgCutStep++;
01018   for (int jx=0;jx<kNPars;jx++) {
01019     dif =(*pars)[jx]-(*base)[jx];
01020     (*pars)[jx] = (*base)[jx] +dif*fact;
01021   }
01022   pars->ready();
01023   return 1;
01024 }
01025 //______________________________________________________________________________
01026 int StiTrackNodeHelper::nudge()
01027 {
01028   if(!mHit) return 0;
01029   StiNodePars *pars = &mBestPars;
01030   for (int i=0;i<2;i++,pars =&mPredPars) {
01031     double deltaX = mHitPars[0]-pars->x();
01032     if (fabs(deltaX) <1e-6) continue;
01033     double deltaL = deltaX/pars->_cosCA;
01034     double deltaE = pars->curv()*deltaL;
01035     pars->x()      = mHitPars[0];
01036     pars->y()     += pars->_sinCA *deltaL;
01037     pars->z()     += pars->tanl()  *deltaL;
01038     pars->eta()   +=               deltaE;
01039     pars->_cosCA -= pars->_sinCA *deltaE;
01040     pars->_sinCA += pars->_cosCA *deltaE;
01041     if (pars->_cosCA>=1.) pars->ready();
01042     if (pars->check()) return 1;
01043   }
01044   return 0;
01045 }
01046 //______________________________________________________________________________
01047 double StiTrackNodeHelper::pathIn(const StiDetector *det,StiNodePars *pars)
01048 {
01049   if (!det) return 0.; 
01050   double thickness = det->getShape()->getThickness();
01051   double t = pars->tanl();
01052   double c = pars->_cosCA;
01053   return (thickness*::sqrt(1.+t*t)) / c;
01054 }
01055 //______________________________________________________________________________
01056 int StiTrackNodeHelper::getHitErrors(const StiHit *hit,const StiNodePars *pars,StiHitErrs *hrr)
01057 {
01058   hrr->reset();
01059   const StiDetector *det = hit->detector();
01060   const StiHitErrorCalculator *calc = (det)? det->getHitErrorCalculator():0;
01061   if (calc) {//calculate it
01062      calc->calculateError(pars,hrr->hYY,hrr->hZZ);
01063   } else    {//get from hit
01064     const float *ermx = hit->errMtx();    
01065     for (int i=0;i<6;i++){hrr->A[i]=ermx[i];}
01066   }
01067   return (!det);
01068 }
01069 //______________________________________________________________________________
01070 int errTest(StiNodePars &predP,StiNodeErrs &predE,
01071             const StiHit *hit ,StiHitErrs  &hitErr,
01072             StiNodePars &fitdP,StiNodeErrs &fitdE, double chi2)
01073 {
01074 
01075   StiNodePars mineP,hittP;
01076   StiNodeErrs mineE,hittE;
01077   hittP.x() = hit->x();
01078   hittP.y() = hit->y();
01079   hittP.z() = hit->z();
01080   memcpy(hittE.A,hitErr.A,sizeof(StiNodeErrs));
01081   
01082   double myChi2 = StiTrackNodeHelper::joinTwo(
01083                   3,hittP.P,hittE.A,
01084                   6,predP.P,predE.A,
01085                     mineP.P,mineE.A);
01086 
01087 
01088   int ndif = 0;
01089   for (int i=0;i<kNPars;i++) {
01090     double diff = fabs(mineP.P[i]-fitdP.P[i]);
01091     if (diff < 1e-10) continue;
01092     diff/=0.5*(fabs(mineP.P[i])+fabs(fitdP.P[i]));
01093     if (diff < 1e-5 ) continue;
01094     ndif++;
01095     LOG_DEBUG << Form("errTest(P): %g(%d) - %g(%d) = %g",mineE.A[i],i,fitdE.A[i],i,diff)<< endm;
01096   }
01097   if (ndif){ mineP.print();fitdP.print();}
01098 
01099   for (int i=0;i<kNErrs;i++) {
01100     double diff = fabs(mineE.A[i]-fitdE.A[i]);
01101     if (diff < 1e-10) continue;
01102     diff/=0.5*(fabs(mineE.A[i])+fabs(fitdE.A[i]));
01103     if (diff < 1e-5 ) continue;
01104     ndif+=100;
01105     LOG_DEBUG << Form("errTest(E): %g(%d) - %g(%d) = %g",mineE.A[i],i,fitdE.A[i],i,diff)<< endm;
01106   }
01107   if (ndif>=100){ mineE.print();fitdE.print();}
01108   
01109   double diff = fabs((chi2-myChi2)/(chi2+myChi2));
01110   if (diff > 1e-5 ) {
01111     ndif+=1000;
01112     LOG_DEBUG << Form("errTest(C): %g() - %g() = %g",myChi2,chi2,diff)<< endm;
01113   }
01114 
01115   return ndif;
01116 } 
01117 
01118 //_____________________________________________________________________________
01119 void QaFit::add(double val)
01120 {
01121   double v = val; double p = mPrev; mPrev = v;
01122   int n = (mTally)? 2:1;
01123   mTally++;
01124   for (int i=0;i<n;i++) {
01125     mAver[i] +=v;
01126     mErrr[i] +=v*v;
01127     if (mMaxi[i]<fabs(v)) mMaxi[i]=fabs(v);
01128     if (v<0) mNega[i]++;
01129     v = v*p;
01130   }
01131 }
01132 //_____________________________________________________________________________
01133 void QaFit::finish()
01134 {
01135   if( mEnded) return;
01136   if(!mTally) return;
01137   mEnded = 1;
01138   int n = mTally;
01139   for (int i=0;i<2;i++) {
01140     mAver[i]/= n;
01141     mErrr[i]/= n;
01142     if (mErrr[i]<1e-10) mErrr[i]=1e-10;
01143     mErrr[i] = sqrt(mErrr[i]);
01144     if (!(--n)) break;
01145   }
01146 }
01147 //_____________________________________________________________________________
01148 double QaFit::getAccu(int k)
01149 {
01150   finish();
01151   if (mTally-k<=1) return 0;
01152   return mErrr[k];
01153 }  
01154 //_____________________________________________________________________________
01155 double QaFit::getNStd(int k)
01156 {
01157   finish();
01158   int n = mTally-k;
01159   if (!n) return 0;
01160   return mAver[k]*sqrt(double(n));
01161 }  
01162 //_____________________________________________________________________________
01163 double QaFit::getNSgn(int k)
01164 {
01165   finish();
01166   int n = mTally-k;
01167   if (!n) return 0;
01168   return (n-2*mNega[k])/sqrt(double(n));
01169 }  
01170 //_____________________________________________________________________________
01171 void QaFit::getInfo(double *info)
01172 {
01173   for (int i=0;i<2;i++) {
01174     int l = i*4;
01175     info[l+0]=getAccu(i);
01176     info[l+1]=getNStd(i);
01177     info[l+2]=getNSgn(i);
01178     info[l+3]=getMaxi(i);
01179   }
01180 }  
01181   
01182   
01183