[u/mrichter/AliRoot.git] / HLT / TPCLib / tracking-ca / AliHLTTPCCATrackParam.cxx

// $Id$
//***************************************************************************
// This file is property of and copyright by the ALICE HLT Project          * 
// ALICE Experiment at CERN, All rights reserved.                           *
//                                                                          *
// Primary Authors: Sergey Gorbunov <sergey.gorbunov@kip.uni-heidelberg.de> *
//                  Ivan Kisel <kisel@kip.uni-heidelberg.de>                *
//                  for The ALICE HLT Project.                              *
//                                                                          *
// Permission to use, copy, modify and distribute this software and its     *
// documentation strictly for non-commercial purposes is hereby granted     *
// without fee, provided that the above copyright notice appears in all     *
// copies and that both the copyright notice and this permission notice     *
// appear in the supporting documentation. The authors make no claims       *
// about the suitability of this software for any purpose. It is            *
// provided "as is" without express or implied warranty.                    *
//***************************************************************************

#include "AliHLTTPCCATrackParam.h"
#include "TMath.h"
#include "AliExternalTrackParam.h"

//ClassImp(AliHLTTPCCATrackParam)

//
// Circle in XY:
// 
// R  = 1/TMath::Abs(Kappa);
// Xc = X - sin(Phi)/Kappa;
// Yc = Y + cos(Phi)/Kappa;
//


void AliHLTTPCCATrackParam::ConstructXY3( const Float_t x[3], const Float_t y[3], 
					  const Float_t sigmaY2[3], Float_t CosPhi0 )
{
  //* Construct the track in XY plane by 3 points

  Float_t x0 = x[0];
  Float_t y0 = y[0];
  Float_t x1 = x[1] - x0;
  Float_t y1 = y[1] - y0;
  Float_t x2 = x[2] - x0;
  Float_t y2 = y[2] - y0;
  
  Float_t a1 = x1*x1 + y1*y1;
  Float_t a2 = x2*x2 + y2*y2;
  Float_t a = 2*(x1*y2 - y1*x2);
  Float_t lx =  a1*y2 - a2*y1;
  Float_t ly = -a1*x2 + a2*x1;
  Float_t l = TMath::Sqrt(lx*lx + ly*ly);
 
  Float_t li = 1./l;
  Float_t li2 = li*li;
  Float_t li3 = li2*li;
  Float_t cosPhi = ly*li;

  Float_t sinPhi = -lx*li;
  Float_t kappa = a/l;

  Float_t dlx = a2 - a1;     //  D lx / D y0
  Float_t dly = -a;          //  D ly / D y0
  Float_t dA  = 2*(x2 - x1); // D a  / D y0
  Float_t dl = (lx*dlx + ly*dly)*li;
  
  // D sinPhi,kappa / D y0

  Float_t d0[2] = { -(dlx*ly-lx*dly)*ly*li3, (dA*l-a*dl)*li2 };

  // D sinPhi,kappa / D y1 

  dlx = -a2 + 2*y1*y2;
  dly = -2*x2*y1;
  dA  = -2*x2;
  dl = (lx*dlx + ly*dly)*li;

  Float_t d1[2] = { -(dlx*ly-lx*dly)*ly*li3, (dA*l-a*dl)*li2 };

  // D sinPhi,kappa / D y2

  dlx = a1 - 2*y1*y2;
  dly = -2*x1*y2;
  dA  = 2*x1;
  dl = (lx*dlx + ly*dly)*li;

  Float_t d2[2] = { -(dlx*ly-lx*dly)*ly*li3, (dA*l-a*dl)*li2 };
   
  if( CosPhi0*cosPhi <0 ){
    cosPhi = -cosPhi;
    sinPhi = -sinPhi;
    kappa = -kappa;   
    d0[0] = -d0[0];
    d0[1] = -d0[1];
    d1[0] = -d1[0];
    d1[1] = -d1[1];
    d2[0] = -d2[0];
    d2[1] = -d2[1];
  }
  
  X() = x0;  
  Y() = y0;
  SinPhi() = sinPhi;
  Kappa() = kappa;
  CosPhi() = cosPhi;

  Float_t s0 = sigmaY2[0];
  Float_t s1 = sigmaY2[1];
  Float_t s2 = sigmaY2[2];
  
  fC[0] = s0;
  fC[1] = 0;
  fC[2] = 0;

  fC[3] = d0[0]*s0;
  fC[4] = 0;
  fC[5] = d0[0]*d0[0]*s0 + d1[0]*d1[0]*s1 + d2[0]*d2[0]*s2;

  fC[6] = 0;
  fC[7] = 0;
  fC[8] = 0;
  fC[9] = 0;

  fC[10] = d0[1]*s0;
  fC[11] = 0;
  fC[12] = d0[0]*d0[1]*s0 + d1[0]*d1[1]*s1 + d2[0]*d2[1]*s2;
  fC[13] = 0;
  fC[14] = d0[1]*d0[1]*s0 + d1[1]*d1[1]*s1 + d2[1]*d2[1]*s2;
}


Float_t  AliHLTTPCCATrackParam::GetS( Float_t x, Float_t y ) const
{
  //* Get XY path length to the given point

  Float_t k  = GetKappa();
  Float_t ex = GetCosPhi();
  Float_t ey = GetSinPhi();
  x-= GetX();
  y-= GetY();
  Float_t dS = x*ex + y*ey;
  if( TMath::Abs(k)>1.e-4 ) dS = TMath::ATan2( k*dS, 1+k*(x*ey-y*ex) )/k;
  return dS;
}

