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

// @(#) $Id$
//*************************************************************************
// This file is property of and copyright by the ALICE HLT Project        * 
// ALICE Experiment at CERN, All rights reserved.                         *
//                                                                        *
// Primary Authors: Jochen Thaeder <thaeder@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 "AliHLTTPCCATrackPar.h"
#include "TMath.h"

ClassImp(AliHLTTPCCATrackPar);

void AliHLTTPCCATrackPar::Init()
{
  //* Initialization 

  for( Int_t i=0; i<7; i++ ) fP[i] = 0;
  for( Int_t i=0; i<28; i++ ) fC[i] = 0;
  fC[0] = fC[2] = fC[5] = 10000;
  fC[9] = fC[14] = fC[20] = 10000.;
  fC[27] = 10.;
  fChi2 = 0;
  fNDF = -5;
}


void AliHLTTPCCATrackPar::Normalize( Double_t Direction[3] )
{
  //* Normalize the track

  Double_t p2 = fP[3]*fP[3] + fP[4]*fP[4] + fP[5]*fP[5];
  if( p2<1.e-4 ) return;
  Double_t a2 = 1./p2;
  Double_t a = sqrt(a2);

  if( Direction && ( fP[3]*Direction[0] + fP[4]*Direction[1] + fP[5]*Direction[2] < 0 ) ) a = -a;
  
  Double_t ex = fP[3]*a, ey = fP[4]*a, ez = fP[5]*a, qp = fP[6]*a;

  fP[3] = ex;
  fP[4] = ey;
  fP[5] = ez;
  fP[6] = qp;

  Double_t 
    h0 = fC[ 6]*ex + fC[10]*ey + fC[15]*ez,
    h1 = fC[ 7]*ex + fC[11]*ey + fC[16]*ez, 
    h2 = fC[ 8]*ex + fC[12]*ey + fC[17]*ez,
    h3 = fC[ 9]*ex + fC[13]*ey + fC[18]*ez, 
    h4 = fC[13]*ex + fC[14]*ey + fC[19]*ez, 
    h5 = fC[18]*ex + fC[19]*ey + fC[20]*ez,
    h6 = fC[24]*ex + fC[25]*ey + fC[26]*ez,
    d  = h3*ex + h4*ey + h5*ez, 
    hh = h6 - qp*d ;

  fC[ 6]= a*(fC[ 6] -ex*h0); fC[ 7]= a*(fC[ 7] -ex*h1); fC[ 8]= a*(fC[ 8] -ex*h2); 
  fC[10]= a*(fC[10] -ey*h0); fC[11]= a*(fC[11] -ey*h1); fC[12]= a*(fC[12] -ey*h2); 
  fC[15]= a*(fC[15] -ez*h0); fC[16]= a*(fC[16] -ez*h1); fC[17]= a*(fC[17] -ez*h2); 
  fC[21]= a*(fC[21] -qp*h0); fC[22]= a*(fC[22] -qp*h1); fC[23]= a*(fC[23] -qp*h2);     

  fC[ 9]= a2*( fC[ 9] -h3*ex -h3*ex + d*ex*ex );
  fC[13]= a2*( fC[13] -h4*ex -h3*ey + d*ey*ex ); 
  fC[14]= a2*( fC[14] -h4*ey -h4*ey + d*ey*ey );

  fC[18]= a2*( fC[18] -h5*ex -h3*ez + d*ez*ex ); 
  fC[19]= a2*( fC[19] -h5*ey -h4*ez + d*ez*ey ); 
  fC[20]= a2*( fC[20] -h5*ez -h5*ez + d*ez*ez );

  fC[24]= a2*( fC[24] -qp*h3 - hh*ex ); 
  fC[25]= a2*( fC[25] -qp*h4 - hh*ey ); 
  fC[26]= a2*( fC[26] -qp*h5 - hh*ez );
  fC[27]= a2*( fC[27] -qp*h6 - hh*qp  ); 
}


Double_t AliHLTTPCCATrackPar::GetDsToPointBz( Double_t Bz, const Double_t xyz[3], const Double_t *T0 ) const
{
  //* Get dS to a certain space point for Bz field
  const Double_t kCLight = 0.000299792458;
  Double_t bq = Bz*T0[6]*kCLight;
  Double_t pt2 = T0[3]*T0[3] + T0[4]*T0[4];
  if( pt2<1.e-4 ) return 0;
  Double_t dx = xyz[0] - T0[0];
  Double_t dy = xyz[1] - T0[1]; 
  Double_t a = dx*T0[3]+dy*T0[4];
  Double_t dS = 0;
  if( TMath::Abs(bq)<1.e-8 ) dS = a/pt2;
  else dS = TMath::ATan2( bq*a, pt2 + bq*(dy*T0[3] -dx*T0[4]) )/bq;
  return dS;
}

Double_t AliHLTTPCCATrackPar::GetDsToPointBz( Double_t Bz, const Double_t xyz[3] ) const 
{
  return GetDsToPointBz( Bz, xyz, fP );
}

