StRoot  1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
StiTrackNodeHelper.cxx
1 #include <stdio.h>
2 #include <stdlib.h>
3 #include "StiUtilities/StiDebug.h"
4 #include "StiTrackNodeHelper.h"
5 #include "StiElossCalculator.h"
6 #include "StDetectorDbMaker/StiHitErrorCalculator.h"
7 #include "StMessMgr.h"
8 #include "TArrayD.h"
9 #include "TSystem.h"
10 #include "TCernLib.h"
11 
12 //#define __CHECKIT__ // Enable unused paramter and error checks
13 
14 #define NICE(a) ( ((a) <= -M_PI)? ((a)+2*M_PI) :\
15  ((a) > M_PI)? ((a)-2*M_PI) : (a))
16 
17 #define sinX(a) StiTrackNode::sinX(a)
18 static const double kMaxEta = 1.5;
19 static const double kMaxCur = 0.2;
20 static const double kMaxSin = 0.99;
21 static const double kMinCos = sqrt(1.-kMaxSin*kMaxSin);
22 
23 static const double DY=0.9,DZ=0.9,DEta=0.1,DPti=3,DTan=0.1;
24 static const double MAXSTEP[]={0,DY,DZ,DEta,DPti,DTan};
25 static const double ERROR_FACTOR = 2.;
26 int StiTrackNodeHelper::_debug = 0;
27 int StiTrackNodeHelper::mgCutStep=0;
28 
29 //______________________________________________________________________________
30 int errTest(StiNodePars &predP,StiNodeErrs &predE,
31  const StiHit *hit,StiHitErrs &hitErr,
32  StiNodePars &fitdP,StiNodeErrs &fitdE,double chi2);
33 
34 //______________________________________________________________________________
35 void StiTrackNodeHelper::set(double chi2Max,double chi2Vtx,double errConfidence,int iter)
36 {
37  reset();
38  mChi2Max = chi2Max;
39  mChi2Vtx = chi2Vtx;
40  mNodeErrFactor = 10;
41  mHitsErrFactor = 1.;
42  if (errConfidence>0.1) {
43  mNodeErrFactor = (1./(errConfidence));
44  mHitsErrFactor = (1./(1. - errConfidence));
45  }
46  mIter = iter;
47  if (!mIter) mFlipFlopNode = 0;
48 }
49 //______________________________________________________________________________
50 void StiTrackNodeHelper::reset()
51 {
52  memset(mBeg,'A',mEnd-mBeg+1);
53  mWorstNode =0;
54  mVertexNode=0;
55  mUsed = 0;
56  mCurvQa.reset();
57  mTanlQa.reset();
58 }
59 
60 //______________________________________________________________________________
61 void StiTrackNodeHelper::set(StiKalmanTrackNode *pNode,StiKalmanTrackNode *sNode)
62 {
63  static const double EC = 2.99792458e-4;
64  if(!pNode) reset();
65  mParentNode = pNode;
66  mTargetNode = sNode;
67  mTargetHz = mTargetNode->getHz();
68  mParentHz = mTargetHz;
69  if (mParentNode) {
70  mParentHz = mParentNode->getHz();
71  assert(fabs(mParentHz-mParentNode->mFP.hz()) < EC*0.1); // allow the difference in 100 Gauss. TODO check why 10 is not enough
72 #ifdef __CHECKIT__
73  mParentNode->mFP.check("2StiTrackNodeHelper::set");
74 #endif
75  }
76  if (mTargetNode->isValid()) {
77 #ifdef __CHECKIT__
78  mTargetNode->mFP.check("1StiTrackNodeHelper::set");
79 #endif
80  assert(fabs(mTargetHz-mTargetNode->mFP.hz()) < EC*0.1);
81  }
82 
83  mDetector = mTargetNode->getDetector();
84  if (!mDetector) mVertexNode = mTargetNode;
85  mHit = mTargetNode->getHit();
86  if (mHit) {
87  double time = 0;
88  if (mHit->vy() || mHit->vz()) time = mTargetNode->getTime();
89  mHitPars[0]=mHit->x();
90  mHitPars[1]=mHit->y(time);
91  mHitPars[2]=mHit->z(time);
92  }
93 
94  if (!mIter) mTargetNode->mFlipFlop=0;
95 }
96 //______________________________________________________________________________
97 int StiTrackNodeHelper::propagatePars(const StiNodePars &parPars
98  , StiNodePars &rotPars
99  , StiNodePars &proPars)
100 {
101  int ierr = 0;
102  alpha = mTargetNode->_alpha - mParentNode->_alpha;
103  ca=1;sa=0;
104  rotPars = parPars;
105  if (fabs(alpha) > 1.e-6) { //rotation part
106 
107  double xt1=parPars.x();
108  double yt1=parPars.y();
109  double cosCA0 = parPars._cosCA;
110  double sinCA0 = parPars._sinCA;
111 
112  ca = cos(alpha);
113  sa = sin(alpha);
114 
115  rotPars.x() = xt1*ca + yt1*sa;
116  rotPars.y() = -xt1*sa + yt1*ca;
117  rotPars._cosCA = cosCA0*ca+sinCA0*sa;
118  rotPars._sinCA = -cosCA0*sa+sinCA0*ca;
119  double nor = 0.5*(rotPars._sinCA*rotPars._sinCA+rotPars._cosCA*rotPars._cosCA +1);
120  rotPars._cosCA /= nor;
121  rotPars._sinCA /= nor;
122  rotPars.eta()= NICE(parPars.eta()-alpha);
123  }// end of rotation part
124  ierr = rotPars.check(); // check parameter validity to continue
125  if (ierr) return 1;
126 
127 // Propagation
128  x1 = rotPars.x();
129  int kase = (mDetector) ? mDetector->getShape()->getShapeCode():0;
130  switch (kase) {
131  case 0: x2 = mHitPars[0]; break;
132  case 1: x2 = mDetector->getPlacement()->getNormalRadius(); break;
133  case 2: {
134 
135  double out[2][3];
136  double rxy = mDetector->getPlacement()->getNormalRadius();
137  if (rotPars.P[0]*rotPars._cosCA+rotPars.P[1]*rotPars._sinCA<0)
138  { x2 = rxy; break;}
139  double rxy2P = rotPars.rxy2();
140  int outside = (rxy2P>rxy*rxy);
141  int nSol = StiTrackNode::cylCross(rotPars.P,&rotPars._cosCA,rotPars.curv(),rxy,mDir,out);
142  double *ou = out[0];
143  if (nSol==2) {
144  int kaze = outside + 2*mDir;
145  switch (kaze) {
146  case 0: return 2;
147  case 1: ou = out[1]; break;
148  case 2: ou = out[1]; break;
149  case 3: return 3;
150  default: assert(0);
151  } }
152 
153  x2 = ou[0];
154  break;
155  }
156  default: assert(0 && "wrong shape code");
157  }
158 
159  dx = x2-x1;
160  if (fabs(dx)<1e-5) { proPars = rotPars; return 0;}
161  rho = 0.5*(mTargetHz*rotPars.ptin()+rotPars.curv());
162  dsin = rho*dx;
163  sinCA2=rotPars._sinCA + dsin;
164  if (fabs(sinCA2) > kMaxSin) {
165  sinCA2 = (sinCA2<0) ? -kMaxSin:kMaxSin;
166  dsin = sinCA2-rotPars._sinCA;
167  dx = (fabs(rho)>1e-5)? dsin/rho:0;
168  x2 = x1 + dx;
169  }
170  cosCA2 = ::sqrt((1.-sinCA2)*(1.+sinCA2));
171  sumSin = rotPars._sinCA+sinCA2;
172  sumCos = rotPars._cosCA+cosCA2;
173  dy = (fabs(sumCos)>1e-5) ? dx*(sumSin/sumCos):0;
174  y2 = rotPars.y()+dy;
175  dl0 = rotPars._cosCA*dx+rotPars._sinCA*dy;
176  sind = dl0*rho;
177  if (fabs(dsin) < 0.02 && rotPars._cosCA >0) { //tiny angle
178  dl = dl0*(1.+sind*sind/6);
179  } else {
180  double cosd = cosCA2*rotPars._cosCA+sinCA2*rotPars._sinCA;
181  dl = atan2(sind,cosd)/rho;
182  }
183  proPars.x() = x2;
184  proPars.y() = y2;
185  proPars.z() = rotPars.z() + dl*rotPars.tanl();
186  proPars.eta() = (rotPars.eta()+rho*dl);
187  proPars.eta() = NICE(proPars.eta());
188  proPars.ptin() = rotPars.ptin();
189  proPars.hz() = mTargetHz;
190  proPars.curv() = proPars.ptin()*mTargetHz;
191  proPars.tanl() = rotPars.tanl();
192  proPars._sinCA = sinCA2;
193  proPars._cosCA = cosCA2;
194  ierr = proPars.check();
195  if (ierr) return 2;
196  return 0;
197 }
198 //______________________________________________________________________________
199 int StiTrackNodeHelper::propagateFitd()
200 {
201 // account eloss in matrix of transofrmation
202 // remember that in matrix diagonal and correction factors
203 // 1. subtructed.
204  int ierr = 0;
205  mBestPars.ptin() *= (1+mMcs._ptinCorr);
206  mBestPars.curv() *= (1+mMcs._ptinCorr);
207  mMtx.A[4][4] = (mMtx.A[4][4]+1)*(1+mMcs._ptinCorr) -1;
208 
209  StiNodePars rotPars;
210  ierr = propagatePars(mFitdParentPars,rotPars,mPredPars);
211  if (ierr) return 1;
212 // cutStep(&mBestPars,&mPredPars);
213  mPredPars.hz() = mTargetHz;
214  mPredPars.ptin() *= (1+mMcs._ptinCorr);
215  mPredPars.curv() *= (1+mMcs._ptinCorr);
216  mPredPars.ready();
217  ierr = mPredPars.check();
218  if (ierr) return 2;
219  return 0;
220 }
221 
222 
223 
224 //______________________________________________________________________________
225 int StiTrackNodeHelper::propagateMtx()
226 {
227 // fYE == dY/dEta
228  double fYE= dx*(1.+mBestParentRotPars._cosCA*cosCA2+mBestParentRotPars._sinCA*sinCA2)/(sumCos*cosCA2);
229 // fZE == dZ/dEta
230  double dLdEta = dy/cosCA2;
231  double fZE = mBestPars.tanl()*dLdEta;
232 // fZT == dZ/dTanL;
233  double fZT= dl;
234 
235 
236 // fEC == dEta/dRho
237  double fEC = dx/cosCA2;
238 // fYC == dY/dRho
239  double fYC=(dl0)/sumCos*fEC;
240 // fZC == dZ/dRho
241  double dang = dl*rho;
242  double C2LDX = dl*dl*(
243  0.5*sinCA2*pow((1+pow(dang/2,2)*sinX(dang/2)),2) +
244  cosCA2*dang*sinX(dang));
245 
246  double fZC = mBestPars.tanl()*C2LDX/cosCA2;
247 
248  fEC*=mTargetHz; fYC*=mTargetHz;fZC*=mTargetHz;
249 
250 
251  mMtx.reset();
252 // X related derivatives
253  mMtx.A[0][0] = -1;
254  mMtx.A[1][0] = -sinCA2/cosCA2;
255  mMtx.A[2][0] = -mBestPars.tanl()/cosCA2 ;
256  mMtx.A[3][0] = -mBestPars.curv()/cosCA2 ; ;
257 
258  mMtx.A[1][3]=fYE; mMtx.A[1][4]=fYC; mMtx.A[2][3]=fZE;
259  mMtx.A[2][4]=fZC; mMtx.A[2][5]=fZT; mMtx.A[3][4]=fEC;
260  double fYX = mMtx.A[1][0];
261  mMtx.A[1][0] = fYX*ca-sa;
262  mMtx.A[1][1] = fYX*sa+ca-1;
263  return 0;
264 }
265 //______________________________________________________________________________
266 int StiTrackNodeHelper::propagateError()
267 {
268  mPredErrs = mFitdParentErrs;
269  StiTrackNode::errPropag6(mPredErrs.G(),mMtx.A,kNPars);
270  int force = fabs(dl)> StiNodeErrs::kBigLen;
271  mPredErrs.recov(force);
272  mPredErrs._cEE+=mMcs._cEE; //add err to <eta*eta> eta crossing angle//add err to <eta*eta> eta crossing angle
273  mPredErrs._cPP+=mMcs._cPP; //add err to <curv*curv> //add err to <curv*curv>
274  mPredErrs._cTP+=mMcs._cTP; //add err to <tanL*curv> //add err to <tanL*curv>
275  mPredErrs._cTT+=mMcs._cTT; //add err to <tanL*tanL> //add err to <tanL*tanL>
276  int ierr = mPredErrs.check();
277  if (ierr) return 1;
278  return 0;
279 }
280 
281 //______________________________________________________________________________
282 int StiTrackNodeHelper::makeFit(int smooth)
283 {
284  int ierr=0;
285  mState = 0;
286  mChi2 = 1e13;
287  if (mParentNode) {
288 // Select the best params to make matrix
289 
290 // Get best params for derivatives
291  if (smooth) {//joined pars always the best
292  mBestParentPars = mJoinPars;
293  mBestParentErrs = mJoinErrs;
294  mBestDelta = mJoinErrs.getDelta();
295  } else {
296  double delta = mFitdErrs.getDelta();
297  double er1 = delta*delta;
298  double er2 = mSavdDelta*mSavdDelta;
299  double wt = er1/(er1+er2);
300  mBestParentPars = mFitdPars;
301  mBestParentPars.merge(wt,mSavdParentPars);
302  mBestDelta = sqrt(er1*er2/(er1+er2));
303 
304  }
305 #ifdef __CHECKIT__
306  mBestParentPars.check("1makeFit");
307 #endif
308  mFitdParentPars = mFitdPars;
309  mFitdParentErrs = mFitdErrs;
310 #ifdef __CHECKIT__
311  mFitdParentPars.check("2makeFit");
312  mFitdParentErrs.check("3makeFit");
313 #endif
314 
315  ierr = propagatePars(mBestParentPars,mBestParentRotPars,mBestPars);
316  if(ierr) return 1;
317 
318  ierr = propagateMtx(); if(ierr) return 2;
319  ierr = propagateMCS(); if(ierr) return 3;
320  ierr = propagateFitd(); if(ierr) return 4;
321  ierr = propagateError(); if(ierr) return 5;
322 
323  }
324  mState = StiTrackNode::kTNReady;
325 
326 // Save parameters for future,
327 // when target node will became parent one
328  mSavdParentPars = mTargetNode->mFP;
329  mSavdDelta = (mTargetNode->isValid())? mTargetNode->mFE.getDelta():3e33;
330 
331  if (!mParentNode) {
332  if (!smooth) mgCutStep = 0;
333  mPredErrs = mTargetNode->mFE;
334  ierr = mPredErrs.check(); if (ierr) return 11;
335  mPredPars = mTargetNode->mFP;
336  ierr = mPredPars.check(); if (ierr) return 12;
337  mBestPars = mPredPars;
338  mBestDelta = mPredErrs.getDelta();
339  mJoinPars = mPredPars;
340  resetError(mNodeErrFactor);
341  }
342 // Set fitted pars to predicted for the absent hit case
343  mFitdPars = mPredPars;
344  mFitdErrs = mPredErrs;
345 
346  int ians = 1;
347  mChi2 =0;
348  do {//technical (fake) loop
349  if (!mHit) break;
350  setHitErrs();
351  if (nudge()) return 13;
352  mChi2 = 3e33;
353  double chi2 = evalChi2();
354  if (mTargetNode == mVertexNode) {
355  if (chi2>mChi2Vtx) return 14;
356  } else {
357  if (chi2>mChi2Max) break;
358  }
359  mChi2 = chi2; if (mChi2>999) mChi2=999;
360  ians = updateNode();
361  if (debug() & 8) { LOG_DEBUG << Form("%5d ",ians); StiKalmanTrackNode::PrintStep();}
362  if (!ians) break;
363  if (mTargetNode == mVertexNode) return 15;
364  mState = StiTrackNode::kTNReady;
365  mFitdPars = mPredPars;
366  mFitdErrs = mPredErrs;
367  mChi2 = 3e33;
368  }while(0);
369 
370  ierr = (!smooth)? save():join();
371  if (ierr) return 16;
372 
373 
374  do { //fake loop
375  if (!smooth) break;
376  if (!mHit) break;
377  if (mDetector && (!mFlipFlopNode || mTargetNode->mFlipFlop > mFlipFlopNode->mFlipFlop))
378  {mFlipFlopNode=mTargetNode;}
379  if(mState!=StiTrackNode::kTNFitEnd) break;
380  mUsed++;
381  if (mDetector && (!mWorstNode || mChi2>mWorstNode->getChi2()))
382  {mWorstNode=mTargetNode;}
383  if (!mParentNode) break;
384  double accu,errr;
385  accu = mJoinPars.ptin() - mBestParentPars.ptin()*(1+mMcs._ptinCorr);
386  errr = sqrt(0.5*(fabs(mJoinErrs._cPP)+fabs(mBestParentErrs._cPP)));
387  if (errr > 0) mCurvQa.add(accu/errr);
388  accu = mJoinPars.tanl() - mBestParentPars.tanl();
389  errr = sqrt(0.5*(fabs(mJoinErrs._cTT)+fabs(mBestParentErrs._cTT)));
390  if (errr > 0) mTanlQa.add(accu/errr);
391  }while(0);
392 
393  return 0;
394 }
395 
396 //______________________________________________________________________________
397 int StiTrackNodeHelper::join()
398 {
399 
400  enum {kOLdValid=1,kNewFitd=2,kJoiUnFit=4};
401 // if (!mParentNode) return 0;
402 
403 
404  int ierr = 0;
405  double chi2;
406 
407  int kase = mTargetNode->isValid();
408  if (mState==StiTrackNode::kTNFitEnd) kase |=kNewFitd;
409  do {
410  switch(kase) {
411  case 0: // Old invalid & New UnFitd
412  mChi2 = (mHit)? 3e33:0;
413  mState = StiTrackNode::kTNReady;
414 
415  case kNewFitd: // Old invalid & New Fitd
416  mJoinPars = mFitdPars;
417  mJoinErrs = mFitdErrs;
418  kase = -1;
419  break;
420 
421 // case kOLdValid|kNewFitd|kJoiUnFit: // Old valid & New Fitd & Join UnFit
422 // mChi2 = 3e33;
423 // mFitdPars = mPredPars;
424 // mFitdErrs = mPredErrs;
425 // mState = StiTrackNode::kTNReady;
426 //
427  case kOLdValid:; // Old valid & New UnFitd
428  case kOLdValid|kNewFitd:; // Old valid & New Fitd
429 
430  mTargetNode->mPE().recov();
431  mFitdErrs.recov();
432  chi2 = joinTwo(kNPars,mTargetNode->mPP().P,mTargetNode->mPE().G()
433  ,kNPars,mFitdPars.P ,mFitdErrs.G()
434  ,mJoinPars.P ,mJoinErrs.G());
435  mJoinPars.hz() = mTargetHz;
436  mJoinErrs.recov();
437 
438 
439  if (kase == (kOLdValid|kNewFitd)) { //Check errors improvements
440  if (mHrr.hYY <= mJoinErrs._cYY) {
441  LOG_DEBUG << Form("StiTrackNodeHelper::updateNode() WRONG hYY(%g) < nYY(%g)"
442  ,mHrr.hYY,mFitdErrs._cYY)<< endm;
443  return -13;
444  }
445  if (mHrr.hZZ <= mJoinErrs._cZZ) {
446  LOG_DEBUG << Form("StiTrackNodeHelper::updateNode() WRONG hZZ(%g) < nZZ(%g)"
447  ,mHrr.hZZ,mFitdErrs._cZZ) << endm;
448  return -14;
449  }
450  } //End Check errors improvements
451  mJoinPars.ready();
452  ierr = mJoinPars.check(); if (ierr) return 2;
453  if (kase!=(kOLdValid|kNewFitd)) {kase = -1; break;}
454 
455  mChi2 = 3e33;
456 // chi2 = joinChi2(); //Test join Chi2
457  chi2 = recvChi2(); //Test join Chi2
458  mChi2 = 3e33;
459  if (chi2>mChi2Max && mTargetNode!=mVertexNode)
460  { kase |=kJoiUnFit; return 99;} //join Chi2 too big
461  mChi2 = (chi2>999)? 999:chi2;
462  mState = StiTrackNode::kTNFitEnd;
463  kase = -1; break;
464 
465  default: assert(0);
466  }//end Switch
467  } while(kase>=0);
468 
469  if (std::fabs(mJoinPars.hz() - mTargetHz) > 1e-10)
470  {
471  LOG_WARN << "Expected |mJoinPars.hz() - mTargetHz| <= 1e-10 "
472  << "instead |" << mJoinPars.hz() << " - " << mTargetHz << "| = "
473  << std::fabs(mJoinPars.hz() - mTargetHz) << ". "
474  << "Will set mJoinPars.hz to " << mTargetHz << endm;
475  mJoinPars.hz() = mTargetHz;
476  }
477 
478  assert(fabs(mTargetNode->getHz()-mTargetHz)<=1e-10);
479 
480 
481  mTargetNode->mFE = mJoinErrs;
482  mTargetNode->mPE() = mJoinErrs;
483  mTargetNode->mFP = mJoinPars;
484  mTargetNode->mPP() = mJoinPars;
485  mTargetNode->mHrr = mHrr;
486  if (mTargetNode!=mVertexNode) mTargetNode->mUnTouch = mUnTouch;
487  mTargetNode->_state = mState;
488  if (mHit && ((mChi2<1000) != (mTargetNode->getChi2()<1000))) mTargetNode->mFlipFlop++;
489  if (mTargetNode!=mVertexNode) mTargetNode->setChi2(mChi2);
490 
491 // Sanity check
492  double myMaxChi2 = (mTargetNode==mVertexNode)? 1000:mChi2Max;
493  if (mState == StiTrackNode::kTNFitEnd) {
494  assert(mChi2 <myMaxChi2);
495  } else {
496  assert(mChi2 <=0 || mChi2 >=myMaxChi2);
497  }
498 
499  return 0;
500 }
501 //______________________________________________________________________________
502 double StiTrackNodeHelper::joinTwo(int nP1,const double *P1,const double *E1
503  ,int nP2,const double *P2,const double *E2
504  , double *PJ, double *EJ)
505 {
506 
507  assert(nP1<=nP2);
508  int nE1 = nP1*(nP1+1)/2;
509  int nE2 = nP2*(nP2+1)/2;
510  TArrayD ard(nE2*6);
511  double *a = ard.GetArray();
512  double *sumE = (a);
513  double *sumEI = (a+=nE2);
514  double *e1sumEIe1 = (a+=nE2);
515  double *subP = (a+=nE2);
516  double *sumEIsubP = (a+=nE2);
517  double chi2=3e33,p,q;
518 
519 // Choose the smalest errors
520  const double *p1 = P1, *p2 = P2, *e1 = E1, *e2 = E2, *t;
521  double choice = (nP1==nP2)? 0:1;
522  if (!choice ) {
523  for (int i=0,n=1;i<nE2;i+=(++n)) {
524  p=fabs(e1[i]);q=fabs(e2[i]);choice += (p-q)/(p+q+1e-10);
525  }}
526  if ( choice >0) {t = p2; p2 = p1; p1 = t; t = e2; e2 = e1; e1 = t;}
527 
528  do {//empty loop
529 // Join errors
530  TCL::vadd(e1,e2,sumE,nE1);
531  int negati = sumE[2]<0;
532  if (negati) TCL::vcopyn(sumE,sumE,nE1);
533  int ign0re = sumE[0]<=0;
534  if (ign0re) sumE[0] = 1;
535  TCL::trsinv(sumE,sumEI,nP1);
536  if (ign0re) {sumE[0] = 0; sumEI[0] = 0;}
537  if (negati) {TCL::vcopyn(sumE,sumE,nE1);TCL::vcopyn(sumEI,sumEI,nE1);}
538  TCL::vsub(p2 ,p1 ,subP ,nP1);
539  TCL::trasat(subP,sumEI,&chi2,1,nP1);
540  if (!EJ) break;
541  TCL::trqsq (e1 ,sumEI,e1sumEIe1,nP2);
542  TCL::vsub(e1,e1sumEIe1,EJ,nE2);
543  } while(0);
544 // Join params
545  if (PJ) {
546  TCL::tras(subP ,sumEI,sumEIsubP,1,nP1);
547  TCL::tras(sumEIsubP,e1 ,PJ ,1,nP2);
548  TCL::vadd(PJ ,p1 ,PJ ,nP2);
549  }
550  return chi2;
551 }
552 #if 0
553 //______________________________________________________________________________
554 double StiTrackNodeHelper::joinVtx(const double *P1,const double *E1
555  ,const double *P2,const double *E2
556  , double *PJ, double *EJ)
557 {
558  enum {nP1=3,nE1=6,nP2=kNPars,nE2=kNErrs};
559  double E1m[nE1],E2m[nE2];
560 
561  TCL::vzero(E1m ,nE1);
562  TCL::ucopy(E2,E2m ,nE2);
563  TCL::vadd (E1,E2m,E2m,nE1);
564  return joinTwo(nP1,P1,E1m,nP2,P2,E2m,PJ,EJ);
565 
566 }
567 #endif//0
568 #if 0
569 //______________________________________________________________________________
570 double StiTrackNodeHelper::joinVtx(const double *P1,const double *E1
571  ,const double *P2,const double *E2
572  , double *PJ, double *EJ)
573 {
574  enum {nP1=3,nE1=6,nP2=kNPars,nE2=kNErrs};
575 
576  TArrayD ard(nE2*7);
577  double *p = ard.GetArray();
578  double *sumE = (p);
579  double *sumEI = (p+=nE2);
580  double *sumEI_E1_sumEI = (p+=nE2);
581  double *E2_sumEI_E2 = (p+=nE2);
582  double *E2_sumEI_E1_sumEI_E2 = (p+=nE2);
583  double *sumEIsubP = (p+=nE2);
584  double *subP = (p+=nE2);
585 
586  double chi2=3e33;
587 
588 
589  do {//empty loop
590 // Join errors
591  TCL::ucopy(E2,sumE,nE1);
592  int ign0re = sumE[0]<=0;
593  if (ign0re) sumE[0] = 1;
594  TCL::trsinv(sumE,sumEI,nP1);
595  if (ign0re) {sumE[0] = 0; sumEI[0] = 0;}
596  TCL::vsub(P1 ,P2 ,subP ,nP1);
597  TCL::trasat(subP,sumEI,&chi2,1,nP1);
598  if (!EJ) break;
599  TCL::trqsq (E2 ,sumEI,E2_sumEI_E2,nP2);
600  TCL::vsub(E2,E2_sumEI_E2,EJ,nE2);
601 // Now account errors of vertex itself
602  TCL::trqsq (sumEI,E1,sumEI_E1_sumEI,nP1);
603  TCL::trqsq (E2,sumEI_E1_sumEI,E2_sumEI_E1_sumEI_E2,nP2);
604  TCL::vadd(EJ,E2_sumEI_E1_sumEI_E2,EJ,nE2);
605 
606  } while(0);
607 // Join params
608  if (PJ) {
609  TCL::tras(subP ,sumEI,sumEIsubP,1,nP1);
610  TCL::tras(sumEIsubP,E2 ,PJ ,1,nP2);
611  TCL::vadd(PJ ,P2 ,PJ ,nP2);
612  }
613  return chi2;
614 }
615 #endif//1
616 //______________________________________________________________________________
617 double StiTrackNodeHelper::joinVtx(const double *Y,const StiHitErrs &B
618  ,const StiNodePars &X,const StiNodeErrs &A
619  , StiNodePars *M, StiNodeErrs *C)
620 {
621 // Y : x,y,z of vertex. B: error matrix of Y
622 // X : track parameters. A: error matrix of X
623 // M : track parameters of X+Y. C: error matrix of M
624 
625 
626  enum {nP1=3,nE1=6,nP2=6,nE2=21};
627 
628  StiNodeErrs Ai=A; //Inverted A
629 
630  Ai._cXX=1;
631  TCL::trsinv(Ai.G(),Ai.G(),nP2);
632  Ai._cXX=0;
633 
634 
635  double Ai11i[6],Ai10[3][3],T10[3][3],dif[6],m[6];
636  Ai.get11(Ai11i);
637  Ai.get10(Ai10[0]);
638  TCL::trsinv(Ai11i ,Ai11i,3);
639  TCL::trsa (Ai11i,Ai10[0],T10[0],3,3); //Ai11*Ai10
640  TCL::ucopy(Y,m,3);
641  TCL::vsub (X.P,Y,dif,3);
642  TCL::mxmpy(T10[0],dif,m+3,3,3,1);
643  TCL::vadd(X.P+3,m+3,m+3,3); //m: resulting params
644  if (M) {TCL::ucopy(m,M->P,nP2); M->ready();} //fill resulting params
645 
646  TCL::vsub(X.P,m,dif,nP2); //dif = X - M
647  double chi2;
648  TCL::trasat(dif,Ai.G(),&chi2,1,nP2); //calc chi2
649  if (!C) return chi2;
650  // Error matrix calculation
651  C->reset();
652 
653  double TX[nP1][nP2];memset(TX[0],0,sizeof(TX));
654  for (int i=0;i<3;i++) {TCL::ucopy(T10[i],TX[i],3); TX[i][i+3]=1;}
655  double C11[nE1];
656  TCL::trasat(TX[0],A.G(),C11,nP1,nP2);
657  C->set11(C11);
658 
659  double TY[nP2][nP1];memset(TY[0],0,sizeof(TX));
660  for (int i=0;i<3;i++) {TCL::vcopyn(T10[i],TY[i+3],3); TY[i][i]=1;}
661  TY[0][0] = 0;
662  double CYY[nE2];
663  TCL::trasat(TY[0],B.G(),CYY,nP2,nP1);
664  TCL::vadd(CYY,C->G(),C->G(),nE2);
665  return chi2;
666 }
667 //______________________________________________________________________________
668 int StiTrackNodeHelper::save()
669 {
670  assert(fabs(mPredPars.hz()-mTargetHz)<=1e-10);
671  assert(fabs(mFitdPars.hz()-mTargetHz)<=1e-10);
672  assert(fabs(mTargetNode->getHz()-mTargetHz)<=1e-10);
673 
674  mTargetNode->mPP() = mPredPars;
675  mTargetNode->mFP = mFitdPars;
676  mTargetNode->mPE() = mPredErrs;
677  mTargetNode->mFE = mFitdErrs;
678  mTargetNode->_state = mState;
679  if (mHit && ((mChi2<1000) != (mTargetNode->getChi2()<1000))) mTargetNode->mFlipFlop++;
680  if (mTargetNode!=mVertexNode) mTargetNode->setChi2(mChi2);
681  return 0;
682 }
683 //______________________________________________________________________________
692 //______________________________________________________________________________
693 int StiTrackNodeHelper::propagateMCS()
694 {
695  mMcs.reset();
696  if (!mDetector) return 0;
697  mMcs._ptinCorr = 0;
698  if (fabs(mBestPars.ptin())<=1e-3) return 0;
699  double pt = 1./(fabs(mBestPars.ptin())+1e-6);
700 assert(pt<1e3);
701  double relRadThickness;
702  // Half path length in previous node
703  double pL1=0,pL2=0,pL3=0,d1=0,d2=0,d3=0,dxEloss=0,dx=0;
704  pL1=0.5*pathIn(mParentNode->getDetector(),&mBestParentPars);
705  // Half path length in this node
706  pL3=0.5*pathIn(mDetector,&mBestPars);
707  // Gap path length
708  double t = mBestPars.tanl();
709  pL2= fabs(dl*sqrt(1+t*t));
710  double x0p = 1e11,x0Gas=1e11,x0=1e11;
711  dx = mBestPars.x() - mBestParentRotPars.x();
712  double tanl = mBestPars.tanl();
713  double pti = mBestPars.ptin();
714  double p2 = (1.+tanl*tanl)*pt*pt;
715  double m = StiKalmanTrackFinderParameters::instance()->getMassHypothesis();
716  double m2 = m*m;
717  double e2 = p2+m2;
718  double beta2 = p2/e2;
719 
720  const StiMaterial *curMat = mDetector->getMaterial();
721  const StiElossCalculator *curLos = curMat->getElossCalculator();
722  d3 =(curLos) ? curLos->calculate(1.,m, beta2):0;
723  x0 = curMat->getX0();
724  const StiMaterial *curGas = mDetector->getGas();
725 
726 
727  const StiDetector *preDet = mParentNode->getDetector();
728  const StiMaterial *preMat = curGas,*preGas=curGas;
729  const StiElossCalculator *preLos = 0;
730  if (preDet) {
731  preMat = preDet->getMaterial();
732  preGas = preDet->getGas();
733  if (preMat) {
734  preLos = preMat->getElossCalculator();
735  x0p = preMat->getX0();
736  if (preLos) {
737  d1 = preLos->calculate(1.,m, beta2);
738  } } }
739 // Gas is UNDER detector
740  const StiMaterial *gasMat = (dx>0)?curGas : preGas;
741  if (gasMat) {
742  x0Gas = gasMat->getX0();
743  const StiElossCalculator *gasLos = gasMat->getElossCalculator();
744  if (gasLos) {
745  d2 = gasLos->calculate(1.,m, beta2);
746  } }
747 
748 
749  pL2=pL2-pL1-pL3; if (pL2<0) pL2=0;
750  relRadThickness = pL1/x0p+pL2/x0Gas+pL3/x0;
751 
752  dxEloss = d1*pL1+ d2*pL2 + d3*pL3;
753 
754  double theta2 = StiKalmanTrackNode::mcs2(relRadThickness,beta2,p2);
755  double cos2Li = (1.+ tanl*tanl); // 1/cos(lamda)**2
756  double f = mHitsErrFactor;
757  mMcs._cEE = cos2Li *theta2*f;
758  mMcs._cPP = tanl*tanl*pti*pti *theta2*f;
759  mMcs._cTP = pti*tanl*cos2Li *theta2*f;
760  mMcs._cTT = cos2Li*cos2Li *theta2*f;
761 assert(mMcs._cPP>=0);
762 assert(mMcs._cTT>=0);
763 assert(mMcs._cEE>=0);
764 
765 
766  int sign = ( dx>=0)? 1:-1;
767  double dE = sign*dxEloss;
768 // save detLoss and gasLoss for investigation only
769  StiELoss *el = mTargetNode->getELoss();
770  el[0].mELoss = 2*d3*pL3;
771  el[0].mLen = 2*pL3;
772  el[0].mDens = curMat->getDensity();
773  el[0].mX0 = x0;
774  el[1].mELoss = 2*d2*pL2;
775  el[1].mLen = 2*pL2;
776  el[1].mDens = gasMat->getDensity();
777  el[1].mX0 = x0Gas;
778 
779  mMcs._ptinCorr = ::sqrt(e2)*dE/p2;
780  if (fabs(mMcs._ptinCorr)>0.1) mMcs._ptinCorr = (dE<0)? -0.1:0.1;
781 
782 
783 
784  return 0;
785 }
786 //______________________________________________________________________________
800 //______________________________________________________________________________
801 double StiTrackNodeHelper::evalChi2()
802 {
803  double r00, r01,r11,chi2;
804  //If required, recalculate the errors of the detector hits.
805  //Do not attempt this calculation for the main vertex.
806  if (fabs(mPredPars._sinCA)>0.99 ) return 1e41;
807  if (fabs(mPredPars.eta()) >kMaxEta) return 1e41;
808  if (fabs(mPredPars.curv()) >kMaxCur) return 1e41;
809  if (!mDetector) { //Primay vertex
810  mHitPars[0] = mPredPars.x();
811  chi2 = joinVtx(mHitPars,mHrr,mPredPars,mPredErrs);
812  } else { //Normal hit
813 
814  r00=mPredErrs._cYY+mHrr.hYY;
815  r01=mPredErrs._cZY+mHrr.hZY;
816  r11=mPredErrs._cZZ+mHrr.hZZ;
817  mDetm = r00*r11 - r01*r01;
818  if (mDetm<r00*r11*1.e-5) {
819  LOG_DEBUG << Form("StiTrackNodeHelper::evalChi2 *** zero determinant %g",mDetm)<< endm;
820  return 1e60;
821  }
822  double tmp=r00; r00=r11; r11=tmp; r01=-r01;
823 
824  double dyt=(mPredPars.y()-mHitPars[1]);
825  double dzt=(mPredPars.z()-mHitPars[2]);
826  chi2 = (dyt*r00*dyt + 2*r01*dyt*dzt + dzt*r11*dzt)/mDetm;
827  }
828  return chi2;
829 }
830 //______________________________________________________________________________
831 /*
832  * double StiTrackNodeHelper::joinChi2()
833  * {
834  * double chi2;
835  * double mergPars[3],mHitPars[3],mergErrs[6];
836  * chi2 = joinTwo(3,mPredPars.P ,mPredErrs.A
837  * ,3,mTargetNode->mPP().P,mTargetNode->mPE().A
838  * ,mergPars ,mergErrs);
839  *
840  * mHitPars[0] = mHit->x();
841  * mHitPars[1] = mHit->y();
842  * mHitPars[2] = mHit->z();
843  * chi2 = joinTwo(3,mergPars,mergErrs,3,mHitPars,mHrr.A);
844  * // Save untouched by current hit node's y,z & errors for alignment
845  * mUnTouch.mPar[0] = mergPars[1];
846  * mUnTouch.mPar[1] = mergPars[2];
847  * mUnTouch.mErr[0] = mergErrs[2];
848  * mUnTouch.mErr[1] = mergErrs[4];
849  * mUnTouch.mErr[2] = mergErrs[5];
850  * return chi2;
851  * }
852  */
853 //______________________________________________________________________________
854 double StiTrackNodeHelper::recvChi2()
855 {
856  if (fabs(mJoinPars._sinCA)>0.99 ) return 1e41;
857  if (fabs(mJoinPars.eta()) >kMaxEta) return 1e41;
858  if (fabs(mJoinPars.curv()) >kMaxCur) return 1e41;
859  if (!mDetector) {//Primary vertex
860  double chi2 =joinVtx(mHitPars,mHrr ,mPredPars ,mPredErrs );
861  return chi2;
862  }
863 
864  StiHitErrs myHrr = mHrr;
865  StiNodeErrs recovErrs;
866  StiNodePars recovPars;
867  double f = -(1./mHitsErrFactor);
868  myHrr*=f;
869  double r11,r12,r22;
870  if ((r11=myHrr.hYY+mJoinErrs._cYY) >=0) return 1e41;
871  if ((r22=myHrr.hZZ+mJoinErrs._cZZ) >=0) return 1e41;
872  r12 =myHrr.hZY+mJoinErrs._cZY;
873  if (r11*r22-r12*r12<0) return 1e41;
874 
875 
876  double chi2 = joinTwo(3,mHitPars , myHrr.G()
877  ,3,mJoinPars.P,mJoinErrs.G()
878  ,recovPars.P,recovErrs.G());
879 
880  mUnTouch.set(recovPars,recovErrs);
881  return -chi2; //account that result is negative
882 }
883 //______________________________________________________________________________
884 int StiTrackNodeHelper::setHitErrs()
885 {
886  getHitErrors(mHit,&mFitdPars,&mHrr);
887  mHrr*=mHitsErrFactor;
888  return 0;
889 }
890 //______________________________________________________________________________
910 //______________________________________________________________________________
911 int StiTrackNodeHelper::updateNode()
912 {
913  mState = StiTrackNode::kTNFitBeg;
914  double r00,r01,r11;
915  if (!mDetector) { //Primary vertex
916  mHitPars[0] = mPredPars.x();
917  double chi2 = joinVtx(mHitPars,mHrr,mPredPars,mPredErrs,&mFitdPars,&mFitdErrs);
918  mFitdPars.curv() = mTargetHz*mFitdPars.ptin();
919  assert(chi2>900 || fabs(mChi2-chi2)<1e-10);
920  if (debug()) {
921  StiKalmanTrackNode::ResetComment(Form("Vertex "));
922  }
923  } else { //Normal Hit
924  r00=mHrr.hYY+mPredErrs._cYY;
925  r01=mHrr.hZY+mPredErrs._cZY;
926  r11=mHrr.hZZ+mPredErrs._cZZ;
927  mDetm=r00*r11 - r01*r01;
928  if (!(mDetm>(r00*r11)*1.e-5)) return 99;
929  assert(mDetm>(r00*r11)*1.e-5);
930 
931  // inverse matrix
932  double tmp=r00; r00=r11/mDetm; r11=tmp/mDetm; r01=-r01/mDetm;
933  // update error matrix
934  double k00=mPredErrs._cYY*r00+mPredErrs._cZY*r01, k01=mPredErrs._cYY*r01+mPredErrs._cZY*r11;
935  double k10=mPredErrs._cZY*r00+mPredErrs._cZZ*r01, k11=mPredErrs._cZY*r01+mPredErrs._cZZ*r11;
936  double k20=mPredErrs._cEY*r00+mPredErrs._cEZ*r01, k21=mPredErrs._cEY*r01+mPredErrs._cEZ*r11;
937  double k30=mPredErrs._cPY*r00+mPredErrs._cPZ*r01, k31=mPredErrs._cPY*r01+mPredErrs._cPZ*r11;
938  double k40=mPredErrs._cTY*r00+mPredErrs._cTZ*r01, k41=mPredErrs._cTY*r01+mPredErrs._cTZ*r11;
939 
940  double myY = mPredPars.y();
941  double myZ = mPredPars.z();
942  double dyt = mHitPars[1] - myY;
943  double dzt = mHitPars[2] - myZ;
944  double dPt = k30*dyt + k31*dzt;
945  double dEt = k20*dyt + k21*dzt;
946  double dTa = k40*dyt + k41*dzt;
947  double eta = NICE(mPredPars.eta() + dEt);
948  double pti = mPredPars.ptin() + dPt;
949  double tanl = mPredPars.tanl() + dTa;
950  // Check if any of the quantities required to pursue the update
951  // are infinite. If so, it means the tracks cannot be update/propagated
952  // any longer and should therefore be abandoned. Just return. This is
953  // not a big but rather a feature of the fact a helicoidal tracks!!!
954  // update Kalman state
955  double p0 = myY + k00*dyt + k01*dzt;
956  double p1 = myZ + k10*dyt + k11*dzt;
957  //mPredPars._tanl += k40*dyt + k41*dzt;
958  double sinCA = sin(eta);
959  // The following test introduces a track propagation error but happens
960  // only when the track should be aborted so we don't care...
961 
962  mFitdPars.hz() = mTargetHz;
963  mFitdPars.x() = mPredPars.x();
964  mFitdPars.y() = p0;
965  mFitdPars.z() = p1;
966  mFitdPars.eta() = eta;
967  mFitdPars.ptin() = pti;
968  mFitdPars.curv() = mTargetHz*pti;
969  mFitdPars.tanl() = tanl;
970  mFitdPars._sinCA = sinCA;
971  mFitdPars._cosCA = ::sqrt((1.-mFitdPars._sinCA)*(1.+mFitdPars._sinCA));
972  if (!mDetector)
973  assert(fabs(mFitdPars.y()-mHitPars[1])>1e-10 || fabs(mHitPars[0])<4);
974  assert(mFitdPars.x()>0);
975  if (mFitdPars.check()) return -11;
976  // update error matrix
977  double c00=mPredErrs._cYY;
978  double c10=mPredErrs._cZY, c11=mPredErrs._cZZ;
979  double c20=mPredErrs._cEY, c21=mPredErrs._cEZ;//, c22=mPredErrs._cEE;
980  double c30=mPredErrs._cPY, c31=mPredErrs._cPZ;//, c32=mPredErrs._cPE, c33=mPredErrs._cPP;
981  double c40=mPredErrs._cTY, c41=mPredErrs._cTZ;//, c42=mPredErrs._cTE, c43=mPredErrs._cTP, c44=mPredErrs._cTT;
982  mFitdErrs._cYY-=k00*c00+k01*c10;
983  mFitdErrs._cZY-=k10*c00+k11*c10;mFitdErrs._cZZ-=k10*c10+k11*c11;
984  mFitdErrs._cEY-=k20*c00+k21*c10;mFitdErrs._cEZ-=k20*c10+k21*c11;mFitdErrs._cEE-=k20*c20+k21*c21;
985  mFitdErrs._cPY-=k30*c00+k31*c10;mFitdErrs._cPZ-=k30*c10+k31*c11;mFitdErrs._cPE-=k30*c20+k31*c21;mFitdErrs._cPP-=k30*c30+k31*c31;
986  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;
987  }
988  mFitdErrs.recov(0);
989 
990 
991  if (mFitdErrs.check()) return -12;
992 // mFitdErrs.recov();
993 
994 
995 static int ERRTEST=0;
996 if(ERRTEST) errTest(mPredPars,mPredErrs,mHit,mHrr,mFitdPars,mFitdErrs,mChi2);
997 
998 //prod assert(mHrr.hYY > mFitdErrs._cYY);
999 //prod assert(mHrr.hZZ > mFitdErrs._cZZ);
1000  if (mDetector) { //Not a primary
1001  if (mHrr.hYY <= mFitdErrs._cYY) {
1002  LOG_DEBUG << Form("StiTrackNodeHelper::updateNode() WRONG hYY(%g) < nYY(%g)"
1003  ,mHrr.hYY,mFitdErrs._cYY)<< endm;
1004  return -13;
1005  }
1006  if (mHrr.hZZ <= mFitdErrs._cZZ) {
1007  LOG_DEBUG << Form("StiTrackNodeHelper::updateNode() WRONG hZZ(%g) < nZZ(%g)"
1008  ,mHrr.hZZ,mFitdErrs._cZZ)<< endm;
1009  return -14;
1010  }
1011  } //EndIf Not a primary
1012  if (debug()) StiKalmanTrackNode::comment += Form(" chi2 = %6.2f",mChi2);
1013  if (mTargetNode && debug()) {
1014  mTargetNode->PrintpT("U");
1015  }
1016  mState = StiTrackNode::kTNFitEnd;
1017  return 0;
1018 }
1019 
1020 //______________________________________________________________________________
1021 void StiTrackNodeHelper::resetError(double fk)
1022 {
1023  if (fk) do {//fake loop
1024  if(mPredErrs._cYY>DY*DY) break;
1025  if(mPredErrs._cZZ>DZ*DZ) break;
1026  if(mPredErrs._cEE>DEta*DEta) break;
1027  if(mPredErrs._cPP>DPti*DPti) break;
1028  if(mPredErrs._cTT>DTan*DTan) break;
1029  mPredErrs*=fk;
1030  return;
1031  }while(0);
1032 
1033  mPredErrs.reset();
1034  mPredErrs._cYY=DY*DY;
1035  mPredErrs._cZZ=DZ*DZ;
1036  mPredErrs._cEE=DEta*DEta;
1037  mPredErrs._cPP=DPti*DPti;
1038  mPredErrs._cTT=DTan*DTan;
1039 }
1040 //______________________________________________________________________________
1041 int StiTrackNodeHelper::cutStep(StiNodePars *pars,StiNodePars *base)
1042 {
1043  double fact=1,dif,fak;
1044  double cuts[kNPars];
1045  memcpy(cuts,MAXSTEP,sizeof(cuts));
1046  if (mBestDelta<DY) {cuts[1]=mBestDelta;cuts[2]=mBestDelta;}
1047 
1048  for (int jx=0;jx<kNPars;jx++) {
1049  dif =(fabs((*pars)[jx]-(*base)[jx]));
1050  fak = (dif >cuts[jx]) ? cuts[jx]/dif:1;
1051  if (fak < fact) fact = fak;
1052  }
1053  if (fact>=1) return 0;
1054  mgCutStep++;
1055  for (int jx=0;jx<kNPars;jx++) {
1056  dif =(*pars)[jx]-(*base)[jx];
1057  (*pars)[jx] = (*base)[jx] +dif*fact;
1058  }
1059  pars->ready();
1060  return 1;
1061 }
1062 //______________________________________________________________________________
1063 int StiTrackNodeHelper::nudge()
1064 {
1065  if(!mHit) return 0;
1066  StiNodePars *pars = &mBestPars;
1067  for (int i=0;i<2;i++,pars =&mPredPars) {
1068  double deltaX = mHitPars[0]-pars->x();
1069  if (fabs(deltaX) <1e-6) continue;
1070  double deltaL = deltaX/pars->_cosCA;
1071  double deltaE = pars->curv()*deltaL;
1072  pars->x() = mHitPars[0];
1073  pars->y() += pars->_sinCA *deltaL;
1074  pars->z() += pars->tanl() *deltaL;
1075  pars->eta() += deltaE;
1076  double cosCA = pars->_cosCA;
1077  pars->_cosCA -= pars->_sinCA *deltaE;
1078  pars->_sinCA += cosCA *deltaE;
1079  if (fabs(pars->_cosCA)>=0.99
1080  ||fabs(pars->_sinCA)>=0.99) pars->ready();
1081  if (pars->check()) return 1;
1082  }
1083  return 0;
1084 }
1085 //______________________________________________________________________________
1086 double StiTrackNodeHelper::pathIn(const StiDetector *det,StiNodePars *pars)
1087 {
1088  if (!det) return 0.;
1089  double thickness = det->getShape()->getThickness();
1090  double t = pars->tanl();
1091  double c = fabs(pars->_cosCA);
1092  if (det->getShape()->getShapeCode()!=kPlanar) {
1093  double CA = pars->eta()-atan2(pars->y(),pars->x());
1094  c = cos(CA);
1095  }
1096  if (fabs(c)<kMinCos) return 0.;
1097 // if (fabs(c)<kMinCos) c=kMinCos;
1098  return (thickness*::sqrt(1.+t*t)) / fabs(c);
1099 }
1100 //______________________________________________________________________________
1101 int StiTrackNodeHelper::getHitErrors(const StiHit *hit,const StiNodePars *pars,StiHitErrs *hrr)
1102 {
1103  hrr->reset();
1104  const StiDetector *det = hit->detector();
1105  const StiHitErrorCalculator *calc = (det)? det->getHitErrorCalculator():0;
1106  if (calc) {//calculate it
1107  calc->calculateError(pars,hrr->hYY,hrr->hZZ);
1108  } else {//get from hit
1109  const float *ermx = hit->errMtx();
1110  for (int i=0;i<6;i++){hrr->G()[i]=ermx[i];}
1111  }
1112  return (!det);
1113 }
1114 //______________________________________________________________________________
1115 int errTest(StiNodePars &predP,StiNodeErrs &predE,
1116  const StiHit *hit ,StiHitErrs &hitErr,
1117  StiNodePars &fitdP,StiNodeErrs &fitdE, double chi2)
1118 {
1119 
1120  StiNodePars mineP,hittP;
1121  StiNodeErrs mineE,hittE;
1122  hittP.x() = hit->x();
1123  hittP.y() = hit->y();
1124  hittP.z() = hit->z();
1125  memcpy(hittE.G(),hitErr.G(),sizeof(StiNodeErrs));
1126 
1127  double myChi2 = StiTrackNodeHelper::joinTwo(
1128  3,hittP.P,hittE.G(),
1129  6,predP.P,predE.G(),
1130  mineP.P,mineE.G());
1131 
1132 
1133  int ndif = 0;
1134  for (int i=0;i<kNPars;i++) {
1135  double diff = fabs(mineP.P[i]-fitdP.P[i]);
1136  if (diff < 1e-10) continue;
1137  diff/=0.5*(fabs(mineP.P[i])+fabs(fitdP.P[i]));
1138  if (diff < 1e-5 ) continue;
1139  ndif++;
1140  LOG_DEBUG << Form("errTest(P): %g(%d) - %g(%d) = %g",mineE.G()[i],i,fitdE.G()[i],i,diff)<< endm;
1141  }
1142  if (ndif){ mineP.print();fitdP.print();}
1143 
1144  for (int i=0;i<kNErrs;i++) {
1145  double diff = fabs(mineE.G()[i]-fitdE.G()[i]);
1146  if (diff < 1e-10) continue;
1147  diff/=0.5*(fabs(mineE.G()[i])+fabs(fitdE.G()[i]));
1148  if (diff < 1e-5 ) continue;
1149  ndif+=100;
1150  LOG_DEBUG << Form("errTest(E): %g(%d) - %g(%d) = %g",mineE.G()[i],i,fitdE.G()[i],i,diff)<< endm;
1151  }
1152  if (ndif>=100){ mineE.print();fitdE.print();}
1153 
1154  double diff = fabs((chi2-myChi2)/(chi2+myChi2));
1155  if (diff > 1e-5 ) {
1156  ndif+=1000;
1157  LOG_DEBUG << Form("errTest(C): %g() - %g() = %g",myChi2,chi2,diff)<< endm;
1158  }
1159 
1160  return ndif;
1161 }
1162 
1163 //_____________________________________________________________________________
1164 void QaFit::add(double val)
1165 {
1166  double v = val; double p = mPrev; mPrev = v;
1167  int n = (mTally)? 2:1;
1168  mTally++;
1169  for (int i=0;i<n;i++) {
1170  mAver[i] +=v;
1171  mErrr[i] +=v*v;
1172  if (mMaxi[i]<fabs(v)) mMaxi[i]=fabs(v);
1173  if (v<0) mNega[i]++;
1174  v = v*p;
1175  }
1176 }
1177 //_____________________________________________________________________________
1178 void QaFit::finish()
1179 {
1180  if( mEnded) return;
1181  if(!mTally) return;
1182  mEnded = 1;
1183  int n = mTally;
1184  for (int i=0;i<2;i++) {
1185  mAver[i]/= n;
1186  mErrr[i]/= n;
1187  if (mErrr[i]<1e-10) mErrr[i]=1e-10;
1188  mErrr[i] = sqrt(mErrr[i]);
1189  if (!(--n)) break;
1190  }
1191 }
1192 //_____________________________________________________________________________
1193 double QaFit::getAccu(int k)
1194 {
1195  finish();
1196  if (mTally-k<=1) return 0;
1197  return mErrr[k];
1198 }
1199 //_____________________________________________________________________________
1200 double QaFit::getNStd(int k)
1201 {
1202  finish();
1203  int n = mTally-k;
1204  if (n <= 0) return 0;
1205  return mAver[k]*sqrt(double(n));
1206 }
1207 //_____________________________________________________________________________
1208 double QaFit::getNSgn(int k)
1209 {
1210  finish();
1211  int n = mTally-k;
1212  if (n <= 0) return 0;
1213  return (n-2*mNega[k])/sqrt(double(n));
1214 }
1215 //_____________________________________________________________________________
1216 void QaFit::getInfo(double *info)
1217 {
1218  for (int i=0;i<2;i++) {
1219  int l = i*4;
1220  info[l+0]=getAccu(i);
1221  info[l+1]=getNStd(i);
1222  info[l+2]=getNSgn(i);
1223  info[l+3]=getMaxi(i);
1224  }
1225 }
1226 
1227 
1228 
StiHit
Definition: StiHit.h:51
StiHit::x
const Float_t & x() const
Return the local x, y, z values.
Definition: StiHit.h:67
StiHit::detector
const StiDetector * detector() const
Definition: StiHit.h:96
StiTrackNode::errPropag6
static void errPropag6(double G[21], const double F[6][6], int nF)
Definition: StiTrackNode.cxx:46
StiHitErrorCalculator::calculateError
virtual void calculateError(Double_t _z, Double_t _eta, Double_t _tanl, Double_t &ecross, Double_t &edip, Double_t fudgeFactor=1) const
coeff[6] = 0:intrinsicY 1: driftY 2: crossY 3:intrinsicZ 4: driftZ 5: crossZ
Definition: StiChairs.cxx:7
StiMaterial::getElossCalculator
StiElossCalculator * getElossCalculator() const
Get Eloss calculator.
Definition: StiMaterial.cxx:102
TCL::trsa
static float * trsa(const float *s, const float *a, float *b, int m, int n)
Definition: TCernLib.cxx:1111
TCL::trqsq
static float * trqsq(const float *q, const float *s, float *r, int m)
Definition: TCernLib.cxx:1045
StiElossCalculator::calculate
double calculate(double charge2, double m, double beta2) const
Definition: StiElossCalculator.cxx:56
TCL::trsinv
static float * trsinv(const float *g, float *gi, int n)
Definition: TCernLib.cxx:1160
TCL::trasat
static float * trasat(const float *a, const float *s, float *r, int m, int n)
Definition: TCernLib.cxx:540
StiNodePars::_cosCA
double _cosCA
sine and cosine of cross angle
Definition: StiNodePars.h:51
TCL::tras
static float * tras(const float *a, const float *s, float *b, int m, int n)
Definition: TCernLib.cxx:470
StiMaterial::getX0
double getX0() const
Get the radiation length in centimeter.
Definition: StiMaterial.h:37
StiKalmanTrackNode::mFE
StiNodeErrs mFE
covariance matrix of the track parameters
Definition: StiKalmanTrackNode.h:284
StiMaterial::getDensity
double getDensity() const
Get the material density in grams/cubic centimeter.
Definition: StiMaterial.h:35
Generated by   doxygen 1.8.5