void  AliHLTTPCCATrackParam::GetDCAPoint( Float_t x, Float_t y, Float_t z,
					   Float_t &xp, Float_t &yp, Float_t &zp ) const
{
  //* Get the track point closest to the (x,y,z)

  Float_t x0 = GetX();
  Float_t y0 = GetY();
  Float_t k  = GetKappa();
  Float_t ex = GetCosPhi();
  Float_t ey = GetSinPhi();
  Float_t dx = x - x0;
  Float_t dy = y - y0; 
  Float_t ax = dx*k+ey;
  Float_t ay = dy*k-ex;
  Float_t a = sqrt( ax*ax+ay*ay );
  xp = x0 + (dx - ey*( (dx*dx+dy*dy)*k - 2*(-dx*ey+dy*ex) )/(a+1) )/a;
  yp = y0 + (dy + ex*( (dx*dx+dy*dy)*k - 2*(-dx*ey+dy*ex) )/(a+1) )/a;
  Float_t s = GetS(x,y);
  zp = GetZ() + GetDzDs()*s;
  if( TMath::Abs(k)>1.e-2 ){
    Float_t dZ = TMath::Abs( GetDzDs()*TMath::TwoPi()/k );
    if( dZ>.1 ){
      zp+= TMath::Nint((z-zp)/dZ)*dZ;    
    }
  }
}

void AliHLTTPCCATrackParam::ConstructXYZ3( const Float_t p0[5], const Float_t p1[5], 
					   const Float_t p2[5], 
					   Float_t CosPhi0, Float_t t0[] )
{      
  //* Construct the track in XYZ by 3 points

  Float_t px[3]   = { p0[0], p1[0], p2[0] };
  Float_t py[3]   = { p0[1], p1[1], p2[1] };
  Float_t pz[3]   = { p0[2], p1[2], p2[2] };
  Float_t ps2y[3] = { p0[3]*p0[3], p1[3]*p1[3], p2[3]*p2[3] };
  Float_t ps2z[3] = { p0[4]*p0[4], p1[4]*p1[4], p2[4]*p2[4] };

  Float_t kold = t0 ?t0[4] :0;
  ConstructXY3( px, py, ps2y, CosPhi0 );

  Float_t pS[3] = { GetS(px[0],py[0]), GetS(px[1],py[1]), GetS(px[2],py[2]) };
  Float_t k = Kappa();
  if( TMath::Abs(k)>1.e-2 ){    
    Float_t dS = TMath::Abs( TMath::TwoPi()/k );
    pS[1]+= TMath::Nint( (pS[0]-pS[1])/dS )*dS; // not more than half turn
    pS[2]+= TMath::Nint( (pS[1]-pS[2])/dS )*dS;
    if( t0 ){
      Float_t dZ = TMath::Abs(t0[3]*dS);
      if( TMath::Abs(dZ)>1. ){
	Float_t dsDz = 1./t0[3];
	if( kold*k<0 ) dsDz = -dsDz;
	Float_t s0 = (pz[0]-t0[1])*dsDz;
	Float_t s1 = (pz[1]-t0[1])*dsDz;
	Float_t s2 = (pz[2]-t0[1])*dsDz;	
	pS[0]+= TMath::Nint( (s0-pS[0])/dS )*dS ;
	pS[1]+= TMath::Nint( (s1-pS[1])/dS )*dS ;
	pS[2]+= TMath::Nint( (s2-pS[2])/dS )*dS ;	
      }
    }
  }

  Float_t s = pS[0] + pS[1] + pS[2];
  Float_t z = pz[0] + pz[1] + pz[2];
  Float_t sz = pS[0]*pz[0] + pS[1]*pz[1] + pS[2]*pz[2];
  Float_t ss = pS[0]*pS[0] + pS[1]*pS[1] + pS[2]*pS[2];
  
  Float_t a = 3*ss-s*s;
  Z() = (z*ss-sz*s)/a; // z0
  DzDs() = (3*sz-z*s)/a; // t = dz/ds
    
  Float_t dz0[3] = {ss - pS[0]*s,ss - pS[1]*s,ss - pS[2]*s };
  Float_t dt [3] = {3*pS[0] - s, 3*pS[1] - s, 3*pS[2] - s };

  fC[2] = (dz0[0]*dz0[0]*ps2z[0] + dz0[1]*dz0[1]*ps2z[1] + dz0[2]*dz0[2]*ps2z[2])/a/a;
  fC[7]= (dz0[0]*dt [0]*ps2z[0] + dz0[1]*dt [1]*ps2z[1] + dz0[2]*dt [2]*ps2z[2])/a/a;  
  fC[9]= (dt [0]*dt [0]*ps2z[0] + dt [1]*dt [1]*ps2z[1] + dt [2]*dt [2]*ps2z[2])/a/a;  
}