void AliHLTTPCCATrackPar::TransportBz( Double_t Bz, Double_t S, Double_t *T0 ) 
{ 
  //* Transport the particle on dS, for Bz field
 
  const Double_t kCLight = 0.000299792458;
  Bz = Bz*kCLight;
  Double_t bs= Bz*S;
  Double_t bqs= bs*T0[6];
  Double_t s = TMath::Sin(bqs), c = TMath::Cos(bqs);
  Double_t sB, cB, dsB, dcB;
  if( TMath::Abs(bqs)>1.e-10){
    sB= s/Bz/T0[6];
    cB= (1-c)/Bz/T0[6];
    dsB = (c*S - sB)/T0[6];
    dcB = (s*S-cB)/T0[6];
  }else{
    sB = (1. - bqs*bqs/6.)*S;
    cB = .5*sB*bqs;
    dsB = - T0[6]*bs*bs/3.*S;
    dcB = .5*(sB*bs - dsB*bqs);
  }
  
  Double_t px = T0[3];
  Double_t py = T0[4];
  Double_t pz = T0[5];
  
  Double_t d[7] = { fP[0]-T0[0], fP[1]-T0[1], fP[2]-T0[2], 
		    fP[3]-T0[3], fP[4]-T0[4], fP[5]-T0[5], fP[6]-T0[6] };

  T0[0] = T0[0] + sB*px + cB*py;
  T0[1] = T0[1] - cB*px + sB*py;
  T0[2] = T0[2] +  S*pz                       ;
  T0[3] =          c*px + s*py;
  T0[4] =         -s*px + c*py;
  T0[5] = T0[5];
  T0[6] = T0[6];

 
  Double_t mJ[7][7] = { {1,0,0,   sB, cB,  0,   dsB*px + dcB*py },
                        {0,1,0,  -cB, sB,  0, - dcB*px + dsB*py },
                        {0,0,1,    0,  0,  S,   0               },
                        {0,0,0,    c,  s,  0,   (-s*px + c*py)*bs },
                        {0,0,0,   -s,  c,  0,   (-c*px - s*py)*bs },
                        {0,0,0,    0,  0,  1,   0                 },
                        {0,0,0,    0,  0,  0,   1                 }  };

  for( Int_t i=0; i<7; i++){
    fP[i] = T0[i];
    for( Int_t j=0; j<7; j++) fP[i] += mJ[i][j]*d[j];
  }

  Double_t mA[7][7];
  for( Int_t k=0,i=0; i<7; i++)
    for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = fC[k]; 

  Double_t mJC[7][7];
  for( Int_t i=0; i<7; i++ )
    for( Int_t j=0; j<7; j++ ){
      mJC[i][j]=0;
      for( Int_t k=0; k<7; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
    }
  
  for( Int_t k=0,i=0; i<7; i++)
    for( Int_t j=0; j<=i; j++, k++ ){
      fC[k] = 0;
      for( Int_t l=0; l<7; l++ ) fC[k]+=mJC[i][l]*mJ[j][l];
    }
}

void AliHLTTPCCATrackPar::TransportBz( Double_t Bz, Double_t dS ) 
{ 
  //* Transport the particle on dS, for Bz field
  TransportBz( Bz, dS, fP );
}

void AliHLTTPCCATrackPar::GetConnectionMatrix( Double_t B, const Double_t p[3], Double_t G[6], const Double_t *T0  ) const 
{
  //* Calculate connection matrix between track and point p
  if( !G ) return;
  const Double_t kLight = 0.000299792458;
  B*=kLight;
  Double_t dx = p[0]-T0[0], dy = p[1]-T0[1], dz = p[2]-T0[2];
  Double_t px2= T0[3]*T0[3], py2= T0[4]*T0[4], pz2= T0[5]*T0[5];
  //Double_t B2 = B*B;
  Double_t s2 = (dx*dx + dy*dy + dz*dz);
  Double_t p2 = px2 + py2 + pz2;
  if( p2>1.e-4 ) s2/=p2;
  Double_t x = T0[3]*s2;
  Double_t xx= px2*s2, xy= x*T0[4], xz= x*T0[5], yy= py2*s2, yz= T0[4]*T0[5]*s2;
  //Double_t Bxy= B*xy; 
  G[ 0]= xx;
  G[ 1]= xy;   G[ 2]= yy;
  G[ 3]= xz;   G[ 4]= yz;   G[ 5]= pz2*s2;  
  /*
  C[ 0]+= xx;
  C[ 1]+= xy;   C[ 2]+= yy;
  C[ 3]+= xz;   C[ 4]+= yz;   C[ 5]+= pz2*s2;  
  C[ 6]+= Bxy; C[ 7]+= B*yy; C[ 8]+= B*yz;    C[ 9]+=B2*yy;
  C[10]-= B*xx; C[11]-= Bxy; C[12]-=B*xz;     C[13]-=B2*xy; C[14]+=B2*xx;  
  */
}


void AliHLTTPCCATrackPar::Filter( const Double_t m[3], const Double_t V[6], const Double_t V1[6] )
{
  //* !
  Double_t 
    c00 = fC[ 0],
    c10 = fC[ 1], c11 = fC[ 2],
    c20 = fC[ 3], c21 = fC[ 4], c22 = fC[ 5],
    c30 = fC[ 6], c31 = fC[ 7], c32 = fC[ 8],
    c40 = fC[10], c41 = fC[11], c42 = fC[12],
    c50 = fC[15], c51 = fC[16], c52 = fC[17],
    c60 = fC[21], c61 = fC[22], c62 = fC[23];
  
  double
    z0 = m[0]-fP[0],
    z1 = m[1]-fP[1],
    z2 = m[2]-fP[2];
  
  Double_t mS[6] = { c00+V[0]+V1[0], c10+V[1]+V1[1], c11+V[2]+V1[2],
		     c20+V[3]+V1[3], c21+V[4]+V1[4], c22+V[5]+V1[5] };
  Double_t mSi[6];
  mSi[0] = mS[4]*mS[4] - mS[2]*mS[5];
  mSi[1] = mS[1]*mS[5] - mS[3]*mS[4];
  mSi[3] = mS[2]*mS[3] - mS[1]*mS[4];
  Double_t det = (mS[0]*mSi[0] + mS[1]*mSi[1] + mS[3]*mSi[3]);
  if( TMath::Abs(det)<1.e-10 ) return;
  det = 1./det;
  mSi[0] *= det;
  mSi[1] *= det;
  mSi[3] *= det;
  mSi[2] = ( mS[3]*mS[3] - mS[0]*mS[5] )*det;
  mSi[4] = ( mS[0]*mS[4] - mS[1]*mS[3] )*det;
  mSi[5] = ( mS[1]*mS[1] - mS[0]*mS[2] )*det;
  
  fNDF  += 2;
  fChi2 += ( +(mSi[0]*z0 + mSi[1]*z1 + mSi[3]*z2)*z0
	    +(mSi[1]*z0 + mSi[2]*z1 + mSi[4]*z2)*z1
	    +(mSi[3]*z0 + mSi[4]*z1 + mSi[5]*z2)*z2 );
        
  Double_t k0, k1, k2 ; // k = CHtS
    
  k0 = c00*mSi[0] + c10*mSi[1] + c20*mSi[3];
  k1 = c00*mSi[1] + c10*mSi[2] + c20*mSi[4];
  k2 = c00*mSi[3] + c10*mSi[4] + c20*mSi[5];
    
  fP[ 0]+= k0*z0  + k1*z1  + k2*z2 ;
  fC[ 0]-= k0*c00 + k1*c10 + k2*c20;
  
  k0 = c10*mSi[0] + c11*mSi[1] + c21*mSi[3];
  k1 = c10*mSi[1] + c11*mSi[2] + c21*mSi[4];
  k2 = c10*mSi[3] + c11*mSi[4] + c21*mSi[5];
  
  fP[ 1]+= k0*z0  + k1*z1  + k2*z2 ;
  fC[ 1]-= k0*c00 + k1*c10 + k2*c20;
  fC[ 2]-= k0*c10 + k1*c11 + k2*c21;
  
  k0 = c20*mSi[0] + c21*mSi[1] + c22*mSi[3];
  k1 = c20*mSi[1] + c21*mSi[2] + c22*mSi[4];
  k2 = c20*mSi[3] + c21*mSi[4] + c22*mSi[5];
  
  fP[ 2]+= k0*z0  + k1*z1  + k2*z2 ;
  fC[ 3]-= k0*c00 + k1*c10 + k2*c20;
  fC[ 4]-= k0*c10 + k1*c11 + k2*c21;
  fC[ 5]-= k0*c20 + k1*c21 + k2*c22;
  
  k0 = c30*mSi[0] + c31*mSi[1] + c32*mSi[3];
  k1 = c30*mSi[1] + c31*mSi[2] + c32*mSi[4];
  k2 = c30*mSi[3] + c31*mSi[4] + c32*mSi[5];
  
  fP[ 3]+= k0*z0  + k1*z1  + k2*z2 ;
  fC[ 6]-= k0*c00 + k1*c10 + k2*c20;
  fC[ 7]-= k0*c10 + k1*c11 + k2*c21;
  fC[ 8]-= k0*c20 + k1*c21 + k2*c22;
  fC[ 9]-= k0*c30 + k1*c31 + k2*c32;
  
  k0 = c40*mSi[0] + c41*mSi[1] + c42*mSi[3];
  k1 = c40*mSi[1] + c41*mSi[2] + c42*mSi[4];
  k2 = c40*mSi[3] + c41*mSi[4] + c42*mSi[5];
    
  fP[ 4]+= k0*z0  + k1*z1  + k2*z2 ;
  fC[10]-= k0*c00 + k1*c10 + k2*c20;
  fC[11]-= k0*c10 + k1*c11 + k2*c21;
  fC[12]-= k0*c20 + k1*c21 + k2*c22;
  fC[13]-= k0*c30 + k1*c31 + k2*c32;
  fC[14]-= k0*c40 + k1*c41 + k2*c42;

  k0 = c50*mSi[0] + c51*mSi[1] + c52*mSi[3];
  k1 = c50*mSi[1] + c51*mSi[2] + c52*mSi[4];
  k2 = c50*mSi[3] + c51*mSi[4] + c52*mSi[5];
  
  fP[ 5]+= k0*z0  + k1*z1  + k2*z2 ;
  fC[15]-= k0*c00 + k1*c10 + k2*c20;
  fC[16]-= k0*c10 + k1*c11 + k2*c21;
  fC[17]-= k0*c20 + k1*c21 + k2*c22;
  fC[18]-= k0*c30 + k1*c31 + k2*c32;
  fC[19]-= k0*c40 + k1*c41 + k2*c42;
  fC[20]-= k0*c50 + k1*c51 + k2*c52;

  k0 = c60*mSi[0] + c61*mSi[1] + c62*mSi[3];
  k1 = c60*mSi[1] + c61*mSi[2] + c62*mSi[4];
  k2 = c60*mSi[3] + c61*mSi[4] + c62*mSi[5];
  
  fP[ 6]+= k0*z0  + k1*z1  + k2*z2 ;
  fC[21]-= k0*c00 + k1*c10 + k2*c20;
  fC[22]-= k0*c10 + k1*c11 + k2*c21;
  fC[23]-= k0*c20 + k1*c21 + k2*c22;
  fC[24]-= k0*c30 + k1*c31 + k2*c32;
  fC[25]-= k0*c40 + k1*c41 + k2*c42;
  fC[26]-= k0*c50 + k1*c51 + k2*c52;
  fC[27]-= k0*c60 + k1*c61 + k2*c62;
}


void AliHLTTPCCATrackPar::Rotate( Double_t alpha )
{
  //* !
  Double_t cA = TMath::Cos( alpha );
  Double_t sA = TMath::Sin( alpha );
  Double_t x= fP[0], y= fP[1], px= fP[3], py= fP[4];
  fP[0] = x*cA + y*sA;
  fP[1] =-x*sA + y*cA;
  fP[2] = fP[2];
  fP[3] = px*cA + py*sA;
  fP[4] =-px*sA + py*cA;
  fP[5] = fP[5];
  fP[6] = fP[6];

  Double_t mJ[7][7] = { { cA,sA, 0,  0,  0,  0,  0 },
                        {-sA,cA, 0,  0,  0,  0,  0 },
                        {  0, 0, 1,  0,  0,  0,  0 },
                        {  0, 0, 0, cA, sA,  0,  0 },
                        {  0, 0, 0,-sA, cA,  0,  0 },
                        {  0, 0, 0,  0,  0,  1,  0 },
                        {  0, 0, 0,  0,  0,  0,  1 }  };

  Double_t mA[7][7];
  for( Int_t k=0,i=0; i<7; i++)
    for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = fC[k]; 

  Double_t mJC[7][7];
  for( Int_t i=0; i<7; i++ )
    for( Int_t j=0; j<7; j++ ){
      mJC[i][j]=0;
      for( Int_t k=0; k<7; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
    }
  
  for( Int_t k=0,i=0; i<7; i++)
    for( Int_t j=0; j<=i; j++, k++ ){
      fC[k] = 0;
      for( Int_t l=0; l<7; l++ ) fC[k]+=mJC[i][l]*mJ[j][l];
    }
}


void AliHLTTPCCATrackPar::ConvertTo5( Double_t alpha, Double_t T[], Double_t C[] )
const {
  //* !
  AliHLTTPCCATrackPar t = *this;
  t.Rotate(alpha);
  Double_t 
    x= t.fP[0], y= t.fP[1], z = t.fP[2], 
    ex= t.fP[3], ey= t.fP[4], ez = t.fP[5], qp = t.fP[6];

  Double_t p2 = ex*ex+ey*ey+ez*ez;
  if( p2<1.e-4 ) p2 = 1;
  Double_t n2 = 1./p2;
  Double_t n = sqrt(n2);

  T[5] = x;
  T[0] = y;
  T[1] = z;
  T[2] = ey/ex;
  T[3] = ez/ex;
  T[4] = qp*n;

  Double_t mJ[5][7] = { { -T[2], 1, 0,  0,  0,  0,  0 },
                        { -T[3], 0, 1,  0,  0,  0,  0 },
                        { 0, 0, 0,  -T[2]/ex,  1./ex,  0,  0 },
                        { 0, 0, 0, -T[3]/ex,  0,  1./ex,  0 },
                        { 0, 0, 0, -T[4]*n2*ex, -T[4]*n2*ey, -T[4]*n2*ez, n }};

  Double_t mA[7][7];
  for( Int_t k=0,i=0; i<7; i++)
    for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = t.fC[k]; 

  Double_t mJC[5][7];
  for( Int_t i=0; i<5; i++ )
    for( Int_t j=0; j<7; j++ ){
      mJC[i][j]=0;
      for( Int_t k=0; k<7; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
    }
  
  for( Int_t k=0,i=0; i<5; i++)
    for( Int_t j=0; j<=i; j++, k++ ){
      C[k] = 0;
      for( Int_t l=0; l<7; l++ ) C[k]+=mJC[i][l]*mJ[j][l];
    }
}
Commit	Line	Data
326c2d4b	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: Jochen Thaeder <thaeder@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 "AliHLTTPCCATrackPar.h"
	20	#include "TMath.h"
	21
	22	ClassImp(AliHLTTPCCATrackPar);
	23
	24	void AliHLTTPCCATrackPar::Init()
	25	{
	26	//* Initialization
	27
	28	for( Int_t i=0; i<7; i++ ) fP[i] = 0;
	29	for( Int_t i=0; i<28; i++ ) fC[i] = 0;
	30	fC[0] = fC[2] = fC[5] = 10000;
	31	fC[9] = fC[14] = fC[20] = 10000.;
	32	fC[27] = 10.;
	33	fChi2 = 0;
	34	fNDF = -5;
	35	}
	36
	37
	38	void AliHLTTPCCATrackPar::Normalize( Double_t Direction[3] )
	39	{
	40	//* Normalize the track
	41
	42	Double_t p2 = fP[3]fP[3] + fP[4]fP[4] + fP[5]*fP[5];
	43	if( p2<1.e-4 ) return;
	44	Double_t a2 = 1./p2;
	45	Double_t a = sqrt(a2);
	46
	47	if( Direction && ( fP[3]Direction[0] + fP[4]Direction[1] + fP[5]*Direction[2] < 0 ) ) a = -a;
	48
	49	Double_t ex = fP[3]a, ey = fP[4]a, ez = fP[5]a, qp = fP[6]a;
	50
	51	fP[3] = ex;
	52	fP[4] = ey;
	53	fP[5] = ez;
	54	fP[6] = qp;
	55
	56	Double_t
	57	h0 = fC[ 6]ex + fC[10]ey + fC[15]*ez,
	58	h1 = fC[ 7]ex + fC[11]ey + fC[16]*ez,
	59	h2 = fC[ 8]ex + fC[12]ey + fC[17]*ez,
	60	h3 = fC[ 9]ex + fC[13]ey + fC[18]*ez,
	61	h4 = fC[13]ex + fC[14]ey + fC[19]*ez,
	62	h5 = fC[18]ex + fC[19]ey + fC[20]*ez,
	63	h6 = fC[24]ex + fC[25]ey + fC[26]*ez,
	64	d = h3ex + h4ey + h5*ez,
65	hh = h6 - qp*d ;
66
67	fC[ 6]= a(fC[ 6] -exh0); fC[ 7]= a(fC[ 7] -exh1); fC[ 8]= a(fC[ 8] -exh2);
68	fC[10]= a(fC[10] -eyh0); fC[11]= a(fC[11] -eyh1); fC[12]= a(fC[12] -eyh2);
69	fC[15]= a(fC[15] -ezh0); fC[16]= a(fC[16] -ezh1); fC[17]= a(fC[17] -ezh2);
70	fC[21]= a(fC[21] -qph0); fC[22]= a(fC[22] -qph1); fC[23]= a(fC[23] -qph2);
71
72	fC[ 9]= a2( fC[ 9] -h3ex -h3ex + dex*ex );
73	fC[13]= a2( fC[13] -h4ex -h3ey + dey*ex );
74	fC[14]= a2( fC[14] -h4ey -h4ey + dey*ey );
75
76	fC[18]= a2( fC[18] -h5ex -h3ez + dez*ex );
77	fC[19]= a2( fC[19] -h5ey -h4ez + dez*ey );
78	fC[20]= a2( fC[20] -h5ez -h5ez + dez*ez );
79
80	fC[24]= a2( fC[24] -qph3 - hh*ex );
81	fC[25]= a2( fC[25] -qph4 - hh*ey );
82	fC[26]= a2( fC[26] -qph5 - hh*ez );
83	fC[27]= a2( fC[27] -qph6 - hh*qp );
84	}
85
86
87	Double_t AliHLTTPCCATrackPar::GetDsToPointBz( Double_t Bz, const Double_t xyz[3], const Double_t *T0 ) const
88	{
89	//* Get dS to a certain space point for Bz field
90	const Double_t kCLight = 0.000299792458;
91	Double_t bq = BzT0[6]kCLight;
92	Double_t pt2 = T0[3]T0[3] + T0[4]T0[4];
93	if( pt2<1.e-4 ) return 0;
94	Double_t dx = xyz[0] - T0[0];
95	Double_t dy = xyz[1] - T0[1];
96	Double_t a = dxT0[3]+dyT0[4];
97	Double_t dS = 0;
98	if( TMath::Abs(bq)<1.e-8 ) dS = a/pt2;
99	else dS = TMath::ATan2( bqa, pt2 + bq(dyT0[3] -dxT0[4]) )/bq;
100	return dS;
101	}
102
103	Double_t AliHLTTPCCATrackPar::GetDsToPointBz( Double_t Bz, const Double_t xyz[3] ) const
104	{
105	return GetDsToPointBz( Bz, xyz, fP );
106	}
107
108	void AliHLTTPCCATrackPar::TransportBz( Double_t Bz, Double_t S, Double_t *T0 )
109	{
110	//* Transport the particle on dS, for Bz field
111
112	const Double_t kCLight = 0.000299792458;
113	Bz = Bz*kCLight;
114	Double_t bs= Bz*S;
115	Double_t bqs= bs*T0[6];
116	Double_t s = TMath::Sin(bqs), c = TMath::Cos(bqs);
117	Double_t sB, cB, dsB, dcB;
118	if( TMath::Abs(bqs)>1.e-10){
119	sB= s/Bz/T0[6];
120	cB= (1-c)/Bz/T0[6];
121	dsB = (c*S - sB)/T0[6];
122	dcB = (s*S-cB)/T0[6];
123	}else{
124	sB = (1. - bqsbqs/6.)S;
125	cB = .5sBbqs;
126	dsB = - T0[6]bsbs/3.*S;
127	dcB = .5(sBbs - dsB*bqs);
128	}
129
130	Double_t px = T0[3];
131	Double_t py = T0[4];
132	Double_t pz = T0[5];
133
134	Double_t d[7] = { fP[0]-T0[0], fP[1]-T0[1], fP[2]-T0[2],
135	fP[3]-T0[3], fP[4]-T0[4], fP[5]-T0[5], fP[6]-T0[6] };
136
137	T0[0] = T0[0] + sBpx + cBpy;
138	T0[1] = T0[1] - cBpx + sBpy;
139	T0[2] = T0[2] + S*pz ;
140	T0[3] = cpx + spy;
141	T0[4] = -spx + cpy;
142	T0[5] = T0[5];
143	T0[6] = T0[6];
144
145
146	Double_t mJ[7][7] = { {1,0,0, sB, cB, 0, dsBpx + dcBpy },
147	{0,1,0, -cB, sB, 0, - dcBpx + dsBpy },
148	{0,0,1, 0, 0, S, 0 },
149	{0,0,0, c, s, 0, (-spx + cpy)*bs },
150	{0,0,0, -s, c, 0, (-cpx - spy)*bs },
151	{0,0,0, 0, 0, 1, 0 },
152	{0,0,0, 0, 0, 0, 1 } };
153
154	for( Int_t i=0; i<7; i++){
155	fP[i] = T0[i];
156	for( Int_t j=0; j<7; j++) fP[i] += mJ[i][j]*d[j];
157	}
158
159	Double_t mA[7][7];
160	for( Int_t k=0,i=0; i<7; i++)
161	for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = fC[k];
162
163	Double_t mJC[7][7];
164	for( Int_t i=0; i<7; i++ )
165	for( Int_t j=0; j<7; j++ ){
166	mJC[i][j]=0;
167	for( Int_t k=0; k<7; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
168	}
169
170	for( Int_t k=0,i=0; i<7; i++)
171	for( Int_t j=0; j<=i; j++, k++ ){
172	fC[k] = 0;
173	for( Int_t l=0; l<7; l++ ) fC[k]+=mJC[i][l]*mJ[j][l];
174	}
175	}
176
177	void AliHLTTPCCATrackPar::TransportBz( Double_t Bz, Double_t dS )
178	{
179	//* Transport the particle on dS, for Bz field
180	TransportBz( Bz, dS, fP );
181	}
182
183	void AliHLTTPCCATrackPar::GetConnectionMatrix( Double_t B, const Double_t p[3], Double_t G[6], const Double_t *T0 ) const
184	{
185	//* Calculate connection matrix between track and point p
186	if( !G ) return;
187	const Double_t kLight = 0.000299792458;
188	B*=kLight;
189	Double_t dx = p[0]-T0[0], dy = p[1]-T0[1], dz = p[2]-T0[2];
190	Double_t px2= T0[3]T0[3], py2= T0[4]T0[4], pz2= T0[5]*T0[5];
191	//Double_t B2 = B*B;
192	Double_t s2 = (dxdx + dydy + dz*dz);
193	Double_t p2 = px2 + py2 + pz2;
194	if( p2>1.e-4 ) s2/=p2;
195	Double_t x = T0[3]*s2;
196	Double_t xx= px2s2, xy= xT0[4], xz= xT0[5], yy= py2s2, yz= T0[4]T0[5]s2;
197	//Double_t Bxy= B*xy;
198	G[ 0]= xx;
199	G[ 1]= xy; G[ 2]= yy;
200	G[ 3]= xz; G[ 4]= yz; G[ 5]= pz2*s2;
201	/*
202	C[ 0]+= xx;
203	C[ 1]+= xy; C[ 2]+= yy;
204	C[ 3]+= xz; C[ 4]+= yz; C[ 5]+= pz2*s2;
205	C[ 6]+= Bxy; C[ 7]+= Byy; C[ 8]+= Byz; C[ 9]+=B2*yy;
206	C[10]-= Bxx; C[11]-= Bxy; C[12]-=Bxz; C[13]-=B2xy; C[14]+=B2xx;
207	*/
208	}
209
210
e962f438	211	void AliHLTTPCCATrackPar::Filter( const Double_t m[3], const Double_t V[6], const Double_t V1[6] )
326c2d4b	212	{
	213	//* !
	214	Double_t
	215	c00 = fC[ 0],
	216	c10 = fC[ 1], c11 = fC[ 2],
	217	c20 = fC[ 3], c21 = fC[ 4], c22 = fC[ 5],
	218	c30 = fC[ 6], c31 = fC[ 7], c32 = fC[ 8],
	219	c40 = fC[10], c41 = fC[11], c42 = fC[12],
	220	c50 = fC[15], c51 = fC[16], c52 = fC[17],
	221	c60 = fC[21], c61 = fC[22], c62 = fC[23];
	222
	223	double
	224	z0 = m[0]-fP[0],
	225	z1 = m[1]-fP[1],
	226	z2 = m[2]-fP[2];
	227
	228	Double_t mS[6] = { c00+V[0]+V1[0], c10+V[1]+V1[1], c11+V[2]+V1[2],
	229	c20+V[3]+V1[3], c21+V[4]+V1[4], c22+V[5]+V1[5] };
	230	Double_t mSi[6];
	231	mSi[0] = mS[4]mS[4] - mS[2]mS[5];
	232	mSi[1] = mS[1]mS[5] - mS[3]mS[4];
	233	mSi[3] = mS[2]mS[3] - mS[1]mS[4];
	234	Double_t det = (mS[0]mSi[0] + mS[1]mSi[1] + mS[3]*mSi[3]);
	235	if( TMath::Abs(det)<1.e-10 ) return;
	236	det = 1./det;
	237	mSi[0] *= det;
	238	mSi[1] *= det;
	239	mSi[3] *= det;
	240	mSi[2] = ( mS[3]mS[3] - mS[0]mS[5] )*det;
	241	mSi[4] = ( mS[0]mS[4] - mS[1]mS[3] )*det;
	242	mSi[5] = ( mS[1]mS[1] - mS[0]mS[2] )*det;
	243
	244	fNDF += 2;
	245	fChi2 += ( +(mSi[0]z0 + mSi[1]z1 + mSi[3]z2)z0
	246	+(mSi[1]z0 + mSi[2]z1 + mSi[4]z2)z1
	247	+(mSi[3]z0 + mSi[4]z1 + mSi[5]z2)z2 );
	248
	249	Double_t k0, k1, k2 ; // k = CHtS
	250
	251	k0 = c00mSi[0] + c10mSi[1] + c20*mSi[3];
	252	k1 = c00mSi[1] + c10mSi[2] + c20*mSi[4];
	253	k2 = c00mSi[3] + c10mSi[4] + c20*mSi[5];
	254
	255	fP[ 0]+= k0z0 + k1z1 + k2*z2 ;
	256	fC[ 0]-= k0c00 + k1c10 + k2*c20;
	257
	258	k0 = c10mSi[0] + c11mSi[1] + c21*mSi[3];
	259	k1 = c10mSi[1] + c11mSi[2] + c21*mSi[4];
	260	k2 = c10mSi[3] + c11mSi[4] + c21*mSi[5];
	261
	262	fP[ 1]+= k0z0 + k1z1 + k2*z2 ;
	263	fC[ 1]-= k0c00 + k1c10 + k2*c20;
	264	fC[ 2]-= k0c10 + k1c11 + k2*c21;
	265
	266	k0 = c20mSi[0] + c21mSi[1] + c22*mSi[3];
	267	k1 = c20mSi[1] + c21mSi[2] + c22*mSi[4];
	268	k2 = c20mSi[3] + c21mSi[4] + c22*mSi[5];
	269
	270	fP[ 2]+= k0z0 + k1z1 + k2*z2 ;
	271	fC[ 3]-= k0c00 + k1c10 + k2*c20;
	272	fC[ 4]-= k0c10 + k1c11 + k2*c21;
	273	fC[ 5]-= k0c20 + k1c21 + k2*c22;
	274
	275	k0 = c30mSi[0] + c31mSi[1] + c32*mSi[3];
276	k1 = c30mSi[1] + c31mSi[2] + c32*mSi[4];
277	k2 = c30mSi[3] + c31mSi[4] + c32*mSi[5];
278
279	fP[ 3]+= k0z0 + k1z1 + k2*z2 ;
280	fC[ 6]-= k0c00 + k1c10 + k2*c20;
281	fC[ 7]-= k0c10 + k1c11 + k2*c21;
282	fC[ 8]-= k0c20 + k1c21 + k2*c22;
283	fC[ 9]-= k0c30 + k1c31 + k2*c32;
284
285	k0 = c40mSi[0] + c41mSi[1] + c42*mSi[3];
286	k1 = c40mSi[1] + c41mSi[2] + c42*mSi[4];
287	k2 = c40mSi[3] + c41mSi[4] + c42*mSi[5];
288
289	fP[ 4]+= k0z0 + k1z1 + k2*z2 ;
290	fC[10]-= k0c00 + k1c10 + k2*c20;
291	fC[11]-= k0c10 + k1c11 + k2*c21;
292	fC[12]-= k0c20 + k1c21 + k2*c22;
293	fC[13]-= k0c30 + k1c31 + k2*c32;
294	fC[14]-= k0c40 + k1c41 + k2*c42;
295
296	k0 = c50mSi[0] + c51mSi[1] + c52*mSi[3];
297	k1 = c50mSi[1] + c51mSi[2] + c52*mSi[4];
298	k2 = c50mSi[3] + c51mSi[4] + c52*mSi[5];
299
300	fP[ 5]+= k0z0 + k1z1 + k2*z2 ;
301	fC[15]-= k0c00 + k1c10 + k2*c20;
302	fC[16]-= k0c10 + k1c11 + k2*c21;
303	fC[17]-= k0c20 + k1c21 + k2*c22;
304	fC[18]-= k0c30 + k1c31 + k2*c32;
305	fC[19]-= k0c40 + k1c41 + k2*c42;
306	fC[20]-= k0c50 + k1c51 + k2*c52;
307
308	k0 = c60mSi[0] + c61mSi[1] + c62*mSi[3];
309	k1 = c60mSi[1] + c61mSi[2] + c62*mSi[4];
310	k2 = c60mSi[3] + c61mSi[4] + c62*mSi[5];
311
312	fP[ 6]+= k0z0 + k1z1 + k2*z2 ;
313	fC[21]-= k0c00 + k1c10 + k2*c20;
314	fC[22]-= k0c10 + k1c11 + k2*c21;
315	fC[23]-= k0c20 + k1c21 + k2*c22;
316	fC[24]-= k0c30 + k1c31 + k2*c32;
317	fC[25]-= k0c40 + k1c41 + k2*c42;
318	fC[26]-= k0c50 + k1c51 + k2*c52;
319	fC[27]-= k0c60 + k1c61 + k2*c62;
320	}
321
322
323	void AliHLTTPCCATrackPar::Rotate( Double_t alpha )
324	{
325	//* !
326	Double_t cA = TMath::Cos( alpha );
327	Double_t sA = TMath::Sin( alpha );
328	Double_t x= fP[0], y= fP[1], px= fP[3], py= fP[4];
329	fP[0] = xcA + ysA;
330	fP[1] =-xsA + ycA;
331	fP[2] = fP[2];
332	fP[3] = pxcA + pysA;
333	fP[4] =-pxsA + pycA;
334	fP[5] = fP[5];
335	fP[6] = fP[6];
336
337	Double_t mJ[7][7] = { { cA,sA, 0, 0, 0, 0, 0 },
338	{-sA,cA, 0, 0, 0, 0, 0 },
339	{ 0, 0, 1, 0, 0, 0, 0 },
340	{ 0, 0, 0, cA, sA, 0, 0 },
341	{ 0, 0, 0,-sA, cA, 0, 0 },
342	{ 0, 0, 0, 0, 0, 1, 0 },
343	{ 0, 0, 0, 0, 0, 0, 1 } };
344
345	Double_t mA[7][7];
346	for( Int_t k=0,i=0; i<7; i++)
347	for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = fC[k];
348
349	Double_t mJC[7][7];
350	for( Int_t i=0; i<7; i++ )
351	for( Int_t j=0; j<7; j++ ){
352	mJC[i][j]=0;
353	for( Int_t k=0; k<7; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
354	}
355
356	for( Int_t k=0,i=0; i<7; i++)
357	for( Int_t j=0; j<=i; j++, k++ ){
358	fC[k] = 0;
359	for( Int_t l=0; l<7; l++ ) fC[k]+=mJC[i][l]*mJ[j][l];
360	}
361	}
362
363
364	void AliHLTTPCCATrackPar::ConvertTo5( Double_t alpha, Double_t T[], Double_t C[] )
365	const {
366	//* !
367	AliHLTTPCCATrackPar t = *this;
368	t.Rotate(alpha);
369	Double_t
370	x= t.fP[0], y= t.fP[1], z = t.fP[2],
371	ex= t.fP[3], ey= t.fP[4], ez = t.fP[5], qp = t.fP[6];
372
373	Double_t p2 = exex+eyey+ez*ez;
374	if( p2<1.e-4 ) p2 = 1;
375	Double_t n2 = 1./p2;
376	Double_t n = sqrt(n2);
377
378	T[5] = x;
379	T[0] = y;
380	T[1] = z;
381	T[2] = ey/ex;
382	T[3] = ez/ex;
383	T[4] = qp*n;
384
385	Double_t mJ[5][7] = { { -T[2], 1, 0, 0, 0, 0, 0 },
386	{ -T[3], 0, 1, 0, 0, 0, 0 },
387	{ 0, 0, 0, -T[2]/ex, 1./ex, 0, 0 },
388	{ 0, 0, 0, -T[3]/ex, 0, 1./ex, 0 },
389	{ 0, 0, 0, -T[4]n2ex, -T[4]n2ey, -T[4]n2ez, n }};
390
391	Double_t mA[7][7];
392	for( Int_t k=0,i=0; i<7; i++)
393	for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = t.fC[k];
394
395	Double_t mJC[5][7];
396	for( Int_t i=0; i<5; i++ )
397	for( Int_t j=0; j<7; j++ ){
398	mJC[i][j]=0;
399	for( Int_t k=0; k<7; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
400	}
401
402	for( Int_t k=0,i=0; i<5; i++)
403	for( Int_t j=0; j<=i; j++, k++ ){
404	C[k] = 0;
405	for( Int_t l=0; l<7; l++ ) C[k]+=mJC[i][l]*mJ[j][l];
406	}
407	}