HLT/TPCLib/tracking-ca/AliHLTTPCCATrackParam.cxx

   1 // $Id$
   2 // **************************************************************************
   3 // This file is property of and copyright by the ALICE HLT Project          *
   4 // ALICE Experiment at CERN, All rights reserved.                           *
   5 //                                                                          *
   6 // Primary Authors: Sergey Gorbunov <sergey.gorbunov@kip.uni-heidelberg.de> *
   7 //                  Ivan Kisel <kisel@kip.uni-heidelberg.de>                *
   8 //                  for The ALICE HLT Project.                              *
   9 //                                                                          *
  10 // Permission to use, copy, modify and distribute this software and its     *
  11 // documentation strictly for non-commercial purposes is hereby granted     *
  12 // without fee, provided that the above copyright notice appears in all     *
  13 // copies and that both the copyright notice and this permission notice     *
  14 // appear in the supporting documentation. The authors make no claims       *
  15 // about the suitability of this software for any purpose. It is            *
  16 // provided "as is" without express or implied warranty.                    *
  17 //                                                                          *
  18 //***************************************************************************
  19
  20
  21 #include "AliHLTTPCCATrackParam.h"
  22 #include "AliHLTTPCCAMath.h"
  23 #include <iostream>
  24
  25 //
  26 // Circle in XY:
  27 //
  28 // R  = 1/TMath::Abs(Kappa);
  29 // Xc = X - sin(Phi)/Kappa;
  30 // Yc = Y + cos(Phi)/Kappa;
  31 //
  32
  33 GPUd() Float_t AliHLTTPCCATrackParam::GetDist2( const AliHLTTPCCATrackParam &t ) const
  34 {
  35   // get squared distance between tracks
  36
  37   Float_t dx = GetX() - t.GetX();
  38   Float_t dy = GetY() - t.GetY();
  39   Float_t dz = GetZ() - t.GetZ();
  40   return dx*dx + dy*dy + dz*dz;
  41 }
  42
  43 GPUd() Float_t AliHLTTPCCATrackParam::GetDistXZ2( const AliHLTTPCCATrackParam &t ) const
  44 {
  45   // get squared distance between tracks in X&Z
  46
  47   Float_t dx = GetX() - t.GetX();
  48   Float_t dz = GetZ() - t.GetZ();
  49   return dx*dx + dz*dz;
  50 }
  51
  52
  53 GPUd() void AliHLTTPCCATrackParam::ConstructXY3( const Float_t x[3], const Float_t y[3],
  54                                           const Float_t sigmaY2[3], Float_t CosPhi0 )
  55 {
  56   //* Construct the track in XY plane by 3 points
  57
  58   Float_t x0 = x[0];
  59   Float_t y0 = y[0];
  60   Float_t x1 = x[1] - x0;
  61   Float_t y1 = y[1] - y0;
  62   Float_t x2 = x[2] - x0;
  63   Float_t y2 = y[2] - y0;
  64
  65   Float_t a1 = x1*x1 + y1*y1;
  66   Float_t a2 = x2*x2 + y2*y2;
  67   Float_t a = 2*(x1*y2 - y1*x2);
  68   Float_t lx =  a1*y2 - a2*y1;
  69   Float_t ly = -a1*x2 + a2*x1;
  70   Float_t l = CAMath::Sqrt(lx*lx + ly*ly);
  71
  72   Float_t li = 1./l;
  73   Float_t li2 = li*li;
  74   Float_t li3 = li2*li;
  75   Float_t cosPhi = ly*li;
  76
  77   Float_t sinPhi = -lx*li;
  78   Float_t kappa = a/l;
  79
  80   Float_t dlx = a2 - a1;     //  D lx / D y0
  81   Float_t dly = -a;          //  D ly / D y0
  82   Float_t dA  = 2*(x2 - x1); // D a  / D y0
  83   Float_t dl = (lx*dlx + ly*dly)*li;
  84
  85   // D sinPhi,kappa / D y0
  86
  87   Float_t d0[2] = { -(dlx*ly-lx*dly)*ly*li3, (dA*l-a*dl)*li2 };
  88
  89   // D sinPhi,kappa / D y1
  90
  91   dlx = -a2 + 2*y1*y2;
  92   dly = -2*x2*y1;
  93   dA  = -2*x2;
  94   dl = (lx*dlx + ly*dly)*li;
  95
  96   Float_t d1[2] = { -(dlx*ly-lx*dly)*ly*li3, (dA*l-a*dl)*li2 };
  97
  98   // D sinPhi,kappa / D y2
  99
 100   dlx = a1 - 2*y1*y2;
 101   dly = -2*x1*y2;
 102   dA  = 2*x1;
 103   dl = (lx*dlx + ly*dly)*li;
 104
 105   Float_t d2[2] = { -(dlx*ly-lx*dly)*ly*li3, (dA*l-a*dl)*li2 };
 106
 107   if( CosPhi0*cosPhi <0 ){
 108     cosPhi = -cosPhi;
 109     sinPhi = -sinPhi;
 110     kappa = -kappa;
 111     d0[0] = -d0[0];
 112     d0[1] = -d0[1];
 113     d1[0] = -d1[0];
 114     d1[1] = -d1[1];
 115     d2[0] = -d2[0];
 116     d2[1] = -d2[1];
 117   }
 118
 119   SetX( x0 );
 120   SetY( y0 );
 121   SetSinPhi( sinPhi );
 122   SetKappa( kappa );
 123   SetCosPhi( cosPhi );
 124
 125   Float_t s0 = sigmaY2[0];
 126   Float_t s1 = sigmaY2[1];
 127   Float_t s2 = sigmaY2[2];
 128
 129   fC[0] = s0;
 130   fC[1] = 0;
 131   fC[2] = 100.;
 132
 133   fC[3] = d0[0]*s0;
 134   fC[4] = 0;
 135   fC[5] = d0[0]*d0[0]*s0 + d1[0]*d1[0]*s1 + d2[0]*d2[0]*s2;
 136
 137   fC[6] = 0;
 138   fC[7] = 0;
 139   fC[8] = 0;
 140   fC[9] = 100.;
 141
 142   fC[10] = d0[1]*s0;
 143   fC[11] = 0;
 144   fC[12] = d0[0]*d0[1]*s0 + d1[0]*d1[1]*s1 + d2[0]*d2[1]*s2;
 145   fC[13] = 0;
 146   fC[14] = d0[1]*d0[1]*s0 + d1[1]*d1[1]*s1 + d2[1]*d2[1]*s2;
 147 }
 148
 149
 150 GPUd() Float_t  AliHLTTPCCATrackParam::GetS( Float_t x, Float_t y ) const
 151 {
 152   //* Get XY path length to the given point
 153
 154   Float_t k  = GetKappa();
 155   Float_t ex = GetCosPhi();
 156   Float_t ey = GetSinPhi();
 157   x-= GetX();
 158   y-= GetY();
 159   Float_t dS = x*ex + y*ey;
 160   if( CAMath::Abs(k)>1.e-4 ) dS = CAMath::ATan2( k*dS, 1+k*(x*ey-y*ex) )/k;
 161   return dS;
 162 }
 163
 164 GPUd() void  AliHLTTPCCATrackParam::GetDCAPoint( Float_t x, Float_t y, Float_t z,
 165                                                  Float_t &xp, Float_t &yp, Float_t &zp ) const
 166 {
 167   //* Get the track point closest to the (x,y,z)
 168
 169   Float_t x0 = GetX();
 170   Float_t y0 = GetY();
 171   Float_t k  = GetKappa();
 172   Float_t ex = GetCosPhi();
 173   Float_t ey = GetSinPhi();
 174   Float_t dx = x - x0;
 175   Float_t dy = y - y0;
 176   Float_t ax = dx*k+ey;
 177   Float_t ay = dy*k-ex;
 178   Float_t a = sqrt( ax*ax+ay*ay );
 179   xp = x0 + (dx - ey*( (dx*dx+dy*dy)*k - 2*(-dx*ey+dy*ex) )/(a+1) )/a;
 180   yp = y0 + (dy + ex*( (dx*dx+dy*dy)*k - 2*(-dx*ey+dy*ex) )/(a+1) )/a;
 181   Float_t s = GetS(x,y);
 182   zp = GetZ() + GetDzDs()*s;
 183   if( CAMath::Abs(k)>1.e-2 ){
 184     Float_t dZ = CAMath::Abs( GetDzDs()*CAMath::TwoPi()/k );
 185     if( dZ>.1 ){
 186       zp+= CAMath::Nint((z-zp)/dZ)*dZ;
 187     }
 188   }
 189 }
 190
 191 GPUd() void AliHLTTPCCATrackParam::ConstructXYZ3( const Float_t p0[5], const Float_t p1[5],
 192                                            const Float_t p2[5],
 193                                            Float_t CosPhi0, Float_t t0[] )
 194 {
 195   //* Construct the track in XYZ by 3 points
 196
 197   Float_t px[3]   = { p0[0], p1[0], p2[0] };
 198   Float_t py[3]   = { p0[1], p1[1], p2[1] };
 199   Float_t pz[3]   = { p0[2], p1[2], p2[2] };
 200   Float_t ps2y[3] = { p0[3]*p0[3], p1[3]*p1[3], p2[3]*p2[3] };
 201   Float_t ps2z[3] = { p0[4]*p0[4], p1[4]*p1[4], p2[4]*p2[4] };
 202
 203   Float_t kold = t0 ?t0[4] :0;
 204   ConstructXY3( px, py, ps2y, CosPhi0 );
 205
 206   Float_t pS[3] = { GetS(px[0],py[0]), GetS(px[1],py[1]), GetS(px[2],py[2]) };
 207   Float_t k = Kappa();
 208   if( CAMath::Abs(k)>1.e-2 ){
 209     Float_t dS = CAMath::Abs( CAMath::TwoPi()/k );
 210     pS[1]+= CAMath::Nint( (pS[0]-pS[1])/dS )*dS; // not more than half turn
 211     pS[2]+= CAMath::Nint( (pS[1]-pS[2])/dS )*dS;
 212     if( t0 ){
 213       Float_t dZ = CAMath::Abs(t0[3]*dS);
 214       if( CAMath::Abs(dZ)>1. ){
 215         Float_t dsDz = 1./t0[3];
 216         if( kold*k<0 ) dsDz = -dsDz;
 217         Float_t s0 = (pz[0]-t0[1])*dsDz;
 218         Float_t s1 = (pz[1]-t0[1])*dsDz;
 219         Float_t s2 = (pz[2]-t0[1])*dsDz;
 220         pS[0]+= CAMath::Nint( (s0-pS[0])/dS )*dS ;
 221         pS[1]+= CAMath::Nint( (s1-pS[1])/dS )*dS ;
 222         pS[2]+= CAMath::Nint( (s2-pS[2])/dS )*dS ;
 223       }
 224     }
 225   }
 226
 227   Float_t s = pS[0] + pS[1] + pS[2];
 228   Float_t z = pz[0] + pz[1] + pz[2];
 229   Float_t sz = pS[0]*pz[0] + pS[1]*pz[1] + pS[2]*pz[2];
 230   Float_t ss = pS[0]*pS[0] + pS[1]*pS[1] + pS[2]*pS[2];
 231
 232   Float_t a = 3*ss-s*s;
 233   SetZ( (z*ss-sz*s)/a ); // z0
 234   SetDzDs( (3*sz-z*s)/a ); // t = dz/ds
 235
 236   Float_t dz0[3] = {ss - pS[0]*s,ss - pS[1]*s,ss - pS[2]*s };
 237   Float_t dt [3] = {3*pS[0] - s, 3*pS[1] - s, 3*pS[2] - s };
 238
 239   fC[2] = (dz0[0]*dz0[0]*ps2z[0] + dz0[1]*dz0[1]*ps2z[1] + dz0[2]*dz0[2]*ps2z[2])/a/a;
 240   fC[7]= (dz0[0]*dt [0]*ps2z[0] + dz0[1]*dt [1]*ps2z[1] + dz0[2]*dt [2]*ps2z[2])/a/a;
 241   fC[9]= (dt [0]*dt [0]*ps2z[0] + dt [1]*dt [1]*ps2z[1] + dt [2]*dt [2]*ps2z[2])/a/a;
 242 }
 243
 244
 245 GPUd() Int_t  AliHLTTPCCATrackParam::TransportToX( Float_t x, Float_t maxSinPhi )
 246 {
 247   //* Transport the track parameters to X=x
 248
 249   Float_t x0  = X();
 250   //Float_t y0  = Y();
 251   Float_t k   = Kappa();
 252   Float_t ex = CosPhi();
 253   Float_t ey = SinPhi();
 254   Float_t dx = x - x0;
 255
 256   Float_t ey1 = k*dx + ey;
 257   Float_t ex1;
 258   if( CAMath::Abs(ey1)>maxSinPhi ){ // no intersection
 259     return 0;
 260   }else{
 261     ex1 = CAMath::Sqrt(1 - ey1*ey1);
 262     if( ex<0 ) ex1 = -ex1;
 263   }
 264
 265   Float_t dx2 = dx*dx;
 266   Float_t ss = ey+ey1;
 267   Float_t cc = ex+ex1;
 268
 269   if( CAMath::Abs(cc)<1.e-4 || CAMath::Abs(ex)<1.e-4 || CAMath::Abs(ex1)<1.e-4 ) return 0;
 270
 271   Float_t tg = ss/cc; // tan((phi1+phi)/2)
 272
 273   Float_t dy = dx*tg;
 274   Float_t dl = dx*CAMath::Sqrt(1+tg*tg);
 275
 276   if( cc<0 ) dl = -dl;
 277   Float_t dSin = dl*k/2;
 278   if( dSin > 1 ) dSin = 1;
 279   if( dSin <-1 ) dSin = -1;
 280   Float_t dS = ( CAMath::Abs(k)>1.e-4)  ? (2*CAMath::ASin(dSin)/k) :dl;
 281   Float_t dz = dS*DzDs();
 282
 283
 284   Float_t cci = 1./cc;
 285   Float_t exi = 1./ex;
 286   Float_t ex1i = 1./ex1;
 287
 288   fCosPhi = ex1;
 289   fX += dx;
 290   fP[0]+= dy;
 291   fP[1]+= dz;
 292   fP[2] = ey1;
 293   fP[3] = fP[3];
 294   fP[4] = fP[4];
 295
 296   Float_t h2 = dx*(1+ey*ey1 + ex*ex1)*exi*ex1i*cci;
 297   Float_t h4 = dx2*(cc + ss*ey1*ex1i )*cci*cci;
 298
 299   Float_t c00 = fC[0];
 300   Float_t c10 = fC[1];
 301   Float_t c11 = fC[2];
 302   Float_t c20 = fC[3];
 303   Float_t c21 = fC[4];
 304   Float_t c22 = fC[5];
 305   Float_t c30 = fC[6];
 306   Float_t c31 = fC[7];
 307   Float_t c32 = fC[8];
 308   Float_t c33 = fC[9];
 309   Float_t c40 = fC[10];
 310   Float_t c41 = fC[11];
 311   Float_t c42 = fC[12];
 312   Float_t c43 = fC[13];
 313   Float_t c44 = fC[14];
 314
 315   //Float_t H0[5] = { 1,0, h2,  0, h4 };
 316   //Float_t H1[5] = { 0, 1, 0, dS,  0 };
 317   //Float_t H2[5] = { 0, 0, 1,  0, dx };
 318   //Float_t H3[5] = { 0, 0, 0,  1,  0 };
 319   //Float_t H4[5] = { 0, 0, 0,  0,  1 };
 320
 321
 322   fC[0]=( c00  + h2*h2*c22 + h4*h4*c44
 323           + 2*( h2*c20 + h4*c40 + h2*h4*c42 )  );
 324
 325   fC[1]= c10 + h2*c21 + h4*c41 + dS*(c30 + h2*c32 + h4*c43);
 326   fC[2]= c11 + 2*dS*c31 + dS*dS*c33;
 327
 328   fC[3]= c20 + h2*c22 + h4*c42 + dx*( c40 + h2*c42 + h4*c44);
 329   fC[4]= c21 + dS*c32 + dx*(c41 + dS*c43);
 330   fC[5]= c22 +2*dx*c42 + dx2*c44;
 331
 332   fC[6]= c30 + h2*c32 + h4*c43;
 333   fC[7]= c31 + dS*c33;
 334   fC[8]= c32 + dx*c43;
 335   fC[9]= c33;
 336
 337   fC[10]= c40 + h2*c42 + h4*c44;
 338   fC[11]= c41 + dS*c43;
 339   fC[12]= c42 + dx*c44;
 340   fC[13]= c43;
 341   fC[14]= c44;
 342
 343   return 1;
 344 }
 345
 346
 347 GPUd() Int_t  AliHLTTPCCATrackParam::TransportToX( Float_t x, AliHLTTPCCATrackParam &t0, Float_t maxSinPhi )
 348 {
 349   //* Transport the track parameters to X=x
 350
 351   Float_t x0  = t0.X();
 352   Float_t k   = t0.Kappa();
 353   Float_t ex = t0.CosPhi();
 354   Float_t ey = t0.SinPhi();
 355   Float_t dx = x - x0;
 356
 357   Float_t ey1 = k*dx + ey;
 358   Float_t ex1;
 359   if( CAMath::Abs(ey1)>maxSinPhi ){ // no intersection
 360     return 0;
 361   }else{
 362     ex1 = CAMath::Sqrt(1 - ey1*ey1);
 363     if( ex<0 ) ex1 = -ex1;
 364   }
 365
 366   Float_t dx2 = dx*dx;
 367   Float_t ss = ey+ey1;
 368   Float_t cc = ex+ex1;
 369
 370   if( CAMath::Abs(cc)<1.e-4 || CAMath::Abs(ex)<1.e-4 || CAMath::Abs(ex1)<1.e-4 ) return 0;
 371
 372   Float_t tg = ss/cc; // tan((phi1+phi)/2)
 373
 374   Float_t dy = dx*tg;
 375   Float_t dl = dx*CAMath::Sqrt(1+tg*tg);
 376
 377   if( cc<0 ) dl = -dl;
 378   Float_t dSin = dl*k/2;
 379   if( dSin > 1 ) dSin = 1;
 380   if( dSin <-1 ) dSin = -1;
 381   Float_t dS = ( CAMath::Abs(k)>1.e-4)  ? (2*CAMath::ASin(dSin)/k) :dl;
 382   Float_t dz = dS*t0.DzDs();
 383
 384
 385   Float_t cci = 1./cc;
 386   Float_t exi = 1./ex;
 387   Float_t ex1i = 1./ex1;
 388
 389   Float_t c00 = fC[0];
 390   Float_t c10 = fC[1];
 391   Float_t c11 = fC[2];
 392   Float_t c20 = fC[3];
 393   Float_t c21 = fC[4];
 394   Float_t c22 = fC[5];
 395   Float_t c30 = fC[6];
 396   Float_t c31 = fC[7];
 397   Float_t c32 = fC[8];
 398   Float_t c33 = fC[9];
 399   Float_t c40 = fC[10];
 400   Float_t c41 = fC[11];
 401   Float_t c42 = fC[12];
 402   Float_t c43 = fC[13];
 403   Float_t c44 = fC[14];
 404
 405   Float_t d[5] = { fP[0]-t0.fP[0],
 406                    fP[1]-t0.fP[1],
 407                    fP[2]-t0.fP[2],
 408                    fP[3]-t0.fP[3],
 409                    fP[4]-t0.fP[4] };
 410
 411   //Float_t H0[5] = { 1,0, h2,  0, h4 };
 412   //Float_t H1[5] = { 0, 1, 0, dS,  0 };
 413   //Float_t H2[5] = { 0, 0, 1,  0, dx };
 414   //Float_t H3[5] = { 0, 0, 0,  1,  0 };
 415   //Float_t H4[5] = { 0, 0, 0,  0,  1 };
 416
 417   Float_t h2 = dx*(1+ey*ey1 + ex*ex1)*exi*ex1i*cci;
 418   Float_t h4 = dx2*(cc + ss*ey1*ex1i )*cci*cci;
 419
 420   t0.SetCosPhi( ex1 );
 421   t0.SetX( t0.GetX() + dx );
 422   t0.SetY( t0.GetY() + dy );
 423   t0.SetZ( t0.Z() + dz );
 424   t0.SetSinPhi( ey1 );
 425
 426   fX = t0.X();
 427   fP[0] = t0.fP[0] + d[0]        + h2*d[2]          + h4*d[4];
 428   fP[1] = t0.fP[1]        + d[1]           + dS*d[3];
 429   fP[2] = t0.fP[2]               +d[2]              + dx*d[4];
 430
 431   //CosPhi() = CosPhi()>=0 ?CAMath::Sqrt(1-fP[2]*fP[2]) :-CAMath::Sqrt(1-fP[2]*fP[2]);
 432
 433   fC[0]=( c00  + h2*h2*c22 + h4*h4*c44
 434           + 2*( h2*c20 + h4*c40 + h2*h4*c42 )  );
 435
 436   fC[1]= c10 + h2*c21 + h4*c41 + dS*(c30 + h2*c32 + h4*c43);
 437   fC[2]= c11 + 2*dS*c31 + dS*dS*c33;
 438
 439   fC[3]= c20 + h2*c22 + h4*c42 + dx*( c40 + h2*c42 + h4*c44);
 440   fC[4]= c21 + dS*c32 + dx*(c41 + dS*c43);
 441   fC[5]= c22 +2*dx*c42 + dx2*c44;
 442
 443   fC[6]= c30 + h2*c32 + h4*c43;
 444   fC[7]= c31 + dS*c33;
 445   fC[8]= c32 + dx*c43;
 446   fC[9]= c33;
 447
 448   fC[10]= c40 + h2*c42 + h4*c44;
 449   fC[11]= c41 + dS*c43;
 450   fC[12]= c42 + dx*c44;
 451   fC[13]= c43;
 452   fC[14]= c44;
 453
 454   return 1;
 455 }
 456
 457
 458 GPUd() Int_t  AliHLTTPCCATrackParam::TransportToX0( Float_t x, Float_t sinPhi, Float_t cosPhi )
 459 {
 460   //* Transport the track parameters to X=x
 461
 462   Float_t ex = cosPhi;
 463   Float_t ey = sinPhi;
 464   Float_t dx = x - X();
 465
 466   if( CAMath::Abs(ex)<1.e-4 ) return 0;
 467   Float_t exi = 1./ex;
 468
 469   Float_t dl = dx*exi;
 470   Float_t dy = dl*ey;
 471   Float_t dS = dl;
 472   Float_t dz = dS*DzDs();
 473
 474
 475   Float_t h2 = dx*exi*exi*exi;
 476   Float_t h4 = dx*h2/2;
 477   Float_t h5 = dx;
 478
 479   Float_t c00 = fC[0];
 480   Float_t c10 = fC[1];
 481   Float_t c11 = fC[2];
 482   Float_t c20 = fC[3];
 483   Float_t c21 = fC[4];
 484   Float_t c22 = fC[5];
 485   Float_t c30 = fC[6];
 486   Float_t c31 = fC[7];
 487   Float_t c32 = fC[8];
 488   Float_t c33 = fC[9];
 489   Float_t c40 = fC[10];
 490   Float_t c41 = fC[11];
 491   Float_t c42 = fC[12];
 492   Float_t c43 = fC[13];
 493   Float_t c44 = fC[14];
 494
 495   //Float_t H0[5] = { 1,0, h2,  0, h4 };
 496   //Float_t H1[5] = { 0, 1, 0, dS,  0 };
 497   //Float_t H2[5] = { 0, 0, 1,  0, h5 };
 498   //Float_t H3[5] = { 0, 0, 0,  1,  0 };
 499   //Float_t H4[5] = { 0, 0, 0,  0,  1 };
 500
 501   Float_t ey1 = SinPhi() + h5*Kappa();
 502   //if( TMath::Abs(ey1)>maxSinPhi ){
 503   //std::cout<<" TransportToX0 error: sinPhi="<<ey<<" -> "<<ey1<<std::endl;
 504   //return 0;
 505   //}
 506   fX += dx ;
 507   fP[0]+= dy  + h2*(SinPhi()-ey) + h4*Kappa();
 508   fP[1]+= dz ;
 509   fP[2] = ey1;
 510
 511   //CosPhi() = CAMath::Sqrt(1-ey1*ey1);
 512   //if( ex<0 ) CosPhi() = -CosPhi();
 513
 514   fC[0]=( c00  + h2*h2*c22 + h4*h4*c44
 515           + 2*( h2*c20 + h4*c40 + h2*h4*c42 )  );
 516
 517   fC[1]= c10 + h2*c21 + h4*c41 + dS*(c30 + h2*c32 + h4*c43);
 518   fC[2]= c11 + 2*dS*c31 + dS*dS*c33;
 519
 520   fC[3]= c20 + h2*c22 + h4*c42 + h5*( c40 + h2*c42 + h4*c44);
 521   fC[4]= c21 + dS*c32 + h5*(c41 + dS*c43);
 522   fC[5]= c22 +2*h5*c42 + h5*h5*c44;
 523
 524   fC[6]= c30 + h2*c32 + h4*c43;
 525   fC[7]= c31 + dS*c33;
 526   fC[8]= c32 + h5*c43;
 527   fC[9]= c33;
 528
 529   fC[10]= c40 + h2*c42 + h4*c44;
 530   fC[11]= c41 + dS*c43;
 531   fC[12]= c42 + h5*c44;
 532   fC[13]= c43;
 533   fC[14]= c44;
 534
 535   return 1;
 536 }
 537
 538 GPUd() Int_t  AliHLTTPCCATrackParam::TransportToX0( Float_t x, Float_t maxSinPhi)
 539 {
 540   //* Transport the track parameters to X=x
 541
 542   Float_t ex = CosPhi();
 543   Float_t ey = SinPhi();
 544   Float_t dx = x - X();
 545
 546   if( CAMath::Abs(ex)<1.e-4 ) return 0;
 547   Float_t exi = 1./ex;
 548
 549   Float_t dl = dx*exi;
 550   Float_t dy = dl*ey;
 551   Float_t dS = dl;
 552   Float_t dz = dS*DzDs();
 553
 554
 555   Float_t h2 = dx*exi*exi*exi;
 556   Float_t h4 = dx*h2/2;
 557   Float_t h5 = dx;
 558
 559   Float_t c00 = fC[0];
 560   Float_t c10 = fC[1];
 561   Float_t c11 = fC[2];
 562   Float_t c20 = fC[3];
 563   Float_t c21 = fC[4];
 564   Float_t c22 = fC[5];
 565   Float_t c30 = fC[6];
 566   Float_t c31 = fC[7];
 567   Float_t c32 = fC[8];
 568   Float_t c33 = fC[9];
 569   Float_t c40 = fC[10];
 570   Float_t c41 = fC[11];
 571   Float_t c42 = fC[12];
 572   Float_t c43 = fC[13];
 573   Float_t c44 = fC[14];
 574
 575   //Float_t H0[5] = { 1,0, h2,  0, h4 };
 576   //Float_t H1[5] = { 0, 1, 0, dS,  0 };
 577   //Float_t H2[5] = { 0, 0, 1,  0, h5 };
 578   //Float_t H3[5] = { 0, 0, 0,  1,  0 };
 579   //Float_t H4[5] = { 0, 0, 0,  0,  1 };
 580
 581   Float_t ey1 = SinPhi() + h5*Kappa();
 582   if( CAMath::Abs(ey1)>maxSinPhi ){
 583     //std::cout<<" TransportToX0 error: sinPhi="<<ey<<" -> "<<ey1<<std::endl;
 584     return 0;
 585   }
 586   fX += dx ;
 587   fP[0]+= dy + h4*Kappa();
 588   fP[1]+= dz;
 589   fP[2] = ey1;
 590   fP[3] = fP[3];
 591   fP[4] = fP[4];
 592   fCosPhi = CAMath::Sqrt(1-ey1*ey1);
 593   if( ex<0 ) fCosPhi = -fCosPhi;
 594
 595   fC[0]=( c00  + h2*h2*c22 + h4*h4*c44
 596           + 2*( h2*c20 + h4*c40 + h2*h4*c42 )  );
 597
 598   fC[1]= c10 + h2*c21 + h4*c41 + dS*(c30 + h2*c32 + h4*c43);
 599   fC[2]= c11 + 2*dS*c31 + dS*dS*c33;
 600
 601   fC[3]= c20 + h2*c22 + h4*c42 + h5*( c40 + h2*c42 + h4*c44);
 602   fC[4]= c21 + dS*c32 + h5*(c41 + dS*c43);
 603   fC[5]= c22 +2*h5*c42 + h5*h5*c44;
 604
 605   fC[6]= c30 + h2*c32 + h4*c43;
 606   fC[7]= c31 + dS*c33;
 607   fC[8]= c32 + h5*c43;
 608   fC[9]= c33;
 609
 610   fC[10]= c40 + h2*c42 + h4*c44;
 611   fC[11]= c41 + dS*c43;
 612   fC[12]= c42 + h5*c44;
 613   fC[13]= c43;
 614   fC[14]= c44;
 615
 616   return 1;
 617 }
 618
 619
 620 GPUd() Bool_t AliHLTTPCCATrackParam::TransportToXWithMaterial( Float_t Xto, Float_t Bz )
 621 {
 622   //* Transport the track parameters to X=Xto
 623   AliHLTTPCCATrackFitParam par;
 624   CalculateFitParameters( par, Bz );
 625   return TransportToXWithMaterial(Xto, par );
 626 }
 627
 628
 629 GPUd() Bool_t  AliHLTTPCCATrackParam::TransportToXWithMaterial( Float_t x, AliHLTTPCCATrackFitParam &par )
 630 {
 631   //* Transport the track parameters to X=x
 632
 633   Bool_t ret = 1;
 634
 635   Float_t oldX=GetX();
 636
 637   Float_t x0  = X();
 638   //Float_t y0  = Y();
 639   Float_t k   = Kappa();
 640   Float_t ex = CosPhi();
 641   Float_t ey = SinPhi();
 642   Float_t dx = x - x0;
 643
 644   Float_t ey1 = k*dx + ey;
 645   Float_t ex1;
 646   if( CAMath::Abs(ey1)>.99 ){ // no intersection -> check the border
 647     ey1 = ( ey1>0 ) ?1 :-1;
 648     ex1 = 0;
 649     dx = ( CAMath::Abs(k)>1.e-4) ? ( (ey1-ey)/k ) :0;
 650
 651     Float_t ddx = CAMath::Abs(x0+dx - x)*k*k;
 652     Float_t hx[] = {0, -k, 1+ey };
 653     Float_t sx2 = hx[1]*hx[1]*fC[ 3] + hx[2]*hx[2]*fC[ 5];
 654     if( ddx*ddx>3.5*3.5*sx2 ) ret = 0; // x not withing the error
 655     ret = 0; // any case
 656     return ret;
 657   }else{
 658     ex1 = CAMath::Sqrt(1 - ey1*ey1);
 659     if( ex<0 ) ex1 = -ex1;
 660   }
 661
 662   Float_t dx2 = dx*dx;
 663   fCosPhi = ex1;
 664   Float_t ss = ey+ey1;
 665   Float_t cc = ex+ex1;
 666   Float_t tg = 0;
 667   if( CAMath::Abs(cc)>1.e-4 ) tg = ss/cc; // tan((phi1+phi)/2)
 668   else ret = 0;
 669   Float_t dy = dx*tg;
 670   Float_t dl = dx*CAMath::Sqrt(1+tg*tg);
 671
 672   if( cc<0 ) dl = -dl;
 673   Float_t dSin = dl*k/2;
 674   if( dSin > 1 ) dSin = 1;
 675   if( dSin <-1 ) dSin = -1;
 676   Float_t dS = ( CAMath::Abs(k)>1.e-4)  ? (2*CAMath::ASin(dSin)/k) :dl;
 677   Float_t dz = dS*DzDs();
 678
 679   Float_t cci = 0, exi = 0, ex1i = 0;
 680   if( CAMath::Abs(cc)>1.e-4 ) cci = 1./cc;
 681   else ret = 0;
 682   if( CAMath::Abs(ex)>1.e-4 ) exi = 1./ex;
 683   else ret = 0;
 684   if( CAMath::Abs(ex1)>1.e-4 ) ex1i = 1./ex1;
 685   else ret = 0;
 686
 687   if( !ret ) return ret;
 688
 689   fX += dx;
 690   fP[0]+= dy;
 691   fP[1]+= dz;
 692   fP[2] = ey1;
 693   fP[3] = fP[3];
 694   fP[4] = fP[4];
 695
 696   Float_t h2 = dx*(1+ ex*ex1 + ey*ey1 )*cci*exi*ex1i;
 697   Float_t h4 = dx2*(cc + ss*ey1*ex1i )*cci*cci;
 698
 699   Float_t c00 = fC[0];
 700   Float_t c10 = fC[1];
 701   Float_t c11 = fC[2];
 702   Float_t c20 = fC[3];
 703   Float_t c21 = fC[4];
 704   Float_t c22 = fC[5];
 705   Float_t c30 = fC[6];
 706   Float_t c31 = fC[7];
 707   Float_t c32 = fC[8];
 708   Float_t c33 = fC[9];
 709   Float_t c40 = fC[10];
 710   Float_t c41 = fC[11];
 711   Float_t c42 = fC[12];
 712   Float_t c43 = fC[13];
 713   Float_t c44 = fC[14];
 714
 715   //Float_t H0[5] = { 1,0, h2,  0, h4 };
 716   //Float_t H1[5] = { 0, 1, 0, dS,  0 };
 717   //Float_t H2[5] = { 0, 0, 1,  0, dx };
 718   //Float_t H3[5] = { 0, 0, 0,  1,  0 };
 719   //Float_t H4[5] = { 0, 0, 0,  0,  1 };
 720
 721
 722   fC[0]=( c00  + h2*h2*c22 + h4*h4*c44
 723           + 2*( h2*c20 + h4*c40 + h2*h4*c42 )  );
 724
 725   fC[1]= c10 + h2*c21 + h4*c41 + dS*(c30 + h2*c32 + h4*c43);
 726   fC[2]= c11 + 2*dS*c31 + dS*dS*c33;
 727
 728   fC[3]= c20 + h2*c22 + h4*c42 + dx*( c40 + h2*c42 + h4*c44);
 729   fC[4]= c21 + dS*c32 + dx*(c41 + dS*c43);
 730   fC[5]= c22 +2*dx*c42 + dx2*c44;
 731
 732   fC[6]= c30 + h2*c32 + h4*c43;
 733   fC[7]= c31 + dS*c33;
 734   fC[8]= c32 + dx*c43;
 735   fC[9]= c33;
 736
 737   fC[10]= c40 + h2*c42 + h4*c44;
 738   fC[11]= c41 + dS*c43;
 739   fC[12]= c42 + dx*c44;
 740   fC[13]= c43;
 741   fC[14]= c44;
 742
 743   Float_t d = CAMath::Sqrt(dS*dS + dz*dz );
 744
 745   if (oldX > GetX() ) d = -d;
 746   {
 747     Float_t rho=0.9e-3;
 748     Float_t radLen=28.94;
 749     CorrectForMeanMaterial(d*rho/radLen,d*rho,par);
 750   }
 751
 752   return ret;
 753 }
 754
 755
 756
 757 GPUd() Bool_t  AliHLTTPCCATrackParam::TransportToXWithMaterial( Float_t x, AliHLTTPCCATrackParam &t0, AliHLTTPCCATrackFitParam &par )
 758 {
 759   //* Transport the track parameters to X=x
 760
 761   Bool_t ret = 1;
 762
 763   Float_t oldX=t0.GetX();
 764
 765   Float_t x0  = t0.X();
 766   //Float_t y0  = t0.Y();
 767   Float_t k   = t0.Kappa();
 768   Float_t ex = t0.CosPhi();
 769   Float_t ey = t0.SinPhi();
 770   Float_t dx = x - x0;
 771
 772   Float_t ey1 = k*dx + ey;
 773   Float_t ex1;
 774   if( CAMath::Abs(ey1)>.99 ){ // no intersection -> check the border
 775     ey1 = ( ey1>0 ) ?1 :-1;
 776     ex1 = 0;
 777     dx = ( CAMath::Abs(k)>1.e-4) ? ( (ey1-ey)/k ) :0;
 778
 779     Float_t ddx = CAMath::Abs(x0+dx - x)*k*k;
 780     Float_t hx[] = {0, -k, 1+ey };
 781     Float_t sx2 = hx[1]*hx[1]*fC[ 3] + hx[2]*hx[2]*fC[ 5];
 782     if( ddx*ddx>3.5*3.5*sx2 ) ret = 0; // x not withing the error
 783     ret = 0; // any case
 784     return ret;
 785   }else{
 786     ex1 = CAMath::Sqrt(1 - ey1*ey1);
 787     if( ex<0 ) ex1 = -ex1;
 788   }
 789
 790   Float_t dx2 = dx*dx;
 791   Float_t ss = ey+ey1;
 792   Float_t cc = ex+ex1;
 793   Float_t tg = 0;
 794   if( CAMath::Abs(cc)>1.e-4 ) tg = ss/cc; // tan((phi1+phi)/2)
 795   else ret = 0;
 796   Float_t dy = dx*tg;
 797   Float_t dl = dx*CAMath::Sqrt(1+tg*tg);
 798
 799   if( cc<0 ) dl = -dl;
 800   Float_t dSin = dl*k/2;
 801   if( dSin > 1 ) dSin = 1;
 802   if( dSin <-1 ) dSin = -1;
 803   Float_t dS = ( CAMath::Abs(k)>1.e-4)  ? (2*CAMath::ASin(dSin)/k) :dl;
 804   Float_t dz = dS*t0.DzDs();
 805
 806   Float_t cci = 0, exi = 0, ex1i = 0;
 807   if( CAMath::Abs(cc)>1.e-4 ) cci = 1./cc;
 808   else ret = 0;
 809   if( CAMath::Abs(ex)>1.e-4 ) exi = 1./ex;
 810   else ret = 0;
 811   if( CAMath::Abs(ex1)>1.e-4 ) ex1i = 1./ex1;
 812   else ret = 0;
 813
 814   if( !ret ) return ret;
 815
 816   Float_t h2 = dx*(1+ ex*ex1 + ey*ey1 )*cci*exi*ex1i;
 817   Float_t h4 = dx2*(cc + ss*ey1*ex1i )*cci*cci;
 818
 819   Float_t c00 = fC[0];
 820   Float_t c10 = fC[1];
 821   Float_t c11 = fC[2];
 822   Float_t c20 = fC[3];
 823   Float_t c21 = fC[4];
 824   Float_t c22 = fC[5];
 825   Float_t c30 = fC[6];
 826   Float_t c31 = fC[7];
 827   Float_t c32 = fC[8];
 828   Float_t c33 = fC[9];
 829   Float_t c40 = fC[10];
 830   Float_t c41 = fC[11];
 831   Float_t c42 = fC[12];
 832   Float_t c43 = fC[13];
 833   Float_t c44 = fC[14];
 834
 835   //Float_t H0[5] = { 1,0, h2,  0, h4 };
 836   //Float_t H1[5] = { 0, 1, 0, dS,  0 };
 837   //Float_t H2[5] = { 0, 0, 1,  0, dx };
 838   //Float_t H3[5] = { 0, 0, 0,  1,  0 };
 839   //Float_t H4[5] = { 0, 0, 0,  0,  1 };
 840
 841   Float_t d[5] = { fP[0]-t0.fP[0], fP[1]-t0.fP[1], fP[2]-t0.fP[2],
 842                    fP[3]-t0.fP[3], fP[4]-t0.fP[4] };
 843
 844   t0.SetCosPhi( ex1 );
 845   t0.SetX( t0.X() + dx );
 846   t0.SetY( t0.Y() + dy );
 847   t0.SetZ( t0.Z() + dz );
 848   t0.SetSinPhi( ey1 );
 849
 850   fX = t0.X();
 851   fP[0] = t0.fP[0] + d[0]        + h2*d[2]          + h4*d[4];
 852   fP[1] = t0.fP[1]        + d[1]           + dS*d[3];
 853   fP[2] = t0.fP[2]               +d[2]              + dx*d[4];
 854   fP[3] = fP[3];
 855   fP[4] = fP[4];
 856
 857   //CosPhi() = CAMath::Sqrt(1-fP[2]*fP[2]);
 858
 859   fC[0]=( c00  + h2*h2*c22 + h4*h4*c44
 860           + 2*( h2*c20 + h4*c40 + h2*h4*c42 )  );
 861
 862   fC[1]= c10 + h2*c21 + h4*c41 + dS*(c30 + h2*c32 + h4*c43);
 863   fC[2]= c11 + 2*dS*c31 + dS*dS*c33;
 864
 865   fC[3]= c20 + h2*c22 + h4*c42 + dx*( c40 + h2*c42 + h4*c44);
 866   fC[4]= c21 + dS*c32 + dx*(c41 + dS*c43);
 867   fC[5]= c22 +2*dx*c42 + dx2*c44;
 868
 869   fC[6]= c30 + h2*c32 + h4*c43;
 870   fC[7]= c31 + dS*c33;
 871   fC[8]= c32 + dx*c43;
 872   fC[9]= c33;
 873
 874   fC[10]= c40 + h2*c42 + h4*c44;
 875   fC[11]= c41 + dS*c43;
 876   fC[12]= c42 + dx*c44;
 877   fC[13]= c43;
 878   fC[14]= c44;
 879
 880   Float_t dd = CAMath::Sqrt(dS*dS + dz*dz );
 881
 882   if (oldX > GetX() ) dd = -dd;
 883   {
 884     Float_t rho=0.9e-3;
 885     Float_t radLen=28.94;
 886     CorrectForMeanMaterial(dd*rho/radLen,dd*rho,par);
 887     t0.CorrectForMeanMaterial(dd*rho/radLen,dd*rho,par);
 888   }
 889
 890   return ret;
 891 }
 892
 893
 894 GPUd() Float_t AliHLTTPCCATrackParam::ApproximateBetheBloch( Float_t beta2 )
 895 {
 896   //------------------------------------------------------------------
 897   // This is an approximation of the Bethe-Bloch formula with
 898   // the density effect taken into account at beta*gamma > 3.5
 899   // (the approximation is reasonable only for solid materials)
 900   //------------------------------------------------------------------
 901   if (beta2 >= 1) return 0;
 902
 903   if (beta2/(1-beta2)>3.5*3.5)
 904     return 0.153e-3/beta2*( log(3.5*5940)+0.5*log(beta2/(1-beta2)) - beta2);
 905   return 0.153e-3/beta2*(log(5940*beta2/(1-beta2)) - beta2);
 906 }
 907
 908
 909 GPUd() void AliHLTTPCCATrackParam::CalculateFitParameters( AliHLTTPCCATrackFitParam &par, Float_t Bz, Float_t mass )
 910 {
 911   //*!
 912
 913   const Float_t kCLight = 0.000299792458;
 914   Float_t c = Bz*kCLight;
 915   Float_t p2 = (1.+ fP[3]*fP[3])*c*c;
 916   Float_t k2 = fP[4]*fP[4];
 917   Float_t beta2= p2 / (p2 + mass*mass*k2);
 918   Float_t bethe = ApproximateBetheBloch(beta2);
 919
 920   Float_t pp2 = (k2>1.e-8) ?p2/k2 :10000; // impuls 2
 921   par.fBethe = bethe;
 922   par.fE = CAMath::Sqrt( pp2 + mass*mass);
 923   par.fTheta2 = 14.1*14.1/(beta2*p2*1e6)*k2;
 924   par.fEP2 = par.fE/p2*k2;
 925
 926   // Approximate energy loss fluctuation (M.Ivanov)
 927
 928   const Float_t knst=0.07; // To be tuned.
 929   par.fSigmadE2 = knst*par.fEP2*fP[4];
 930   par.fSigmadE2 = par.fSigmadE2 * par.fSigmadE2;
 931
 932   par.fK22 = (1. + fP[3]*fP[3]);
 933   par.fK33 = par.fK22*par.fK22;
 934   par.fK43 = fP[3]*fP[4]*par.fK22;
 935   par.fK44 = fP[3]*fP[3]*fP[4]*fP[4];
 936 }
 937
 938
 939 GPUd() Bool_t AliHLTTPCCATrackParam::CorrectForMeanMaterial( Float_t xOverX0,  Float_t xTimesRho, const AliHLTTPCCATrackFitParam &par )
 940 {
 941   //------------------------------------------------------------------
 942   // This function corrects the track parameters for the crossed material.
 943   // "xOverX0"   - X/X0, the thickness in units of the radiation length.
 944   // "xTimesRho" - is the product length*density (g/cm^2).
 945   //------------------------------------------------------------------
 946
 947   Float_t &fC22=fC[5];
 948   Float_t &fC33=fC[9];
 949   Float_t &fC40=fC[10];
 950   Float_t &fC41=fC[11];
 951   Float_t &fC42=fC[12];
 952   Float_t &fC43=fC[13];
 953   Float_t &fC44=fC[14];
 954
 955   //Energy losses************************
 956
 957   Float_t dE = par.fBethe*xTimesRho;
 958   if ( CAMath::Abs(dE) > 0.3*par.fE ) return 0; //30% energy loss is too much!
 959   Float_t corr = (1.- par.fEP2*dE);
 960   if( corr<0.3 ) return 0;
 961   fP[4]*= corr;
 962   fC40*= corr;
 963   fC41*= corr;
 964   fC42*= corr;
 965   fC43*= corr;
 966   fC44*= corr*corr;
 967   fC44+= par.fSigmadE2*CAMath::Abs(dE);
 968
 969
 970   //Multiple scattering******************
 971
 972   Float_t theta2 = par.fTheta2*CAMath::Abs(xOverX0);
 973   fC22 += theta2*par.fK22*(1.- fP[2]*fP[2]);
 974   fC33 += theta2*par.fK33;
 975   fC43 += theta2*par.fK43;
 976   fC44 += theta2*par.fK44;
 977
 978   return 1;
 979 }
 980
 981
 982
 983 GPUd() Bool_t AliHLTTPCCATrackParam::Rotate( Float_t alpha, Float_t maxSinPhi )
 984 {
 985   //* Rotate the coordinate system in XY on the angle alpha
 986
 987   Float_t cA = CAMath::Cos( alpha );
 988   Float_t sA = CAMath::Sin( alpha );
 989   Float_t x = X(), y= Y(), sP= SinPhi(), cP= CosPhi();
 990   Float_t cosPhi = cP*cA + sP*sA;
 991   Float_t sinPhi =-cP*sA + sP*cA;
 992
 993   if( CAMath::Abs(sinPhi)> maxSinPhi || CAMath::Abs(cosPhi)<1.e-2 || CAMath::Abs(cP)<1.e-2  ) return 0;
 994
 995   Float_t j0 = cP/cosPhi;
 996   Float_t j2 = cosPhi/cP;
 997
 998   SetX( x*cA +  y*sA );
 999   SetY(-x*sA +  y*cA );
1000   SetCosPhi( cosPhi );
1001   SetSinPhi( sinPhi );
1002
1003
1004   //Float_t J[5][5] = { { j0, 0, 0,  0,  0 }, // Y
1005   //                      {  0, 1, 0,  0,  0 }, // Z
1006   //                      {  0, 0, j2, 0,  0 }, // SinPhi
1007   //                      {  0, 0, 0,  1,  0 }, // DzDs
1008   //                      {  0, 0, 0,  0,  1 } }; // Kappa
1009   //cout<<"alpha="<<alpha<<" "<<x<<" "<<y<<" "<<sP<<" "<<cP<<" "<<j0<<" "<<j2<<endl;
1010   //cout<<"      "<<fC[0]<<" "<<fC[1]<<" "<<fC[6]<<" "<<fC[10]<<" "<<fC[4]<<" "<<fC[5]<<" "<<fC[8]<<" "<<fC[12]<<endl;
1011   fC[0]*= j0*j0;
1012   fC[1]*= j0;
1013   fC[3]*= j0;
1014   fC[6]*= j0;
1015   fC[10]*= j0;
1016
1017   fC[3]*= j2;
1018   fC[4]*= j2;
1019   fC[5]*= j2*j2;
1020   fC[8]*= j2;
1021   fC[12]*= j2;
1022   //cout<<"      "<<fC[0]<<" "<<fC[1]<<" "<<fC[6]<<" "<<fC[10]<<" "<<fC[4]<<" "<<fC[5]<<" "<<fC[8]<<" "<<fC[12]<<endl;
1023   return 1;
1024 }
1025
1026
1027 GPUd() Bool_t AliHLTTPCCATrackParam::RotateNoCos( Float_t alpha, AliHLTTPCCATrackParam &t0, Float_t maxSinPhi )
1028 {
1029   //* Rotate the coordinate system in XY on the angle alpha
1030
1031   Float_t cA = CAMath::Cos( alpha );
1032   Float_t sA = CAMath::Sin( alpha );
1033   Float_t x0 = t0.X(), y0= t0.Y(), sP= t0.SinPhi(), cP= t0.CosPhi();
1034   Float_t cosPhi = cP*cA + sP*sA;
1035   Float_t sinPhi =-cP*sA + sP*cA;
1036
1037   if( CAMath::Abs(sinPhi)> maxSinPhi || CAMath::Abs(cosPhi)<1.e-2 || CAMath::Abs(cP)<1.e-2  ) return 0;
1038
1039   Float_t j0 = cP/cosPhi;
1040   Float_t j2 = cosPhi/cP;
1041   Float_t d[2] = {Y() - y0, SinPhi() - sP};
1042
1043   t0.SetX( x0*cA +  y0*sA );
1044   t0.SetY(-x0*sA +  y0*cA );
1045   t0.SetCosPhi( cosPhi );
1046   t0.SetSinPhi( sinPhi );
1047
1048   //Float_t J[5][5] = { { j0, 0, 0,  0,  0 }, // Y
1049   //                    {  0, 1, 0,  0,  0 }, // Z
1050   //                    {  0, 0, j2, 0,  0 }, // SinPhi
1051   //                    {  0, 0, 0,  1,  0 }, // DzDs
1052   //                    {  0, 0, 0,  0,  1 } }; // Kappa
1053
1054   SetX( t0.X() );
1055   SetY( t0.Y() + j0*d[0] );
1056   SetSinPhi( sinPhi + j2*d[1] );
1057
1058   fC[0]*= j0*j0;
1059   fC[1]*= j0;
1060   fC[3]*= j0;
1061   fC[6]*= j0;
1062   fC[10]*= j0;
1063
1064   fC[3]*= j2;
1065   fC[4]*= j2;
1066   fC[5]*= j2*j2;
1067   fC[8]*= j2;
1068   fC[12]*= j2;
1069
1070   return 1;
1071 }
1072
1073
1074 GPUd() Bool_t AliHLTTPCCATrackParam::Filter2( Float_t y, Float_t z, Float_t err2Y, Float_t err2Z, Float_t maxSinPhi )
1075 {
1076   //* Add the y,z measurement with the Kalman filter
1077
1078   Float_t
1079     c00 = fC[ 0],
1080     c10 = fC[ 1], c11 = fC[ 2],
1081     c20 = fC[ 3], c21 = fC[ 4],
1082     c30 = fC[ 6], c31 = fC[ 7],
1083     c40 = fC[10], c41 = fC[11];
1084
1085   Float_t
1086     z0 = y-fP[0],
1087     z1 = z-fP[1];
1088
1089   Float_t v[3] = {err2Y, 0, err2Z};
1090
1091   Float_t mS[3] = { c00+v[0], c10+v[1], c11+v[2] };
1092
1093   Float_t mSi[3];
1094   Float_t det = (mS[0]*mS[2] - mS[1]*mS[1]);
1095
1096   if( det < 1.e-8 ) return 0;
1097   det = 1./det;
1098   mSi[0] = mS[2]*det;
1099   mSi[1] = -mS[1]*det;
1100   mSi[2] = mS[0]*det;
1101
1102   // K = CHtS
1103
1104   Float_t k00, k01 , k10, k11, k20, k21, k30, k31, k40, k41;
1105
1106   k00 = c00*mSi[0] + c10*mSi[1]; k01 = c00*mSi[1] + c10*mSi[2];
1107   k10 = c10*mSi[0] + c11*mSi[1]; k11 = c10*mSi[1] + c11*mSi[2];
1108   k20 = c20*mSi[0] + c21*mSi[1]; k21 = c20*mSi[1] + c21*mSi[2];
1109   k30 = c30*mSi[0] + c31*mSi[1]; k31 = c30*mSi[1] + c31*mSi[2] ;
1110   k40 = c40*mSi[0] + c41*mSi[1]; k41 = c40*mSi[1] + c41*mSi[2] ;
1111
1112   Float_t sinPhi = fP[2] + k20*z0  + k21*z1  ;
1113   if( CAMath::Abs(sinPhi)>= maxSinPhi ) return 0;
1114
1115   fNDF  += 2;
1116   fChi2 += ( +(mSi[0]*z0 + mSi[1]*z1 )*z0
1117              +(mSi[1]*z0 + mSi[2]*z1 )*z1 );
1118
1119   fP[ 0]+= k00*z0  + k01*z1 ;
1120   fP[ 1]+= k10*z0  + k11*z1  ;
1121   fP[ 2] = sinPhi;
1122   fP[ 3]+= k30*z0  + k31*z1  ;
1123   fP[ 4]+= k40*z0  + k41*z1  ;
1124
1125
1126   fC[ 0]-= k00*c00 + k01*c10 ;
1127
1128   fC[ 1]-= k10*c00 + k11*c10 ;
1129   fC[ 2]-= k10*c10 + k11*c11 ;
1130
1131   fC[ 3]-= k20*c00 + k21*c10 ;
1132   fC[ 4]-= k20*c10 + k21*c11 ;
1133   fC[ 5]-= k20*c20 + k21*c21 ;
1134
1135   fC[ 6]-= k30*c00 + k31*c10 ;
1136   fC[ 7]-= k30*c10 + k31*c11 ;
1137   fC[ 8]-= k30*c20 + k31*c21 ;
1138   fC[ 9]-= k30*c30 + k31*c31 ;
1139
1140   fC[10]-= k40*c00 + k41*c10 ;
1141   fC[11]-= k40*c10 + k41*c11 ;
1142   fC[12]-= k40*c20 + k41*c21 ;
1143   fC[13]-= k40*c30 + k41*c31 ;
1144   fC[14]-= k40*c40 + k41*c41 ;
1145
1146   if( fCosPhi >=0 ){
1147     fCosPhi = CAMath::Sqrt(1-SinPhi()*SinPhi());
1148   }else{
1149     fCosPhi = -CAMath::Sqrt(1-SinPhi()*SinPhi());
1150   }
1151   return 1;
1152 }
1153
1154 GPUd() Bool_t AliHLTTPCCATrackParam::Filter2NoCos( Float_t y, Float_t z, Float_t err2Y, Float_t err2Z )
1155 {
1156   //* Add the y,z measurement with the Kalman filter
1157
1158   Float_t
1159     c00 = fC[ 0],
1160     c10 = fC[ 1], c11 = fC[ 2],
1161     c20 = fC[ 3], c21 = fC[ 4],
1162     c30 = fC[ 6], c31 = fC[ 7],
1163     c40 = fC[10], c41 = fC[11];
1164
1165   Float_t
1166     z0 = y-fP[0],
1167     z1 = z-fP[1];
1168
1169   Float_t v[3] = {err2Y, 0, err2Z};
1170
1171   Float_t mS[3] = { c00+v[0], c10+v[1], c11+v[2] };
1172
1173   Float_t mSi[3];
1174   Float_t det = (mS[0]*mS[2] - mS[1]*mS[1]);
1175
1176   if( !finite(det) || det > 1.e15 ) return 0;
1177   if( det < 1.e-8  ) return 0;
1178   det = 1./det;
1179   mSi[0] = mS[2]*det;
1180   mSi[1] = -mS[1]*det;
1181   mSi[2] = mS[0]*det;
1182
1183   // K = CHtS
1184
1185   Float_t k00, k01 , k10, k11, k20, k21, k30, k31, k40, k41;
1186
1187   k00 = c00*mSi[0] + c10*mSi[1]; k01 = c00*mSi[1] + c10*mSi[2];
1188   k10 = c10*mSi[0] + c11*mSi[1]; k11 = c10*mSi[1] + c11*mSi[2];
1189   k20 = c20*mSi[0] + c21*mSi[1]; k21 = c20*mSi[1] + c21*mSi[2];
1190   k30 = c30*mSi[0] + c31*mSi[1]; k31 = c30*mSi[1] + c31*mSi[2] ;
1191   k40 = c40*mSi[0] + c41*mSi[1]; k41 = c40*mSi[1] + c41*mSi[2] ;
1192
1193   Float_t sinPhi = fP[2] + k20*z0  + k21*z1  ;
1194   //if( CAMath::Abs(sinPhi)>= maxSinPhi ) return 0;
1195
1196   fNDF  += 2;
1197   fChi2 += ( +(mSi[0]*z0 + mSi[1]*z1 )*z0
1198              +(mSi[1]*z0 + mSi[2]*z1 )*z1 );
1199
1200   fP[ 0]+= k00*z0  + k01*z1 ;
1201   fP[ 1]+= k10*z0  + k11*z1  ;
1202   fP[ 2] = sinPhi;
1203   fP[ 3]+= k30*z0  + k31*z1  ;
1204   fP[ 4]+= k40*z0  + k41*z1  ;
1205
1206
1207   fC[ 0]-= k00*c00 + k01*c10 ;
1208
1209   fC[ 1]-= k10*c00 + k11*c10 ;
1210   fC[ 2]-= k10*c10 + k11*c11 ;
1211
1212   fC[ 3]-= k20*c00 + k21*c10 ;
1213   fC[ 4]-= k20*c10 + k21*c11 ;
1214   fC[ 5]-= k20*c20 + k21*c21 ;
1215
1216   fC[ 6]-= k30*c00 + k31*c10 ;
1217   fC[ 7]-= k30*c10 + k31*c11 ;
1218   fC[ 8]-= k30*c20 + k31*c21 ;
1219   fC[ 9]-= k30*c30 + k31*c31 ;
1220
1221   fC[10]-= k40*c00 + k41*c10 ;
1222   fC[11]-= k40*c10 + k41*c11 ;
1223   fC[12]-= k40*c20 + k41*c21 ;
1224   fC[13]-= k40*c30 + k41*c31 ;
1225   fC[14]-= k40*c40 + k41*c41 ;
1226   /*
1227   if( CosPhi()>=0 ){
1228     CosPhi() = CAMath::Sqrt(1-SinPhi()*SinPhi());
1229   }else{
1230     CosPhi() = -CAMath::Sqrt(1-SinPhi()*SinPhi());
1231   }
1232   */
1233   return 1;
1234 }
1235
1236
1237 GPUd() Bool_t AliHLTTPCCATrackParam::Filter2v1( Float_t y, Float_t z, Float_t err2Y, Float_t err2Z, Float_t maxSinPhi )
1238 {
1239   //* Add the y,z measurement with the Kalman filter
1240
1241   Float_t
1242     c00 = fC[ 0],
1243     c10 = fC[ 1], c11 = fC[ 2],
1244     c20 = fC[ 3], c21 = fC[ 4],
1245     c30 = fC[ 6], c31 = fC[ 7],
1246     c40 = fC[10], c41 = fC[11];
1247
1248   err2Y+=c00;
1249   err2Z+=c11;
1250
1251   Float_t
1252     z0 = y-fP[0],
1253     z1 = z-fP[1];
1254
1255   Float_t det = ( err2Y*err2Z - c10*c10);
1256   if( det < 1.e-8 ) return 0;
1257
1258   det = 1./det;
1259
1260   Float_t mS0 = err2Z*det;
1261   Float_t mS1 = -c10*det;
1262   Float_t mS2 = err2Y*det;
1263
1264   // K = CHtS
1265
1266   Float_t k00, k01 , k10, k11, k20, k21, k30, k31, k40, k41;
1267
1268   k00 = c00*mS0 + c10*mS1; k01 = c00*mS1 + c10*mS2;
1269   k10 = c10*mS0 + c11*mS1; k11 = c10*mS1 + c11*mS2;
1270   k20 = c20*mS0 + c21*mS1; k21 = c20*mS1 + c21*mS2;
1271   k30 = c30*mS0 + c31*mS1; k31 = c30*mS1 + c31*mS2;
1272   k40 = c40*mS0 + c41*mS1; k41 = c40*mS1 + c41*mS2;
1273
1274   Float_t sinPhi = fP[2] + k20*z0  + k21*z1  ;
1275   if( CAMath::Abs(sinPhi)>= maxSinPhi ) return 0;
1276
1277   fNDF  += 2;
1278   fChi2 += (mS0*z0 + mS1*z1 )*z0 + (mS1*z0 + mS2*z1 )*z1 ;
1279
1280   fP[ 0]+= k00*z0  + k01*z1 ;
1281   fP[ 1]+= k10*z0  + k11*z1  ;
1282   fP[ 2] = sinPhi;
1283   fP[ 3]+= k30*z0  + k31*z1  ;
1284   fP[ 4]+= k40*z0  + k41*z1  ;
1285
1286
1287   fC[ 0]-= k00*c00 + k01*c10 ;
1288
1289   fC[ 1]-= k10*c00 + k11*c10 ;
1290   fC[ 2]-= k10*c10 + k11*c11 ;
1291
1292   fC[ 3]-= k20*c00 + k21*c10 ;
1293   fC[ 4]-= k20*c10 + k21*c11 ;
1294   fC[ 5]-= k20*c20 + k21*c21 ;
1295
1296   fC[ 6]-= k30*c00 + k31*c10 ;
1297   fC[ 7]-= k30*c10 + k31*c11 ;
1298   fC[ 8]-= k30*c20 + k31*c21 ;
1299   fC[ 9]-= k30*c30 + k31*c31 ;
1300
1301   fC[10]-= k40*c00 + k41*c10 ;
1302   fC[11]-= k40*c10 + k41*c11 ;
1303   fC[12]-= k40*c20 + k41*c21 ;
1304   fC[13]-= k40*c30 + k41*c31 ;
1305   fC[14]-= k40*c40 + k41*c41 ;
1306
1307   if( fCosPhi>=0 ){
1308     fCosPhi = CAMath::Sqrt(1-sinPhi*sinPhi);
1309   }else{
1310     fCosPhi = -CAMath::Sqrt(1-sinPhi*sinPhi);
1311   }
1312   return 1;
1313 }
1314
1315 GPUd() Bool_t AliHLTTPCCATrackParam::Filter20( Float_t y, Float_t z, Float_t err2Y, Float_t err2Z, Float_t maxSinPhi )
1316 {
1317   //* Add the y,z measurement with the Kalman filter
1318
1319   Float_t
1320     c00 = fC[ 0],
1321     c11 = fC[ 2],
1322     c20 = fC[ 3],
1323     c31 = fC[ 7],
1324     c40 = fC[10];
1325
1326   err2Y+=c00;
1327   err2Z+=c11;
1328
1329   Float_t
1330     z0 = y-fP[0],
1331     z1 = z-fP[1];
1332
1333   if( err2Y < 1.e-8 || err2Z<1.e-8 ) return 0;
1334
1335   Float_t mS0 = 1./err2Y;
1336   Float_t mS2 = 1./err2Z;
1337
1338   // K = CHtS
1339
1340   Float_t k00, k11, k20, k31, k40;
1341
1342   k00 = c00*mS0;
1343   k20 = c20*mS0;
1344   k40 = c40*mS0;
1345
1346   k11 = c11*mS2;
1347   k31 = c31*mS2;
1348
1349   Float_t sinPhi = fP[2] + k20*z0  ;
1350
1351
1352   if( CAMath::Abs(sinPhi)>= maxSinPhi ){
1353     std::cout<<"Filter20: sinPhi ="<<fP[2]<<" -> "<<sinPhi<<std::endl;
1354     std::cout<<"z0,z1="<<z0<<" "<<z1<<std::endl;
1355     std::cout<<"c="<<fC[0]<<std::endl;
1356     std::cout<<"  "<<fC[1]<<" "<<fC[2]<<std::endl;
1357     std::cout<<"  "<<fC[3]<<" "<<fC[4]<<" "<<fC[5]<<std::endl;
1358   }
1359   if( CAMath::Abs(sinPhi)>= maxSinPhi ){
1360     return 0;
1361   }
1362   Float_t cosPhi = CAMath::Sqrt(1-sinPhi*sinPhi);
1363   fNDF  += 2;
1364   fChi2 += mS0*z0*z0 + mS2*z1*z1 ;
1365
1366   fP[ 0]+= k00*z0 ;
1367   fP[ 1]+= k11*z1 ;
1368   fP[ 2] = sinPhi ;
1369   fP[ 3]+= k31*z1 ;
1370   fP[ 4]+= k40*z0 ;
1371
1372   fC[ 0]-= k00*c00 ;
1373   fC[ 3]-= k20*c00 ;
1374   fC[ 5]-= k20*c20 ;
1375   fC[10]-= k40*c00 ;
1376   fC[12]-= k40*c20 ;
1377   fC[14]-= k40*c40 ;
1378
1379   fC[ 2]-= k11*c11 ;
1380   fC[ 7]-= k31*c11 ;
1381   fC[ 9]-= k31*c31 ;
1382
1383   fCosPhi = ( fCosPhi >=0 ) ?cosPhi :-cosPhi;
1384   return 1;
1385 }
1386
1387
1388
1389 GPUd() Bool_t AliHLTTPCCATrackParam::Filter200( Float_t y, Float_t z, Float_t err2Y, Float_t err2Z )
1390 {
1391   //* Add the y,z measurement with the Kalman filter
1392
1393   Float_t
1394     c00 = fC[ 0],
1395     c11 = fC[ 2],
1396     c20 = fC[ 3],
1397     c31 = fC[ 7],
1398     c40 = fC[10];
1399
1400   err2Y+=c00;
1401   err2Z+=c11;
1402
1403   Float_t
1404     z0 = y-fP[0],
1405     z1 = z-fP[1];
1406
1407   if( err2Y < 1.e-8 || err2Z<1.e-8 ) return 0;
1408
1409   Float_t mS0 = 1./err2Y;
1410   Float_t mS2 = 1./err2Z;
1411
1412   // K = CHtS
1413
1414   Float_t k00, k11, k20, k31, k40;
1415
1416   k00 = c00*mS0;
1417   k20 = c20*mS0;
1418   k40 = c40*mS0;
1419
1420   k11 = c11*mS2;
1421   k31 = c31*mS2;
1422
1423   Float_t sinPhi = fP[2] + k20*z0  ;
1424
1425
1426   //if( CAMath::Abs(sinPhi)>= maxSinPhi ){
1427   //std::cout<<"Filter20: sinPhi ="<<fP[2]<<" -> "<<sinPhi<<std::endl;
1428   //std::cout<<"z0,z1="<<z0<<" "<<z1<<std::endl;
1429   //std::cout<<"c="<<fC[0]<<std::endl;
1430   //std::cout<<"  "<<fC[1]<<" "<<fC[2]<<std::endl;
1431   //std::cout<<"  "<<fC[3]<<" "<<fC[4]<<" "<<fC[5]<<std::endl;
1432   //}
1433   //if( CAMath::Abs(sinPhi)>= maxSinPhi ){
1434   //return 0;
1435   //}
1436   //Float_t cosPhi = CAMath::Sqrt(1-sinPhi*sinPhi);
1437   fNDF  += 2;
1438   fChi2 += mS0*z0*z0 + mS2*z1*z1 ;
1439
1440   fP[ 0]+= k00*z0 ;
1441   fP[ 1]+= k11*z1 ;
1442   fP[ 2] = sinPhi ;
1443   fP[ 3]+= k31*z1 ;
1444   fP[ 4]+= k40*z0 ;
1445
1446   fC[ 0]-= k00*c00 ;
1447   fC[ 3]-= k20*c00 ;
1448   fC[ 5]-= k20*c20 ;
1449   fC[10]-= k40*c00 ;
1450   fC[12]-= k40*c20 ;
1451   fC[14]-= k40*c40 ;
1452
1453   fC[ 2]-= k11*c11 ;
1454   fC[ 7]-= k31*c11 ;
1455   fC[ 9]-= k31*c31 ;
1456
1457   //fCosPhi = ( fCosPhi >=0 ) ?cosPhi :-cosPhi;
1458   return 1;
1459 }
1460
1461
1462 GPUd() void AliHLTTPCCATrackParam::FilterY( Float_t y, Float_t erry )
1463 {
1464   //* Add the y measurement with the Kalman filter
1465
1466   Float_t
1467     c00 = fC[ 0],
1468     c10 = fC[ 1],
1469     c20 = fC[ 3],
1470     c30 = fC[ 6],
1471     c40 = fC[10];
1472
1473   Float_t
1474     z0 = y-fP[0];
1475
1476   Float_t s = { c00+erry*erry };
1477   if( CAMath::Abs(s)<1.e-4 ) return;
1478
1479   Float_t si = 1/s;
1480
1481   fNDF  += 1;
1482   fChi2 += si*z0*z0;
1483
1484   // K = CHtS
1485
1486   Float_t k0, k1 , k2, k3, k4;
1487
1488   k0 = c00*si;
1489   k1 = c10*si;
1490   k2 = c20*si;
1491   k3 = c30*si;
1492   k4 = c40*si;
1493
1494   Float_t sinPhi = fP[2] + k2*z0 ;
1495   if( CAMath::Abs(sinPhi)>=0.99 ) return;
1496
1497   fP[ 0]+= k0*z0 ;
1498   fP[ 1]+= k1*z0 ;
1499   fP[ 2] = sinPhi;
1500   fP[ 3]+= k3*z0 ;
1501   fP[ 4]+= k4*z0 ;
1502
1503   fC[ 0]-= k0*c00;
1504
1505   fC[ 1]-= k1*c00;
1506   fC[ 2]-= k1*c10;
1507
1508   fC[ 3]-= k2*c00;
1509   fC[ 4]-= k2*c10;
1510   fC[ 5]-= k2*c20;
1511
1512   fC[ 6]-= k3*c00;
1513   fC[ 7]-= k3*c10;
1514   fC[ 8]-= k3*c20;
1515   fC[ 9]-= k3*c30;
1516
1517   fC[10]-= k4*c00;
1518   fC[11]-= k4*c10;
1519   fC[12]-= k4*c20;
1520   fC[13]-= k4*c30;
1521   fC[14]-= k4*c40;
1522
1523   if( fCosPhi>=0 ){
1524     fCosPhi = CAMath::Sqrt(1-SinPhi()*SinPhi());
1525   }else{
1526     fCosPhi = -CAMath::Sqrt(1-SinPhi()*SinPhi());
1527   }
1528
1529 }
1530
1531 GPUd() void AliHLTTPCCATrackParam::FilterZ( Float_t z, Float_t errz )
1532 {
1533   //* Add the z measurement with the Kalman filter
1534
1535   Float_t
1536     c01 = fC[ 1],
1537     c11 = fC[ 2],
1538     c21 = fC[ 4],
1539     c31 = fC[ 7],
1540     c41 = fC[11];
1541
1542   Float_t
1543     z1 = z-fP[1];
1544
1545   Float_t s = c11 + errz*errz;
1546   if( CAMath::Abs(s)<1.e-4 ) return;
1547
1548   Float_t si = 1./s;
1549
1550   fNDF  += 1;
1551   fChi2 += si*z1*z1;
1552
1553   // K = CHtS
1554
1555   Float_t k0, k1 , k2, k3, k4;
1556
1557   k0 = 0;//c01*si;
1558   k1 = c11*si;
1559   k2 = 0;//c21*si;
1560   k3 = c31*si;
1561   k4 = 0;//c41*si;
1562
1563   Float_t sinPhi = fP[2] + k2*z1  ;
1564   if( CAMath::Abs(sinPhi)>=0.99 ) return;
1565
1566   fP[ 0]+= k0*z1 ;
1567   fP[ 1]+= k1*z1 ;
1568   fP[ 2] = sinPhi;
1569   fP[ 3]+= k3*z1 ;
1570   fP[ 4]+= k4*z1 ;
1571
1572
1573   fC[ 0]-= k0*c01 ;
1574
1575   fC[ 1]-= k1*c01 ;
1576   fC[ 2]-= k1*c11 ;
1577
1578   fC[ 3]-= k2*c01 ;
1579   fC[ 4]-= k2*c11 ;
1580   fC[ 5]-= k2*c21 ;
1581
1582   fC[ 6]-= k3*c01 ;
1583   fC[ 7]-= k3*c11 ;
1584   fC[ 8]-= k3*c21 ;
1585   fC[ 9]-= k3*c31 ;
1586
1587   fC[10]-= k4*c01 ;
1588   fC[11]-= k4*c11 ;
1589   fC[12]-= k4*c21 ;
1590   fC[13]-= k4*c31 ;
1591   fC[14]-= k4*c41 ;
1592
1593   if( fCosPhi>=0 ){
1594     fCosPhi = CAMath::Sqrt(1-SinPhi()*SinPhi());
1595   }else{
1596     fCosPhi = -CAMath::Sqrt(1-SinPhi()*SinPhi());
1597   }
1598
1599 }
1600
1601 #if !defined(HLTCA_GPUCODE)
1602 #include <iostream>
1603 #endif
1604
1605 GPUd() void AliHLTTPCCATrackParam::Print() const
1606 {
1607   //* print parameters
1608
1609 #if !defined(HLTCA_GPUCODE)
1610   std::cout<<"track: x="<<GetX()<<" c="<<GetCosPhi()<<", P= "<<GetY()<<" "<<GetZ()<<" "<<GetSinPhi()<<" "<<GetDzDs()<<" "<<GetKappa()<<std::endl;
1611   std::cout<<"errs2: "<<GetErr2Y()<<" "<<GetErr2Z()<<" "<<GetErr2SinPhi()<<" "<<GetErr2DzDs()<<" "<<GetErr2Kappa()<<std::endl;
1612 #endif
1613 }