Bool_t  AliHLTTPCCATrackParam::TransportToX( Float_t x )
{
  //* Transport the track parameters to X=x 

  Bool_t ret = 1;

  Float_t x0  = X();
  //Float_t y0  = Y();
  Float_t k   = Kappa();
  Float_t ex = CosPhi();
  Float_t ey = SinPhi();
  Float_t dx = x - x0;

  Float_t ey1 = k*dx + ey;
  Float_t ex1;
  if( TMath::Abs(ey1)>1 ){ // no intersection -> check the border    
    ey1 = ( ey1>0 ) ?1 :-1;
    ex1 = 0;
    dx = ( TMath::Abs(k)>1.e-4) ? ( (ey1-ey)/k ) :0;
    
    Float_t ddx = TMath::Abs(x0+dx - x)*k*k;
    Float_t hx[] = {0, -k, 1+ey };
    Float_t sx2 = hx[1]*hx[1]*fC[ 3] + hx[2]*hx[2]*fC[ 5];
    if( ddx*ddx>3.5*3.5*sx2 ) ret = 0; // x not withing the error
    ret = 0; // any case
    return ret;
  }else{
    ex1 = TMath::Sqrt(1 - ey1*ey1);
    if( ex<0 ) ex1 = -ex1;  
  }
  
  Float_t dx2 = dx*dx;
  CosPhi() = ex1;
  Float_t ss = ey+ey1;
  Float_t cc = ex+ex1;  
  Float_t tg = 0;
  if( TMath::Abs(cc)>1.e-4 ) tg = ss/cc; // tan((phi1+phi)/2)
  else ret = 0; 
  Float_t dy = dx*tg;
  Float_t dl = dx*TMath::Sqrt(1+tg*tg);

  if( cc<0 ) dl = -dl;
  Float_t dSin = dl*k/2;
  if( dSin > 1 ) dSin = 1;
  if( dSin <-1 ) dSin = -1;
  Float_t dS = ( TMath::Abs(k)>1.e-4)  ? (2*TMath::ASin(dSin)/k) :dl;
  Float_t dz = dS*DzDs();

  Float_t cci = 0, exi = 0, ex1i = 0;
  if( TMath::Abs(cc)>1.e-4 ) cci = 1./cc;
  else ret = 0;
  if( TMath::Abs(ex)>1.e-4 ) exi = 1./ex;
  else ret = 0;
  if( TMath::Abs(ex1)>1.e-4 ) ex1i = 1./ex1;
  else ret = 0;

  if( !ret ) return ret;
  X() += dx;
  fP[0]+= dy;
  fP[1]+= dz;
  fP[2] = ey1;
  fP[3] = fP[3];
  fP[4] = fP[4];

  Float_t h2 = dx*(1+ ex*ex1 + ey*ey1 )*cci*exi*ex1i;
  Float_t h4 = dx2*(cc + ss*ey1*ex1i )*cci*cci;

  Float_t c00 = fC[0];
  Float_t c10 = fC[1];
  Float_t c11 = fC[2];
  Float_t c20 = fC[3];
  Float_t c21 = fC[4];
  Float_t c22 = fC[5];
  Float_t c30 = fC[6];
  Float_t c31 = fC[7];
  Float_t c32 = fC[8];
  Float_t c33 = fC[9];
  Float_t c40 = fC[10];
  Float_t c41 = fC[11];
  Float_t c42 = fC[12];
  Float_t c43 = fC[13];
  Float_t c44 = fC[14];

  //Float_t H0[5] = { 1,0, h2,  0, h4 };
  //Float_t H1[5] = { 0, 1, 0, dS,  0 };
  //Float_t H2[5] = { 0, 0, 1,  0, dx };
  //Float_t H3[5] = { 0, 0, 0,  1,  0 };
  //Float_t H4[5] = { 0, 0, 0,  0,  1 };


  fC[0]=( c00  + h2*h2*c22 + h4*h4*c44 
	  + 2*( h2*c20 + h4*c40 + h2*h4*c42 )  ); 

  fC[1]= c10 + h2*c21 + h4*c41 + dS*(c30 + h2*c32 + h4*c43);
  fC[2]= c11 + 2*dS*c31 + dS*dS*c33;

  fC[3]= c20 + h2*c22 + h4*c42 + dx*( c40 + h2*c42 + h4*c44);
  fC[4]= c21 + dS*c32 + dx*(c41 + dS*c43);
  fC[5]= c22 +2*dx*c42 + dx2*c44;

  fC[6]= c30 + h2*c32 + h4*c43;
  fC[7]= c31 + dS*c33;
  fC[8]= c32 + dx*c43;
  fC[9]= c33;

  fC[10]= c40 + h2*c42 + h4*c44;
  fC[11]= c41 + dS*c43;
  fC[12]= c42 + dx*c44;
  fC[13]= c43;
  fC[14]= c44;

  return ret;
}


Bool_t AliHLTTPCCATrackParam::Rotate( Float_t alpha )
{
  //* Rotate the coordinate system in XY on the angle alpha

  Bool_t ret = 1;

  Float_t cA = TMath::Cos( alpha );
  Float_t sA = TMath::Sin( alpha );
  Float_t x = X(), y= Y(), sP= SinPhi(), cP= CosPhi();

  X()      =   x*cA +  y*sA;
  Y()      =  -x*sA +  y*cA;
  CosPhi() =  cP*cA + sP*sA;
  SinPhi() = -cP*sA + sP*cA;

  Float_t j0 = 0, j2 = 0;
    
  if( TMath::Abs(CosPhi())>1.e-4 ) j0 = cP/CosPhi(); else ret = 0;
  if( TMath::Abs(cP)>1.e-4 ) j2 = CosPhi()/cP; else ret = 0;

  //Float_t J[5][5] = { { j0, 0, 0,  0,  0 }, // Y
  //                      {  0, 1, 0,  0,  0 }, // Z
  //                      {  0, 0, j2, 0,  0 }, // SinPhi
  //	                  {  0, 0, 0,  1,  0 }, // DzDs
  //	                  {  0, 0, 0,  0,  1 } }; // Kappa

  fC[0]*= j0*j0;
  fC[1]*= j0;
  //fC[3]*= j0;
  fC[6]*= j0;
  fC[10]*= j0;

  //fC[3]*= j2;
  fC[4]*= j2;
  fC[5]*= j2*j2; 
  fC[8]*= j2;
  fC[12]*= j2;
  return ret;
}


