StRoot  1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
StiTrackNode.cxx
1 #include <stdio.h>
2 #include <stdlib.h>
3 #include "StiTrackNode.h"
4 #include "TMath.h"
5 #include "TCernLib.h"
6 #include "StMessMgr.h"
7 #include "StiUtilities/StiDebug.h"
8 
9 #define NICE(a) ( ((a) <= -M_PI)? ((a)+2*M_PI) :\
10  ((a) > M_PI)? ((a)-2*M_PI) : (a))
11 
12 int StiTrackNode::mgFlag=0;
13  static const int idx66[6][6] =
14  {{ 0, 1, 3, 6,10,15},{ 1, 2, 4, 7,11,16},{ 3, 4, 5, 8,12,17}
15  ,{ 6, 7, 8, 9,13,18},{10,11,12,13,14,19},{15,16,17,18,19,20}};
16 static const double MAX1ERR[]={3,3,3,0.1,3.,0.1};
17 static const double MAX2ERR[]={MAX1ERR[0]*MAX1ERR[0]
18  ,MAX1ERR[1]*MAX1ERR[1]
19  ,MAX1ERR[2]*MAX1ERR[2]
20  ,MAX1ERR[3]*MAX1ERR[3]
21  ,MAX1ERR[4]*MAX1ERR[4]
22  ,MAX1ERR[5]*MAX1ERR[5]};
23 
24 static const double MIN1ERR[]={1e-4,1e-4,1e-4,1e-5,1e-3,1e-5};
25 static const double MIN2ERR[]={MIN1ERR[0]*MIN1ERR[0]
26  ,MIN1ERR[1]*MIN1ERR[1]
27  ,MIN1ERR[2]*MIN1ERR[2]
28  ,MIN1ERR[3]*MIN1ERR[3]
29  ,MIN1ERR[4]*MIN1ERR[4]
30  ,MIN1ERR[5]*MIN1ERR[5]};
31 static const double recvCORRMAX = 0.99999;
32 static const double chekCORRMAX = 0.99999;
33 static double MAXPARS[]={500,500,500,3.15,100,100};
34 
35 
46 void StiTrackNode::errPropag6( double G[21],const double F[6][6],int nF )
47 {
48  enum {NP=6,NE=21};
49 
50  double g[NE]; memcpy(g, G,sizeof( g));
51  double fg[NP][NP]; memset(fg[0],0,sizeof(fg));
52 
53  for (int i=0;i<nF;i++) {
54  for (int j=0;j<nF;j++) {
55  if (!F[i][j]) continue;
56  for (int k=0;k<NP;k++) {
57  int jk = idx66[j][k];
58  if (!g[jk]) continue;
59  fg[i][k] += F[i][j]*g[jk];
60  }}}
61 
62  for (int i=0;i<NP;i++) {
63  for (int k=i;k<NP;k++) {
64  int ik = idx66[i][k];
65  double s = 0;
66  for (int j=0;j<NP;j++) {
67  if (!F[k][j]) continue;
68  s += fg[i][j]*F[k][j];
69  }
70  G[ik] += (s + fg[i][k] + fg[k][i]);
71  }}
72 
73 }
74 
75 //______________________________________________________________________________
76 void StiHitContino::reset()
77 {
78  memset(this,0,sizeof(StiHitContino));
79  mChi2[0]=1e61;
80 }
81 //______________________________________________________________________________
82 void StiHitContino::add(StiHit *hit,double chi2,double detr)
83 {
84  int i=0;
85  for (;i<kMaxSize;i++) {
86  if (!mHits[i]) break;
87  if (chi2 > mChi2[i]) continue;
88  for (int jr = kMaxSize-2;jr>=i;jr--)
89  {mHits[jr+1]=mHits[jr]; mChi2[jr+1]=mChi2[jr];mDetr[jr+1]=mDetr[jr];}
90  break;
91  }
92  if (i>=kMaxSize) return;
93  mHits[i]=hit; mChi2[i]=chi2;mDetr[i]=detr;
94 }
95 //______________________________________________________________________________
96 void StiHitContino::print(const char* tit) const
97 {
98  if (!tit || !*tit) tit ="print()";
99  int n=getNHits();
100  LOG_DEBUG << Form(" **** StiHitContino::%s nHits=%d",tit,n)<< endm;
101  for (int i=0;i<n;i++) {
102  LOG_DEBUG << Form("%3d - hit=%p chi2 = %g",i,(void*)mHits[i],mChi2[i]);}
103  LOG_DEBUG << endm;
104 }
105 //______________________________________________________________________________
106 int StiHitContino::getNHits() const
107 { int n=0; for(int i=0;i<kMaxSize;i++) {if (mHits[i]) n++;}; return n;}
108 //______________________________________________________________________________
109 int StiTrackNode::cylCross(const double Xp[2],const double Dp[2], const double Rho
110  ,const double rp ,int dir, double out[2][3])
111 {
112 //Circles crossing
113 //==========================================================
114 #define DOT(a,b) a[0]*b[0]+a[1]*b[1]
115 #define MAG2(a) DOT(a,a)
116 #define MAG(a) sqrt(MAG2(a))
117 // Rho -curvature
118 // r - cyl radius
119 //L - distance between centers
120 //d - distance of crossing line d<r
121 // X0,Y0 start track with R
122 // Dx,Dy direction of it
123 // Nx,Ny = -Dy,Dx
124 // Cx,Cy = direction to center
126 //
127 // r**2-d**2 == R**2-(L-d)**2
128 // r**2 == R**2- L**2 +2*L*d
129 //
130 // r**2 +L**2-R**2= 2*L*d
131 //
132 // d = (r**2 +L**2-R**2)/(2*L)
133 //
134 // L**2 = (X0+N*R)**2 = X0**2+R**2 +2*(X0*N)*R
135 // L**2-R**2 = X0**2+ 2*(X0*N)*R
136 //
137 //
138 // d = (r**2 +X0**2+ 2*(X0*N)*R)/(2*sqrt(X0**2+R**2 +2*(X0*N)*R)
139 // d = (r**2 +X0**2+ 2*(X0*N)*R)/(2*sqrt(X0**2+R**2 +2*(X0*N)*R)
140 //
141 //
142 static int nCall=0;nCall++;
143 StiDebug::Break(nCall);
144  double r = rp;
145  int sRho = (Rho<0) ? -1:1;
146  double aRho = fabs(Rho), d=0;
147 
148 //TVector3 D(Dp[0],Dp[1],0.),X(Xp[0],Xp[1],0.);
149 // static TVector3 C, Cd, Cn, N;
150  double C[2], Cd[2], Cn[2], N[2];
151  //D.SetXYZ( Dp[0], Dp[1], 0.0 );
152  // X.SetXYZ( Xp[0], Xp[1], 0.0 );
153 
154  double XX,XN,L;
155  N[0] = -Dp[1];
156  N[1] = Dp[0];
157  XX = MAG2(Xp); //X*X;
158  XN = DOT(Xp,N);//X*N;
159 
160  double LLmRR = XX*aRho+2*XN*sRho;
161  double LL = LLmRR*aRho+1; L = sqrt(LL);
162  d = (r*r*aRho+LLmRR)/(2*L);
163  double P = ((r-d)*(r+d));
164  if (P<=0) {
165  P = 0;
166  r = fabs(LLmRR/(L+1));
167  d = (r*r*aRho+LLmRR)/(2*L);
168  } else {
169  P = sqrt(P);
170  }
171 
172  C[0] = Xp[0]*aRho+N[0]*sRho;
173  C[1] = Xp[1]*aRho+N[1]*sRho;
174 
175  //Cd = C.Unit();
176  double CMag = MAG(C);
177  Cd[0] = C[0] / CMag;
178  Cd[1] = C[1] / CMag;
179 
180  Cn[0] = -Cd[1];
181  Cn[1] = Cd[0];
182 
183 
184  // static TVector3 Out[2];
185  //for (int ix = 0;ix<2; ix++) {
186  // Out[ix] = Cd*d + Cn*p; p = -p;
187  // }
188  double Out[2][2];
189  Out[0][0] = Cd[0]*d + Cn[0]*P;
190  Out[0][1] = Cd[1]*d + Cn[1]*P;
191  Out[1][0] = Cd[0]*d - Cn[0]*P;
192  Out[1][1] = Cd[1]*d - Cn[1]*P;
193 
194  // static TVector3 tmp;
195  double tmp[2];
196 for (int ix = 0;ix<2; ix++) {
197  tmp[0] = Out[ix][0] - Xp[0];
198  tmp[1] = Out[ix][1] - Xp[1];
199  //double len = (Out[ix]-X).Mag();
200  double len = MAG(tmp);
201  double lenaRho =len*aRho; if (lenaRho>2) lenaRho = 1.999;
202  if (lenaRho > 0.01) len = 2*asin(0.5*lenaRho)/aRho;
203  //if ((Out[ix]-X).Dot(D)<0) len = -len;
204  double tst = tmp[0]*Dp[0] + tmp[1]*Dp[1];
205  if ( tst < 0 ) len = -len;
206 
207  //double tst = (X-Out[ix])*D;
208  //if (dir) tst = -tst;
209 //VP if (tst<0) len = M_PI*2*aR-len;
210  out[ix][2] = len;
211  out[ix][0] = Out[ix][0];
212  out[ix][1] = Out[ix][1];
213 }
214  if ((out[0][2])>out[1][2]) { //wrong order
215  for (int j=0;j<3;j++) {
216  double t=out[0][j]; out[0][j] = out[1][j]; out[1][j] = t;
217  } }
218 
219 
220 #if 0
221  TVector3 tC(C[0],C[1],0.);
222  for (int i=0;i<2;i++) {
223 // // printf("x=%g y=%g len=%g\n",out[i][0],out[i][1],out[i][2]);
224  TVector3 Out(out[i][0],out[i][1],0.);
225  double dif = (Out*aRho-tC).Mag()-1.;
226 // // printf("SolAcc=%g\n",dif);
227  assert(fabs(dif)<1e-5);
228  dif = Out.Mag()/r-1;
229 // // printf("SolAcc=%g\n",dif);
230  assert(fabs(dif)<1e-5);
231  }
232 #endif
233  return (P)? 2:0;
234 }
235 
236 
237 //______________________________________________________________________________
238 
239 double StiTrackNode::sinX(double x)
240 {
241  double x2 = x*x;
242  if (x2>0.5) return (sin(x)-x)/x2/x;
243  double nom = -1./6;
244  double sum = nom;
245  for (int it=4;1;it+=2) {
246  nom = -nom*x2/(it*(it+1));
247  sum +=nom;
248  if (fabs(nom) <= 1e-10*fabs(sum)) break;
249  }
250  return sum;
251 }
252 //______________________________________________________________________________
253 void StiTrackNode::mult6(double Rot[kNPars][kNPars],const double Pro[kNPars][kNPars])
254 {
255  double T[kNPars][kNPars];
256 
257  if (!Rot[0][0]) {memcpy(Rot[0],Pro[0],sizeof(T)); return;}
258 
259  memcpy(T[0],Pro[0],sizeof(T));
260 
261  for (int i=0;i<kNPars;i++) {
262  for (int j=0;j<kNPars;j++) {
263  if(!Rot[i][j]) continue;
264  for (int k=0;k<kNPars;k++) {
265  if (!Pro[k][i]) continue;
266  T[k][j] += Pro[k][i]*Rot[i][j];
267  }}}
268  for (int i=0;i<kNPars;i++) {
269  for (int k=0;k<kNPars;k++) {
270  Rot[i][k] += T[i][k];
271 }}
272 }
273 //______________________________________________________________________________
274 double StiTrackNode::getRefPosition() const
275 {
276  if(!_detector) {
277  return x();
278  } else {
279  StiPlacement * place = _detector->getPlacement();
280  assert(place);
281  return place->getLayerRadius();
282  }
283 }
284 //______________________________________________________________________________
285  double StiTrackNode::getLayerAngle() const
286 {
287  assert(_detector);
288  StiPlacement * place = _detector->getPlacement();
289  assert(place);
290  return place->getLayerAngle();
291 }
292 
293 //______________________________________________________________________________
294 double StiNodeErrs::operator()(int i,int j) const
295 {
296  return G()[idx66[i][j]];
297 }
298 //______________________________________________________________________________
299 StiNodeErrs &StiNodeErrs::merge(double wt,StiNodeErrs &other)
300 {
301  double wt0 = 1.-wt;
302  for (int i=0;i<kNErrs;i++) {G()[i] = wt0*G()[i] + wt*other.G()[i];}
303 
304  return *this;
305 }
306 //______________________________________________________________________________
307 void StiNodeErrs::get00( double *a) const
308 {
309  memcpy(a,G(),6*sizeof(double));
310 }
311 //______________________________________________________________________________
312 void StiNodeErrs::set00(const double *a)
313 {
314  memcpy(G(),a,6*sizeof(double));
315 
316 }
317 //______________________________________________________________________________
318 void StiNodeErrs::get10(double *a) const
319 {
320 // 0: 00
321 // 1: 10 11
322 // 3: 20 21 22
323 // 6: 30 31 32 33
324 //10: 40 41 42 43 44
325 //15: 50 51 52 53 54 55
326  const double *A = G();
327  memcpy(a+0,A+ 6,3*sizeof(double));
328  memcpy(a+3,A+10,3*sizeof(double));
329  memcpy(a+6,A+15,3*sizeof(double));
330 }
331 //______________________________________________________________________________
332 void StiNodeErrs::set10(const double *a)
333 {
334  double *A = G();
335  memcpy(A+ 6,a+0,3*sizeof(double));
336  memcpy(A+10,a+3,3*sizeof(double));
337  memcpy(A+15,a+6,3*sizeof(double));
338 }
339 //______________________________________________________________________________
340 void StiNodeErrs::get11( double *a) const
341 {
342  const double *A = G();
343  memcpy(a+0,A+ 9,1*sizeof(double));
344  memcpy(a+1,A+13,2*sizeof(double));
345  memcpy(a+3,A+18,3*sizeof(double));
346 }
347 //______________________________________________________________________________
348 void StiNodeErrs::set11(const double *a)
349 {
350  double *A = G();
351  memcpy(A+ 9,a+0,1*sizeof(double));
352  memcpy(A+13,a+1,2*sizeof(double));
353  memcpy(A+18,a+3,3*sizeof(double));
354 }
355 //______________________________________________________________________________
356 void StiNodeErrs::zeroX()
357 {
358  double *A = G();
359  for (int i=0;i<kNPars;i++) {A[idx66[i][0]]=0;}
360 }
361 //____________________________________________________________
362 void StiNodeErrs::rotate(double alpha,const StiNodePars &pars)
363 {
364 // it is rotation by -alpha
365 
366  double *A = G();
367  double ca = cos(alpha),sa=sin(alpha);
368  double dX = (fabs(pars._cosCA)<1e-5)? 1e-5:pars._cosCA;
369  double dYdX = pars._sinCA/dX;
370  double dZdX = pars.tanl()/dX;
371 
372  double F[6][6]= {{ -1, 0,0,0,0,0}
373  ,{-ca*dYdX-sa,-sa*dYdX+ca-1,0,0,0,0}
374  ,{-ca*dZdX ,-sa*dZdX ,0,0,0,0}
375  ,{ 0, 0,0,0,0,0}
376  ,{ 0, 0,0,0,0,0}
377  ,{ 0, 0,0,0,0,0}};
378 
379  StiTrackNode::errPropag6( A,F,kNPars );
380  for (int i=0,li=0;i<kNPars ;li+=++i) {
381  assert(fabs(A[li+0]) <1e-6);
382  }
383 
384 }
385 //______________________________________________________________________________
386 void StiNodeErrs::recov(int force)
387 {
388 static int nCall = 0; nCall++;
389 StiDebug::Break(nCall);
390  double *A = G();
391 
392  int i0=1,li0=1,isMod=0;
393  if (_cXX>0) {i0=0;li0=0;}
394 
395  double dia[kNPars],fak[kNPars]={1,1,1,1,1,1},corrMax=1;;
396  for (int i=i0,li=li0;i<kNPars ;li+=++i) {
397  double &aii = A[li+i];
398  if (aii < MIN2ERR[i]) aii = MIN2ERR[i];
399  if (aii > MAX2ERR[i]) { fak[i] = sqrt(MAX2ERR[i]/aii); aii = MAX2ERR[i]; isMod=2014;}
400  dia[i] = aii;
401  for (int j=i0;j<i;j++) {
402  double &aij = A[li+j];
403  if (isMod) aij*=fak[i]*fak[j];
404  if (aij*aij <= dia[i]*dia[j]*chekCORRMAX) continue;
405  double qwe = aij*aij/(dia[i]*dia[j]);
406  if (corrMax>=qwe) continue;
407  corrMax=qwe;
408  } }
409  if (corrMax>=chekCORRMAX) {
410  corrMax = sqrt(corrMax/recvCORRMAX);
411  for (int i=i0,li=li0;i<kNPars ;li+=++i) {
412  for (int j=i0;j<i;j++) {
413  A[li+j]/=corrMax;
414  } } }
415 
416  while (((force)? sign():zign())<=0) {
417  for (int i=i0,li=li0;i<kNPars ;li+=++i) {
418  for (int j=i0;j<i;j++) {
419  A[li+j]*=0.9;
420  } } }
421 
422 }
423 //______________________________________________________________________________
424 void StiNodeErrs::print() const
425 {
426  const double *d = G();
427  for (int n=1;n<=6;n++) {
428  LOG_DEBUG << Form("%d - ",n);
429  for (int i=0;i<n;i++){LOG_DEBUG << Form("%g\t",*(d++));}; LOG_DEBUG << endm;
430  }
431 }
432 
433 //______________________________________________________________________________
434 int StiNodeErrs::check(const char *pri) const
435 {
436 
437  const double *A = G();
438  int i=-2008,j=2009,kase=0;
439  double aii=-20091005,ajj=-20101005,aij=-20111005;
440  int i0=0; if (!_cXX) i0 = 1;
441  for (i=i0;i<kNPars;i++) {
442  aii = A[idx66[i][i]];
443  if (aii<0) {kase = 1; break;} //Diagonal must be positive
444  }
445  if (kase) goto RETN;
446  for (i=i0;i<kNPars;i++) {
447  aii = A[idx66[i][i]];
448  for (j=i+1;j<kNPars ;j++) {
449  ajj = A[idx66[j][j]];
450  if (ajj<=0) continue;
451  aij = A[idx66[i][j]];
452  if ((aij*aij)> aii*ajj) {kase = 2; break;}
453  }
454  if (kase) break;
455  }
456 RETN:
457 
458  if (!kase) return kase;
459  if (!pri ) return kase;
460  switch(kase) {
461 
462  case 1: LOG_DEBUG << Form("StiNodeErrs::check(%s) FAILED: Negative diagonal %g[%d][%d]",pri,aii,i,i)<< endm;
463  break;
464  case 2: LOG_DEBUG << Form("StiNodeErrs::check(%s) FAILED: Correlation too big %g[%d][%d]>%g"
465  ,pri,aij,i,j,sqrt(aii*ajj))<<endm;
466  break;
467  case 3: LOG_DEBUG << Form("StiNodeErrs::check(%s) FAILED: Non Positive matrix",pri)<<endm;
468  }
469  return kase;
470 }
471 //____________________________________________________________
472 double StiNodeErrs::zign() const
473 {
474  const double *A = G();
475  double dia[kNPars];
476  double minCorr = 1e11;
477  for (int i=1,li=1;i<kNPars ;li+=++i) {
478  const double &aii = A[li+i];
479  if (aii<0) return aii;
480  dia[i] = aii;
481  for (int j=1;j<i;j++) {
482  const double &aij = A[li+j];
483  double dis = 1-(aij*aij)/(dia[i]*dia[j]);
484  if (dis<0) return dis;
485  if (dis<minCorr) minCorr = dis;
486 
487  } }
488  return minCorr;
489 }
490 //____________________________________________________________
491 double StiNodeErrs::sign() const
492 {
493  enum {n=kNPars};
494  double ans=3e33;
495  const double *a = G();
496  double *xx = (double *)G();
497  double save = *xx; if (!save) *xx = 1;
498  double B[kNErrs];
499  double *b = B;
500  // trchlu.F -- translated by f2c (version 19970219).
501  //
502  //see original documentation of CERNLIB package F112
503 
504  /* Local variables */
505  int ipiv, kpiv, i__, j;
506  double r__, dc;
507  int id, kd;
508  double sum;
509 
510 
511  /* CERN PROGLIB# F112 TRCHLU .VERSION KERNFOR 4.16 870601 */
512  /* ORIG. 18/12/74 W.HART */
513 
514 
515  /* Parameter adjuTments */
516  --b; --a;
517 
518  /* Function Body */
519  ipiv = 0;
520 
521  i__ = 0;
522 
523  do {
524  ++i__;
525  ipiv += i__;
526  kpiv = ipiv;
527  r__ = a[ipiv];
528 
529  for (j = i__; j <= n; ++j) {
530  sum = 0.;
531  if (i__ == 1) goto L40;
532  if (r__ == 0.) goto L42;
533  id = ipiv - i__ + 1;
534  kd = kpiv - i__ + 1;
535 
536  do {
537  sum += b[kd] * b[id];
538  ++kd; ++id;
539  } while (id < ipiv);
540 
541 L40:
542  sum = a[kpiv] - sum;
543 L42:
544  if (j != i__) b[kpiv] = sum * r__;
545  else {
546  if (sum<ans) ans = sum;
547  if (sum<=0.) goto RETN;
548  dc = sqrt(sum);
549  b[kpiv] = dc;
550  if (r__ > 0.) r__ = (double)1. / dc;
551  }
552  kpiv += j;
553  }
554 
555  } while (i__ < n);
556 
557 RETN: *xx=save;
558  return ans;
559 } /* trchlu_ */
560 //______________________________________________________________________________
561 int StiNodeErrs::nan() const
562 {
563  const double *A = G();
564  for (int i=0; i<kNPars;i++) {
565  if (!finite(A[i])) return 100+i;
566  }
567  return 0;
568 }
569 
570 //____________________________________________________________
571 double sign(const double *a,int n)
572 {
573  double ans=3e33;
574  double *aa = (double *)a;
575  double save = aa[0]; if (!save) aa[0] = 1;
576  double B[kNErrs];
577  double *b = B;
578  if (n>kNErrs) b = new double[n];
579 
580  // trchlu.F -- translated by f2c (version 19970219).
581  //
582  //see original documentation of CERNLIB package F112
583 
584  /* Local variables */
585  int ipiv, kpiv, i__, j;
586  double r__, dc;
587  int id, kd;
588  double sum;
589 
590 
591  /* CERN PROGLIB# F112 TRCHLU .VERSION KERNFOR 4.16 870601 */
592  /* ORIG. 18/12/74 W.HART */
593 
594 
595  /* Parameter adjuTments */
596  --b; --a;
597 
598  /* Function Body */
599  ipiv = 0;
600 
601  i__ = 0;
602 
603  do {
604  ++i__;
605  ipiv += i__;
606  kpiv = ipiv;
607  r__ = a[ipiv];
608 
609  for (j = i__; j <= n; ++j) {
610  sum = 0.;
611  if (i__ == 1) goto L40;
612  if (r__ == 0.) goto L42;
613  id = ipiv - i__ + 1;
614  kd = kpiv - i__ + 1;
615 
616  do {
617  sum += b[kd] * b[id];
618  ++kd; ++id;
619  } while (id < ipiv);
620 
621 L40:
622  sum = a[kpiv] - sum;
623 L42:
624  if (j != i__) b[kpiv] = sum * r__;
625  else {
626  if (sum<ans) ans = sum;
627  if (sum<=0.) goto RETN;
628  dc = sqrt(sum);
629  b[kpiv] = dc;
630  if (r__ > 0.) r__ = (double)1. / dc;
631  }
632  kpiv += j;
633  }
634 
635  } while (i__ < n);
636 
637 RETN: aa[0]=save;
638  if (n>kNErrs) delete [] b;
639  return ans;
640 } /* trchlu_ */
641 //______________________________________________________________________________
642 int StiNodePars::check(const char *pri) const
643 {
644 
645  int ierr=0;
646 //?? temp test
647  assert(fabs(_cosCA) <=1 && fabs(_sinCA)<=1);
648  double tmp = (fabs(curv())<1e-6)? 0: curv()-ptin()*hz();
649 // 1km for 1GeV is a zero field
650 // assert(fabs(_hz)<1e-5 || fabs(tmp)<= 1e-3*fabs(_curv));
651  if (fabs(hz())>=1e-5 && fabs(tmp)> 1e-3*fabs(curv())) {ierr=1313; goto FAILED;}
652  for (int i=0;i<kNPars;i++) {if (fabs(P[i]) > MAXPARS[i]) {ierr = i+1 ; break;}}
653  if(ierr) goto FAILED;
654 // for (int i=-2;i<0;i++) {if (fabs(P[i]) > 1.) {ierr = i+12; break;}}
655  if (fabs(_cosCA) > 1) {ierr = 12;}
656  if (fabs(_sinCA) > 1) {ierr = 13;}
657 FAILED:
658  if (!ierr) return ierr;
659  if (!pri ) return ierr;
660  LOG_DEBUG << Form("StiNodePars::check(%s) == FAILED(%d)",pri,ierr)<<endm;
661  print();
662  return ierr;
663 }
664 //______________________________________________________________________________
665 StiNodePars &StiNodePars::merge(double wt,StiNodePars &other)
666 {
667  double wt0 = 1.-wt;
668  for (int i=0;i<kNPars+1;i++) {P[i] = wt0*P[i] + wt*other.P[i];}
669  ready();
670  return *this;
671 }
672 //______________________________________________________________________________
673 StiNodePars &StiNodePars::operator=(const StiNodePars &fr)
674 {
675  assert(fabs(fr._sinCA)<=1);
676  assert(fabs(fr._cosCA)<=1);
677  memcpy (this,&fr,sizeof(fr));
678  return *this;
679 }
680 //______________________________________________________________________________
681 void StiNodePars::rotate(double alpha)
682 {
683 // actually it is rotation by -alpha
684 
685  double xt1=x();
686  double yt1=y();
687  double cosCA0 = _cosCA;
688  double sinCA0 = _sinCA;
689 
690  double ca = cos(alpha);
691  double sa = sin(alpha);
692 
693  x() = xt1*ca + yt1*sa;
694  y() = -xt1*sa + yt1*ca;
695  _cosCA = cosCA0*ca+sinCA0*sa;
696  _sinCA = -cosCA0*sa+sinCA0*ca;
697  double nor = 0.5*(_sinCA*_sinCA+_cosCA*_cosCA +1);
698  _cosCA /= nor;
699  _sinCA /= nor;
700  eta()= NICE(eta()-alpha);
701 }
702 //______________________________________________________________________________
703 int StiNodePars::nan() const
704 {
705  const double *d = &(_cosCA);
706  for (int i=-2; i<=kHz;i++) {
707  if (!finite(*(d++))) return 100+i;
708  }
709  return 0;
710 }
711 //______________________________________________________________________________
712 void StiNodePars::print() const
713 {
714 static const char* tit[]={"cosCA","sinCA","X","Y","Z","Eta","Ptin","TanL","Curv",0};
715  const double *d = P-2;
716  for (int i=-2;i<kNPars+1;i++) {LOG_DEBUG << Form("%s = %g, ",tit[i+2],*(d++));}
717  LOG_DEBUG << endm;
718 }
719 //______________________________________________________________________________
720 void StiHitErrs::rotate(double angle)
721 {
722  double t[2][2];
723  t[0][0] = cos(angle); t[0][1] = -sin(angle);
724  t[1][0] = -t[0][1] ; t[1][1] = t[0][0];
725  double r[3];
726  TCL::trasat(t[0],&hXX,r,2,2);
727  TCL::ucopy(r,&hXX,3);
728 }
729 //______________________________________________________________________________
730 void StiNode2Pars::set(const StiNodePars &pars,StiNodeErrs &errs)
731 {
732  mPar[0]=pars.y(); mPar[1]=pars.z();
733  mErr[0]=errs._cYY;
734  mErr[1]=errs._cZY;
735  mErr[2]=errs._cZZ;
736 }
737 
StiHit
Definition: StiHit.h:51
StiTrackNode::errPropag6
static void errPropag6(double G[21], const double F[6][6], int nF)
Definition: StiTrackNode.cxx:46
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
Generated by   doxygen 1.8.5