void AliHLTTPCCATrackParam::GetExtParam( AliExternalTrackParam &T, Double_t alpha, Double_t Bz ) const
{
  //* Convert to AliExternalTrackParam parameterisation, 
  //* the angle alpha is the global angle of the local X axis 

  Double_t par[5], cov[15];
  for( Int_t i=0; i<5; i++ ) par[i] = fP[i];
  for( Int_t i=0; i<15; i++ ) cov[i] = fC[i];

  if(par[2]>.999 ) par[2]=.999;
  if(par[2]<-.999 ) par[2]=-.999;

  const Double_t kCLight = 0.000299792458;  
  Double_t c = (TMath::Abs(Bz)>1.e-4) ?1./(Bz*kCLight) :1./(5.*kCLight);
  { // kappa => 1/pt
    par[4] *= c;
    cov[10]*= c;
    cov[11]*= c;
    cov[12]*= c;
    cov[13]*= c;
    cov[14]*= c*c;
  }
  if( GetCosPhi()<0 ){ // change direction
    par[2] = -par[2]; // sin phi
    par[3] = -par[3]; // DzDs
    par[4] = -par[4]; // kappa
    cov[3] = -cov[3];
    cov[4] = -cov[4];
    cov[6] = -cov[6];
    cov[7] = -cov[7];
    cov[10] = -cov[10];
    cov[11] = -cov[11];
  }
  T.Set(GetX(),alpha,par,cov);
}

void AliHLTTPCCATrackParam::SetExtParam( const AliExternalTrackParam &T, Double_t Bz )
{
  //* Convert from AliExternalTrackParam parameterisation
  
  for( Int_t i=0; i<5; i++ ) fP[i] = T.GetParameter()[i];
  for( Int_t i=0; i<15; i++ ) fC[i] = T.GetCovariance()[i];
  X() = T.GetX();
  if(SinPhi()>.999 ) SinPhi()=.999;
  if(SinPhi()<-.999 ) SinPhi()=-.999;
  CosPhi() = TMath::Sqrt(1.-SinPhi()*SinPhi());
  const Double_t kCLight = 0.000299792458;  
  Double_t c = Bz*kCLight;
  { // 1/pt -> kappa 
    fP[4] *= c;
    fC[10]*= c;
    fC[11]*= c;
    fC[12]*= c;
    fC[13]*= c;
    fC[14]*= c*c;
  }
}


void AliHLTTPCCATrackParam::Filter( Float_t y, Float_t z, Float_t erry, Float_t errz )
{
  //* Add the y,z measurement with the Kalman filter 

  Float_t 
    c00 = fC[ 0],
    c10 = fC[ 1], c11 = fC[ 2],
    c20 = fC[ 3], c21 = fC[ 4],
    c30 = fC[ 6], c31 = fC[ 7],
    c40 = fC[10], c41 = fC[11];
  
  Float_t
    z0 = y-fP[0],
    z1 = z-fP[1];

  Float_t v[3] = {erry*erry, 0, errz*errz};

  Float_t mS[3] = { c00+v[0], c10+v[1], c11+v[2] };

  Float_t mSi[3];
  Float_t det = (mS[0]*mS[2] - mS[1]*mS[1]);

  if( TMath::Abs(det)<1.e-8 ) return;
  det = 1./det;
  mSi[0] = mS[2]*det;
  mSi[1] = -mS[1]*det;
  mSi[2] = mS[0]*det;
 
  fNDF  += 2;
  fChi2 += ( +(mSi[0]*z0 + mSi[1]*z1 )*z0
	     +(mSi[1]*z0 + mSi[2]*z1 )*z1 );

  // K = CHtS
  
  Float_t k00, k01 , k10, k11, k20, k21, k30, k31, k40, k41;
    
  k00 = c00*mSi[0] + c10*mSi[1]; k01 = c00*mSi[1] + c10*mSi[2];
  k10 = c10*mSi[0] + c11*mSi[1]; k11 = c10*mSi[1] + c11*mSi[2];
  k20 = c20*mSi[0] + c21*mSi[1]; k21 = c20*mSi[1] + c21*mSi[2];
  k30 = c30*mSi[0] + c31*mSi[1]; k31 = c30*mSi[1] + c31*mSi[2] ;
  k40 = c40*mSi[0] + c41*mSi[1]; k41 = c40*mSi[1] + c41*mSi[2] ;

  Float_t sinPhi = fP[2] + k20*z0  + k21*z1  ;
  if( TMath::Abs(sinPhi)>=0.99 ) return;

  fP[ 0]+= k00*z0  + k01*z1 ;
  fP[ 1]+= k10*z0  + k11*z1  ;
  fP[ 2]+= k20*z0  + k21*z1  ;
  fP[ 3]+= k30*z0  + k31*z1  ;
  fP[ 4]+= k40*z0  + k41*z1  ;

    
  fC[ 0]-= k00*c00 + k01*c10 ;
  
  fC[ 1]-= k10*c00 + k11*c10 ;
  fC[ 2]-= k10*c10 + k11*c11 ;

  fC[ 3]-= k20*c00 + k21*c10 ;
  fC[ 4]-= k20*c10 + k21*c11 ;
  fC[ 5]-= k20*c20 + k21*c21 ;

  fC[ 6]-= k30*c00 + k31*c10 ;
  fC[ 7]-= k30*c10 + k31*c11 ;
  fC[ 8]-= k30*c20 + k31*c21 ;
  fC[ 9]-= k30*c30 + k31*c31 ;

  fC[10]-= k40*c00 + k41*c10 ;
  fC[11]-= k40*c10 + k41*c11 ;
  fC[12]-= k40*c20 + k41*c21 ;
  fC[13]-= k40*c30 + k41*c31 ;
  fC[14]-= k40*c40 + k41*c41 ;
    
  if( CosPhi()>=0 ){
    CosPhi() = TMath::Sqrt(1-SinPhi()*SinPhi());
  }else{
    CosPhi() = -TMath::Sqrt(1-SinPhi()*SinPhi());
  }   
    
}
Commit	Line	Data
d54804bf	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	#include "AliHLTTPCCATrackParam.h"
	20	#include "TMath.h"
	21	#include "AliExternalTrackParam.h"
	22
	23	//ClassImp(AliHLTTPCCATrackParam)
	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
	34
	35	void AliHLTTPCCATrackParam::ConstructXY3( const Float_t x[3], const Float_t y[3],
	36	const Float_t sigmaY2[3], Float_t CosPhi0 )
	37	{
	38	//* Construct the track in XY plane by 3 points
	39
	40	Float_t x0 = x[0];
	41	Float_t y0 = y[0];
	42	Float_t x1 = x[1] - x0;
	43	Float_t y1 = y[1] - y0;
	44	Float_t x2 = x[2] - x0;
	45	Float_t y2 = y[2] - y0;
	46
	47	Float_t a1 = x1x1 + y1y1;
	48	Float_t a2 = x2x2 + y2y2;
	49	Float_t a = 2(x1y2 - y1*x2);
	50	Float_t lx = a1y2 - a2y1;
	51	Float_t ly = -a1x2 + a2x1;
	52	Float_t l = TMath::Sqrt(lxlx + lyly);
	53
	54	Float_t li = 1./l;
	55	Float_t li2 = li*li;
	56	Float_t li3 = li2*li;
	57	Float_t cosPhi = ly*li;
	58
	59	Float_t sinPhi = -lx*li;
	60	Float_t kappa = a/l;
	61
	62	Float_t dlx = a2 - a1; // D lx / D y0
	63	Float_t dly = -a; // D ly / D y0
	64	Float_t dA = 2*(x2 - x1); // D a / D y0
65	Float_t dl = (lxdlx + lydly)*li;
66
67	// D sinPhi,kappa / D y0
68
69	Float_t d0[2] = { -(dlxly-lxdly)lyli3, (dAl-adl)*li2 };
70
71	// D sinPhi,kappa / D y1
72
73	dlx = -a2 + 2y1y2;
74	dly = -2x2y1;
75	dA = -2*x2;
76	dl = (lxdlx + lydly)*li;
77
78	Float_t d1[2] = { -(dlxly-lxdly)lyli3, (dAl-adl)*li2 };
79
80	// D sinPhi,kappa / D y2
81
82	dlx = a1 - 2y1y2;
83	dly = -2x1y2;
84	dA = 2*x1;
85	dl = (lxdlx + lydly)*li;
86
87	Float_t d2[2] = { -(dlxly-lxdly)lyli3, (dAl-adl)*li2 };
88
89	if( CosPhi0*cosPhi <0 ){
90	cosPhi = -cosPhi;
91	sinPhi = -sinPhi;
92	kappa = -kappa;
93	d0[0] = -d0[0];
94	d0[1] = -d0[1];
95	d1[0] = -d1[0];
96	d1[1] = -d1[1];
97	d2[0] = -d2[0];
98	d2[1] = -d2[1];
99	}
100
101	X() = x0;
102	Y() = y0;
103	SinPhi() = sinPhi;
104	Kappa() = kappa;
105	CosPhi() = cosPhi;
106
107	Float_t s0 = sigmaY2[0];
108	Float_t s1 = sigmaY2[1];
109	Float_t s2 = sigmaY2[2];
110
111	fC[0] = s0;
112	fC[1] = 0;
113	fC[2] = 0;
114
115	fC[3] = d0[0]*s0;
116	fC[4] = 0;
117	fC[5] = d0[0]d0[0]s0 + d1[0]d1[0]s1 + d2[0]d2[0]s2;
118
119	fC[6] = 0;
120	fC[7] = 0;
121	fC[8] = 0;
122	fC[9] = 0;
123
124	fC[10] = d0[1]*s0;
125	fC[11] = 0;
126	fC[12] = d0[0]d0[1]s0 + d1[0]d1[1]s1 + d2[0]d2[1]s2;
127	fC[13] = 0;
128	fC[14] = d0[1]d0[1]s0 + d1[1]d1[1]s1 + d2[1]d2[1]s2;
129	}
130
131
132	Float_t AliHLTTPCCATrackParam::GetS( Float_t x, Float_t y ) const
133	{
134	//* Get XY path length to the given point
135
136	Float_t k = GetKappa();
137	Float_t ex = GetCosPhi();
138	Float_t ey = GetSinPhi();
139	x-= GetX();
140	y-= GetY();
141	Float_t dS = xex + yey;
142	if( TMath::Abs(k)>1.e-4 ) dS = TMath::ATan2( kdS, 1+k(xey-yex) )/k;
143	return dS;
144	}
145
146	void AliHLTTPCCATrackParam::GetDCAPoint( Float_t x, Float_t y, Float_t z,
147	Float_t &xp, Float_t &yp, Float_t &zp ) const
148	{
149	//* Get the track point closest to the (x,y,z)
150
151	Float_t x0 = GetX();
152	Float_t y0 = GetY();
153	Float_t k = GetKappa();
154	Float_t ex = GetCosPhi();
155	Float_t ey = GetSinPhi();
156	Float_t dx = x - x0;
157	Float_t dy = y - y0;
158	Float_t ax = dx*k+ey;
159	Float_t ay = dy*k-ex;
160	Float_t a = sqrt( axax+ayay );
161	xp = x0 + (dx - ey( (dxdx+dydy)k - 2(-dxey+dy*ex) )/(a+1) )/a;
162	yp = y0 + (dy + ex( (dxdx+dydy)k - 2(-dxey+dy*ex) )/(a+1) )/a;
163	Float_t s = GetS(x,y);
164	zp = GetZ() + GetDzDs()*s;
165	if( TMath::Abs(k)>1.e-2 ){
166	Float_t dZ = TMath::Abs( GetDzDs()*TMath::TwoPi()/k );
167	if( dZ>.1 ){
168	zp+= TMath::Nint((z-zp)/dZ)*dZ;
169	}
170	}
171	}
172
173	void AliHLTTPCCATrackParam::ConstructXYZ3( const Float_t p0[5], const Float_t p1[5],
174	const Float_t p2[5],
175	Float_t CosPhi0, Float_t t0[] )
176	{
177	//* Construct the track in XYZ by 3 points
178
179	Float_t px[3] = { p0[0], p1[0], p2[0] };
180	Float_t py[3] = { p0[1], p1[1], p2[1] };
181	Float_t pz[3] = { p0[2], p1[2], p2[2] };
182	Float_t ps2y[3] = { p0[3]p0[3], p1[3]p1[3], p2[3]*p2[3] };
183	Float_t ps2z[3] = { p0[4]p0[4], p1[4]p1[4], p2[4]*p2[4] };
184
185	Float_t kold = t0 ?t0[4] :0;
186	ConstructXY3( px, py, ps2y, CosPhi0 );
187
188	Float_t pS[3] = { GetS(px[0],py[0]), GetS(px[1],py[1]), GetS(px[2],py[2]) };
189	Float_t k = Kappa();
190	if( TMath::Abs(k)>1.e-2 ){
191	Float_t dS = TMath::Abs( TMath::TwoPi()/k );
192	pS[1]+= TMath::Nint( (pS[0]-pS[1])/dS )*dS; // not more than half turn
193	pS[2]+= TMath::Nint( (pS[1]-pS[2])/dS )*dS;
194	if( t0 ){
195	Float_t dZ = TMath::Abs(t0[3]*dS);
196	if( TMath::Abs(dZ)>1. ){
197	Float_t dsDz = 1./t0[3];
198	if( kold*k<0 ) dsDz = -dsDz;
199	Float_t s0 = (pz[0]-t0[1])*dsDz;
200	Float_t s1 = (pz[1]-t0[1])*dsDz;
201	Float_t s2 = (pz[2]-t0[1])*dsDz;
202	pS[0]+= TMath::Nint( (s0-pS[0])/dS )*dS ;
203	pS[1]+= TMath::Nint( (s1-pS[1])/dS )*dS ;
204	pS[2]+= TMath::Nint( (s2-pS[2])/dS )*dS ;
205	}
206	}
207	}
208
209	Float_t s = pS[0] + pS[1] + pS[2];
210	Float_t z = pz[0] + pz[1] + pz[2];
211	Float_t sz = pS[0]pz[0] + pS[1]pz[1] + pS[2]*pz[2];
212	Float_t ss = pS[0]pS[0] + pS[1]pS[1] + pS[2]*pS[2];
213
214	Float_t a = 3ss-ss;
215	Z() = (zss-szs)/a; // z0
216	DzDs() = (3sz-zs)/a; // t = dz/ds
217
218	Float_t dz0[3] = {ss - pS[0]s,ss - pS[1]s,ss - pS[2]*s };
219	Float_t dt [3] = {3pS[0] - s, 3pS[1] - s, 3*pS[2] - s };
220
221	fC[2] = (dz0[0]dz0[0]ps2z[0] + dz0[1]dz0[1]ps2z[1] + dz0[2]dz0[2]ps2z[2])/a/a;
222	fC[7]= (dz0[0]dt [0]ps2z[0] + dz0[1]dt [1]ps2z[1] + dz0[2]dt [2]ps2z[2])/a/a;
223	fC[9]= (dt [0]dt [0]ps2z[0] + dt [1]dt [1]ps2z[1] + dt [2]dt [2]ps2z[2])/a/a;
224	}
225
226
227	Bool_t AliHLTTPCCATrackParam::TransportToX( Float_t x )
228	{
229	//* Transport the track parameters to X=x
230
231	Bool_t ret = 1;
232
233	Float_t x0 = X();
234	//Float_t y0 = Y();
235	Float_t k = Kappa();
236	Float_t ex = CosPhi();
237	Float_t ey = SinPhi();
238	Float_t dx = x - x0;
239
240	Float_t ey1 = k*dx + ey;
241	Float_t ex1;
242	if( TMath::Abs(ey1)>1 ){ // no intersection -> check the border
243	ey1 = ( ey1>0 ) ?1 :-1;
244	ex1 = 0;
245	dx = ( TMath::Abs(k)>1.e-4) ? ( (ey1-ey)/k ) :0;
246
247	Float_t ddx = TMath::Abs(x0+dx - x)kk;
248	Float_t hx[] = {0, -k, 1+ey };
249	Float_t sx2 = hx[1]hx[1]fC[ 3] + hx[2]hx[2]fC[ 5];
250	if( ddxddx>3.53.5*sx2 ) ret = 0; // x not withing the error
251	ret = 0; // any case
252	return ret;
253	}else{
254	ex1 = TMath::Sqrt(1 - ey1*ey1);
255	if( ex<0 ) ex1 = -ex1;
256	}
257
258	Float_t dx2 = dx*dx;
259	CosPhi() = ex1;
260	Float_t ss = ey+ey1;
261	Float_t cc = ex+ex1;
262	Float_t tg = 0;
263	if( TMath::Abs(cc)>1.e-4 ) tg = ss/cc; // tan((phi1+phi)/2)
264	else ret = 0;
265	Float_t dy = dx*tg;
266	Float_t dl = dxTMath::Sqrt(1+tgtg);
267
268	if( cc<0 ) dl = -dl;
269	Float_t dSin = dl*k/2;
270	if( dSin > 1 ) dSin = 1;
271	if( dSin <-1 ) dSin = -1;
272	Float_t dS = ( TMath::Abs(k)>1.e-4) ? (2*TMath::ASin(dSin)/k) :dl;
273	Float_t dz = dS*DzDs();
274
275	Float_t cci = 0, exi = 0, ex1i = 0;
276	if( TMath::Abs(cc)>1.e-4 ) cci = 1./cc;
277	else ret = 0;
278	if( TMath::Abs(ex)>1.e-4 ) exi = 1./ex;
279	else ret = 0;
280	if( TMath::Abs(ex1)>1.e-4 ) ex1i = 1./ex1;
281	else ret = 0;
282
283	if( !ret ) return ret;
284	X() += dx;
285	fP[0]+= dy;
286	fP[1]+= dz;
287	fP[2] = ey1;
288	fP[3] = fP[3];
289	fP[4] = fP[4];
290
291	Float_t h2 = dx(1+ exex1 + eyey1 )cciexiex1i;
292	Float_t h4 = dx2(cc + ssey1ex1i )cci*cci;
293
294	Float_t c00 = fC[0];
295	Float_t c10 = fC[1];
296	Float_t c11 = fC[2];
297	Float_t c20 = fC[3];
298	Float_t c21 = fC[4];
299	Float_t c22 = fC[5];
300	Float_t c30 = fC[6];
301	Float_t c31 = fC[7];
302	Float_t c32 = fC[8];
303	Float_t c33 = fC[9];
304	Float_t c40 = fC[10];
305	Float_t c41 = fC[11];
306	Float_t c42 = fC[12];
307	Float_t c43 = fC[13];
308	Float_t c44 = fC[14];
309
310	//Float_t H0[5] = { 1,0, h2, 0, h4 };
311	//Float_t H1[5] = { 0, 1, 0, dS, 0 };
312	//Float_t H2[5] = { 0, 0, 1, 0, dx };
313	//Float_t H3[5] = { 0, 0, 0, 1, 0 };
314	//Float_t H4[5] = { 0, 0, 0, 0, 1 };
315
316
317	fC[0]=( c00 + h2h2c22 + h4h4c44
318	+ 2( h2c20 + h4c40 + h2h4*c42 ) );
319
320	fC[1]= c10 + h2c21 + h4c41 + dS(c30 + h2c32 + h4*c43);
321	fC[2]= c11 + 2dSc31 + dSdSc33;
322
323	fC[3]= c20 + h2c22 + h4c42 + dx( c40 + h2c42 + h4*c44);
324	fC[4]= c21 + dSc32 + dx(c41 + dS*c43);
325	fC[5]= c22 +2dxc42 + dx2*c44;
326
327	fC[6]= c30 + h2c32 + h4c43;
328	fC[7]= c31 + dS*c33;
329	fC[8]= c32 + dx*c43;
330	fC[9]= c33;
331
332	fC[10]= c40 + h2c42 + h4c44;
333	fC[11]= c41 + dS*c43;
334	fC[12]= c42 + dx*c44;
335	fC[13]= c43;
336	fC[14]= c44;
337
338	return ret;
339	}
340
341
342
343	Bool_t AliHLTTPCCATrackParam::Rotate( Float_t alpha )
344	{
345	//* Rotate the coordinate system in XY on the angle alpha
346
347	Bool_t ret = 1;
348
349	Float_t cA = TMath::Cos( alpha );
350	Float_t sA = TMath::Sin( alpha );
351	Float_t x = X(), y= Y(), sP= SinPhi(), cP= CosPhi();
352
353	X() = xcA + ysA;
354	Y() = -xsA + ycA;
355	CosPhi() = cPcA + sPsA;
356	SinPhi() = -cPsA + sPcA;
357
358	Float_t j0 = 0, j2 = 0;
359
360	if( TMath::Abs(CosPhi())>1.e-4 ) j0 = cP/CosPhi(); else ret = 0;
361	if( TMath::Abs(cP)>1.e-4 ) j2 = CosPhi()/cP; else ret = 0;
362
363	//Float_t J[5][5] = { { j0, 0, 0, 0, 0 }, // Y
364	// { 0, 1, 0, 0, 0 }, // Z
365	// { 0, 0, j2, 0, 0 }, // SinPhi
366	// { 0, 0, 0, 1, 0 }, // DzDs
367	// { 0, 0, 0, 0, 1 } }; // Kappa
368
369	fC[0]= j0j0;
370	fC[1]*= j0;
371	//fC[3]*= j0;
372	fC[6]*= j0;
373	fC[10]*= j0;
374
375	//fC[3]*= j2;
376	fC[4]*= j2;
377	fC[5]= j2j2;
378	fC[8]*= j2;
379	fC[12]*= j2;
380	return ret;
381	}
382
383
384	void AliHLTTPCCATrackParam::GetExtParam( AliExternalTrackParam &T, Double_t alpha, Double_t Bz ) const
385	{
386	//* Convert to AliExternalTrackParam parameterisation,
387	//* the angle alpha is the global angle of the local X axis
388
389	Double_t par[5], cov[15];
390	for( Int_t i=0; i<5; i++ ) par[i] = fP[i];
391	for( Int_t i=0; i<15; i++ ) cov[i] = fC[i];
392
393	if(par[2]>.999 ) par[2]=.999;
394	if(par[2]<-.999 ) par[2]=-.999;
395
396	const Double_t kCLight = 0.000299792458;
5b41cd63	397	Double_t c = (TMath::Abs(Bz)>1.e-4) ?1./(BzkCLight) :1./(5.kCLight);
d54804bf	398	{ // kappa => 1/pt
	399	par[4] *= c;
	400	cov[10]*= c;
	401	cov[11]*= c;
	402	cov[12]*= c;
	403	cov[13]*= c;
	404	cov[14]= cc;
	405	}
	406	if( GetCosPhi()<0 ){ // change direction
	407	par[2] = -par[2]; // sin phi
	408	par[3] = -par[3]; // DzDs
	409	par[4] = -par[4]; // kappa
	410	cov[3] = -cov[3];
	411	cov[4] = -cov[4];
	412	cov[6] = -cov[6];
	413	cov[7] = -cov[7];
	414	cov[10] = -cov[10];
	415	cov[11] = -cov[11];
	416	}
	417	T.Set(GetX(),alpha,par,cov);
	418	}
	419
	420	void AliHLTTPCCATrackParam::SetExtParam( const AliExternalTrackParam &T, Double_t Bz )
	421	{
	422	//* Convert from AliExternalTrackParam parameterisation
	423
	424	for( Int_t i=0; i<5; i++ ) fP[i] = T.GetParameter()[i];
	425	for( Int_t i=0; i<15; i++ ) fC[i] = T.GetCovariance()[i];
	426	X() = T.GetX();
	427	if(SinPhi()>.999 ) SinPhi()=.999;
	428	if(SinPhi()<-.999 ) SinPhi()=-.999;
	429	CosPhi() = TMath::Sqrt(1.-SinPhi()*SinPhi());
	430	const Double_t kCLight = 0.000299792458;
	431	Double_t c = Bz*kCLight;
	432	{ // 1/pt -> kappa
	433	fP[4] *= c;
	434	fC[10]*= c;
	435	fC[11]*= c;
	436	fC[12]*= c;
	437	fC[13]*= c;
	438	fC[14]= cc;
	439	}
	440	}
	441
	442
	443	void AliHLTTPCCATrackParam::Filter( Float_t y, Float_t z, Float_t erry, Float_t errz )
	444	{
	445	//* Add the y,z measurement with the Kalman filter
	446
	447	Float_t
	448	c00 = fC[ 0],
	449	c10 = fC[ 1], c11 = fC[ 2],
	450	c20 = fC[ 3], c21 = fC[ 4],
	451	c30 = fC[ 6], c31 = fC[ 7],
	452	c40 = fC[10], c41 = fC[11];
	453
	454	Float_t
	455	z0 = y-fP[0],
	456	z1 = z-fP[1];
	457
	458	Float_t v[3] = {erryerry, 0, errzerrz};
	459
	460	Float_t mS[3] = { c00+v[0], c10+v[1], c11+v[2] };
	461
462	Float_t mSi[3];
463	Float_t det = (mS[0]mS[2] - mS[1]mS[1]);
464
465	if( TMath::Abs(det)<1.e-8 ) return;
466	det = 1./det;
467	mSi[0] = mS[2]*det;
468	mSi[1] = -mS[1]*det;
469	mSi[2] = mS[0]*det;
470
471	fNDF += 2;
472	fChi2 += ( +(mSi[0]z0 + mSi[1]z1 )*z0
473	+(mSi[1]z0 + mSi[2]z1 )*z1 );
474
475	// K = CHtS
476
477	Float_t k00, k01 , k10, k11, k20, k21, k30, k31, k40, k41;
478
479	k00 = c00mSi[0] + c10mSi[1]; k01 = c00mSi[1] + c10mSi[2];
480	k10 = c10mSi[0] + c11mSi[1]; k11 = c10mSi[1] + c11mSi[2];
481	k20 = c20mSi[0] + c21mSi[1]; k21 = c20mSi[1] + c21mSi[2];
482	k30 = c30mSi[0] + c31mSi[1]; k31 = c30mSi[1] + c31mSi[2] ;
483	k40 = c40mSi[0] + c41mSi[1]; k41 = c40mSi[1] + c41mSi[2] ;
484
485	Float_t sinPhi = fP[2] + k20z0 + k21z1 ;
486	if( TMath::Abs(sinPhi)>=0.99 ) return;
487
488	fP[ 0]+= k00z0 + k01z1 ;
489	fP[ 1]+= k10z0 + k11z1 ;
490	fP[ 2]+= k20z0 + k21z1 ;
491	fP[ 3]+= k30z0 + k31z1 ;
492	fP[ 4]+= k40z0 + k41z1 ;
493
494
495	fC[ 0]-= k00c00 + k01c10 ;
496
497	fC[ 1]-= k10c00 + k11c10 ;
498	fC[ 2]-= k10c10 + k11c11 ;
499
500	fC[ 3]-= k20c00 + k21c10 ;
501	fC[ 4]-= k20c10 + k21c11 ;
502	fC[ 5]-= k20c20 + k21c21 ;
503
504	fC[ 6]-= k30c00 + k31c10 ;
505	fC[ 7]-= k30c10 + k31c11 ;
506	fC[ 8]-= k30c20 + k31c21 ;
507	fC[ 9]-= k30c30 + k31c31 ;
508
509	fC[10]-= k40c00 + k41c10 ;
510	fC[11]-= k40c10 + k41c11 ;
511	fC[12]-= k40c20 + k41c21 ;
512	fC[13]-= k40c30 + k41c31 ;
513	fC[14]-= k40c40 + k41c41 ;
514
515	if( CosPhi()>=0 ){
516	CosPhi() = TMath::Sqrt(1-SinPhi()*SinPhi());
517	}else{
518	CosPhi() = -TMath::Sqrt(1-SinPhi()*SinPhi());
519	}
520
521	}