[u/mrichter/AliRoot.git] / HLT / TPCLib / tracking-ca / AliHLT3DTrackParam.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 "AliHLT3DTrackParam.h"
#include "TMath.h"

ClassImp(AliHLT3DTrackParam)

//* Transport utilities
  
Double_t AliHLT3DTrackParam::GetDStoPoint( Double_t Bz, const Double_t xyz[3], const Double_t *T0 ) const
{
  //* Get DS = Path/Momentum to a certain space point for Bz field

  Double_t q = fSignQ;
  if( !T0 ) T0 = fParam; 
  else q = T0[6];

  const Double_t kCLight = 0.000299792458;
  Double_t bq = Bz*q*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;
}


void AliHLT3DTrackParam::TransportToDS( Double_t Bz, Double_t DS, Double_t *T0 )
{
  //* Transport the particle on DS = Path/Momentum, for Bz field 

  Double_t tmp[7];
  if( !T0 ){
    T0 = tmp;
    T0[0] = fParam[0];
    T0[1] = fParam[1];
    T0[2] = fParam[2];
    T0[3] = fParam[3];
    T0[4] = fParam[4];
    T0[5] = fParam[5];
    T0[6] = fSignQ;
  }
  const Double_t kCLight = 0.000299792458;
  Bz = Bz*T0[6]*kCLight;
  Double_t bs= Bz*DS;
  Double_t s = TMath::Sin(bs), c = TMath::Cos(bs);
  Double_t sB, cB;
  if( TMath::Abs(bs)>1.e-10){
    sB= s/Bz;
    cB= (1-c)/Bz;
  }else{
    sB = (1. - bs*bs/6.)*DS;
    cB = .5*sB*bs;
  }
    
  Double_t px = T0[3];
  Double_t py = T0[4];
  Double_t pz = T0[5];
  
  Double_t d[6] = { fParam[0]-T0[0], fParam[1]-T0[1], fParam[2]-T0[2], 
		    fParam[3]-T0[3], fParam[4]-T0[4], fParam[5]-T0[5]  };

  T0[0] = T0[0] + sB*px + cB*py;
  T0[1] = T0[1] - cB*px + sB*py;
  T0[2] = T0[2] + DS*pz                       ;
  T0[3] =          c*px + s*py;
  T0[4] =         -s*px + c*py;
  T0[5] = T0[5];
 
  Double_t mJ[6][6] = { {1,0,0,   sB, cB,  0, },
                        {0,1,0,  -cB, sB,  0, },
                        {0,0,1,    0,  0, DS, },
                        {0,0,0,    c,  s,  0, },
                        {0,0,0,   -s,  c,  0, },
                        {0,0,0,    0,  0,  1, }   };

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

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

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


//* Fit utilities 

void AliHLT3DTrackParam::InitializeCovarianceMatrix()
{
  //* Initialization of covariance matrix

  for( Int_t i=0; i<21; i++ ) fCov[i] = 0;
  fSignQ = 0;
  fCov[0] = fCov[ 2] = fCov[ 5] = 100.;
  fCov[9] = fCov[14] = fCov[20] = 10000.;
  fChi2 = 0;
  fNDF = -5;
}

void AliHLT3DTrackParam::GetGlueMatrix( const Double_t xyz[3], 
					Double_t G[6], const Double_t *T0  ) const 
{
  //* !

  if( !T0 ) T0 = fParam;

  Double_t dx = xyz[0]-T0[0], dy = xyz[1]-T0[1], dz = xyz[2]-T0[2];
  Double_t px2= T0[3]*T0[3], py2= T0[4]*T0[4], pz2= T0[5]*T0[5];
  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;
  G[ 0]= xx;
  G[ 1]= xy;   G[ 2]= yy;
  G[ 3]= xz;   G[ 4]= yz;   G[ 5]= pz2*s2;  
}


void AliHLT3DTrackParam::Filter( const Double_t m[3], const Double_t V[6], const Double_t G[6] )
{ 
  //* !
  
  Double_t 
    c00 = fCov[ 0],
    c10 = fCov[ 1], c11 = fCov[ 2],
    c20 = fCov[ 3], c21 = fCov[ 4], c22 = fCov[ 5],
    c30 = fCov[ 6], c31 = fCov[ 7], c32 = fCov[ 8],
    c40 = fCov[10], c41 = fCov[11], c42 = fCov[12],
    c50 = fCov[15], c51 = fCov[16], c52 = fCov[17];
  
  double
    z0 = m[0]-fParam[0],
    z1 = m[1]-fParam[1],
    z2 = m[2]-fParam[2];
  
  Double_t mS[6] = { c00+V[0]+G[0], c10+V[1]+G[1], c11+V[2]+G[2],
		     c20+V[3]+G[3], c21+V[4]+G[4], c22+V[5]+G[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 = 1./(mS[0]*mSi[0] + mS[1]*mSi[1] + mS[3]*mSi[3]);
  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];
    
  fParam[ 0]+= k0*z0  + k1*z1  + k2*z2 ;
  fCov  [ 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];
  
  fParam[ 1]+= k0*z0  + k1*z1  + k2*z2 ;
  fCov  [ 1]-= k0*c00 + k1*c10 + k2*c20;
  fCov  [ 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];
  
  fParam[ 2]+= k0*z0  + k1*z1  + k2*z2 ;
  fCov  [ 3]-= k0*c00 + k1*c10 + k2*c20;
  fCov  [ 4]-= k0*c10 + k1*c11 + k2*c21;
  fCov  [ 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];
  
  fParam[ 3]+= k0*z0  + k1*z1  + k2*z2 ;
  fCov  [ 6]-= k0*c00 + k1*c10 + k2*c20;
  fCov  [ 7]-= k0*c10 + k1*c11 + k2*c21;
  fCov  [ 8]-= k0*c20 + k1*c21 + k2*c22;
  fCov  [ 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];
    
  fParam[ 4]+= k0*z0  + k1*z1  + k2*z2 ;
  fCov  [10]-= k0*c00 + k1*c10 + k2*c20;
  fCov  [11]-= k0*c10 + k1*c11 + k2*c21;
  fCov  [12]-= k0*c20 + k1*c21 + k2*c22;
  fCov  [13]-= k0*c30 + k1*c31 + k2*c32;
  fCov  [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];
  
  fParam[ 5]+= k0*z0  + k1*z1  + k2*z2 ;
  fCov  [15]-= k0*c00 + k1*c10 + k2*c20;
  fCov  [16]-= k0*c10 + k1*c11 + k2*c21;
  fCov  [17]-= k0*c20 + k1*c21 + k2*c22;
  fCov  [18]-= k0*c30 + k1*c31 + k2*c32;
  fCov  [19]-= k0*c40 + k1*c41 + k2*c42;
  fCov  [20]-= k0*c50 + k1*c51 + k2*c52;

  // fit charge

  Double_t px = fParam[3];
  Double_t py = fParam[4];
  Double_t pz = fParam[5];
  
  Double_t p = TMath::Sqrt( px*px + py*py + pz*pz );
  Double_t pi = 1./p;
  Double_t qp = fSignQ*pi;
  Double_t qp3 = qp*pi*pi;
  Double_t 
    c60 = qp3*(c30+c40+c50),
    c61 = qp3*(c31+c41+c51),
    c62 = qp3*(c32+c42+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];
  
  qp+= k0*z0  + k1*z1  + k2*z2 ;
  if( qp>0 ) fSignQ = 1;
  else if(qp<0 ) fSignQ = -1;
  else fSignQ = 0;
}


//* Other utilities

void AliHLT3DTrackParam::SetDirection( Double_t Direction[3] )
{
  //* Change track direction 

  if( fParam[3]*Direction[0] + fParam[4]*Direction[1] + fParam[5]*Direction[2] >= 0 ) return;

  fParam[3] = -fParam[3];
  fParam[4] = -fParam[4];
  fParam[5] = -fParam[5];
  fSignQ    = -fSignQ;

  fCov[ 6]=-fCov[ 6]; fCov[ 7]=-fCov[ 7]; fCov[ 8]=-fCov[ 8];
  fCov[10]=-fCov[10]; fCov[11]=-fCov[11]; fCov[12]=-fCov[12];
  fCov[15]=-fCov[15]; fCov[16]=-fCov[16]; fCov[17]=-fCov[17];
}


void AliHLT3DTrackParam::RotateCoordinateSystem( Double_t alpha )
{
  //* !

  Double_t cA = TMath::Cos( alpha );
  Double_t sA = TMath::Sin( alpha );
  Double_t x= fParam[0], y= fParam[1], px= fParam[3], py= fParam[4];
  fParam[0] = x*cA + y*sA;
  fParam[1] =-x*sA + y*cA;
  fParam[2] = fParam[2];
  fParam[3] = px*cA + py*sA;
  fParam[4] =-px*sA + py*cA;
  fParam[5] = fParam[5];  

  Double_t mJ[6][6] = { { cA,sA, 0,  0,  0,  0 },
                        {-sA,cA, 0,  0,  0,  0 },
                        {  0, 0, 1,  0,  0,  0 },
                        {  0, 0, 0, cA, sA,  0 },
                        {  0, 0, 0,-sA, cA,  0 },
                        {  0, 0, 0,  0,  0,  1 }  };
                  
  Double_t mA[6][6];
  for( Int_t k=0,i=0; i<6; i++)
    for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = fCov[k]; 

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


void AliHLT3DTrackParam::Get5Parameters( Double_t alpha, Double_t T[6], Double_t C[15] ) const
{
  //* !

  AliHLT3DTrackParam t = *this;
  t.RotateCoordinateSystem(alpha);
  Double_t 
    x= t.fParam[0], y= t.fParam[1], z = t.fParam[2], 
    px= t.fParam[3], py= t.fParam[4], pz = t.fParam[5], q = t.fSignQ;

  Double_t p2 = px*px+py*py+pz*pz;
  if( p2<1.e-8 ) p2 = 1;
  Double_t n2 = 1./p2;
  Double_t n = sqrt(n2);

  T[5] = x;
  T[0] = y;
  T[1] = z;
  T[2] = py/px;
  T[3] = pz/px;
  T[4] = q*n;

  Double_t mJ[5][6] = { { -T[2], 1, 0,  0,  0,  0 },
                        { -T[3], 0, 1,  0,  0,  0 },
                        { 0, 0, 0,  -T[2]/px,  1./px,  0 },
                        { 0, 0, 0, -T[3]/px,  0,  1./px },
                        { 0, 0, 0, -T[4]*n2*px, -T[4]*n2*py, -T[4]*n2*pz} };

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

  Double_t mJC[5][6];
  for( Int_t i=0; i<5; i++ )
    for( Int_t j=0; j<6; j++ ){
      mJC[i][j]=0;
      for( Int_t k=0; k<6; 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<6; 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: 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 "AliHLT3DTrackParam.h"
	20	#include "TMath.h"
	21
dc4788ec	22	ClassImp(AliHLT3DTrackParam)
326c2d4b	23
	24	//* Transport utilities
	25
	26	Double_t AliHLT3DTrackParam::GetDStoPoint( Double_t Bz, const Double_t xyz[3], const Double_t *T0 ) const
	27	{
	28	//* Get DS = Path/Momentum to a certain space point for Bz field
	29
	30	Double_t q = fSignQ;
	31	if( !T0 ) T0 = fParam;
	32	else q = T0[6];
	33
	34	const Double_t kCLight = 0.000299792458;
	35	Double_t bq = BzqkCLight;
	36	Double_t pt2 = T0[3]T0[3] + T0[4]T0[4];
	37	if( pt2<1.e-4 ) return 0;
	38	Double_t dx = xyz[0] - T0[0];
	39	Double_t dy = xyz[1] - T0[1];
	40	Double_t a = dxT0[3]+dyT0[4];
	41	Double_t dS = 0;
	42	if( TMath::Abs(bq)<1.e-8 ) dS = a/pt2;
	43	else dS = TMath::ATan2( bqa, pt2 + bq(dyT0[3] -dxT0[4]) )/bq;
	44	return dS;
	45	}
	46
	47
	48	void AliHLT3DTrackParam::TransportToDS( Double_t Bz, Double_t DS, Double_t *T0 )
	49	{
	50	//* Transport the particle on DS = Path/Momentum, for Bz field
	51
	52	Double_t tmp[7];
	53	if( !T0 ){
	54	T0 = tmp;
	55	T0[0] = fParam[0];
	56	T0[1] = fParam[1];
	57	T0[2] = fParam[2];
	58	T0[3] = fParam[3];
	59	T0[4] = fParam[4];
	60	T0[5] = fParam[5];
	61	T0[6] = fSignQ;
	62	}
	63	const Double_t kCLight = 0.000299792458;
	64	Bz = BzT0[6]kCLight;
	65	Double_t bs= Bz*DS;
	66	Double_t s = TMath::Sin(bs), c = TMath::Cos(bs);
	67	Double_t sB, cB;
	68	if( TMath::Abs(bs)>1.e-10){
	69	sB= s/Bz;
	70	cB= (1-c)/Bz;
	71	}else{
	72	sB = (1. - bsbs/6.)DS;
	73	cB = .5sBbs;
	74	}
	75
	76	Double_t px = T0[3];
	77	Double_t py = T0[4];
	78	Double_t pz = T0[5];
	79
	80	Double_t d[6] = { fParam[0]-T0[0], fParam[1]-T0[1], fParam[2]-T0[2],
	81	fParam[3]-T0[3], fParam[4]-T0[4], fParam[5]-T0[5] };
	82
	83	T0[0] = T0[0] + sBpx + cBpy;
	84	T0[1] = T0[1] - cBpx + sBpy;
	85	T0[2] = T0[2] + DS*pz ;
	86	T0[3] = cpx + spy;
87	T0[4] = -spx + cpy;
88	T0[5] = T0[5];
89
90	Double_t mJ[6][6] = { {1,0,0, sB, cB, 0, },
91	{0,1,0, -cB, sB, 0, },
92	{0,0,1, 0, 0, DS, },
93	{0,0,0, c, s, 0, },
94	{0,0,0, -s, c, 0, },
95	{0,0,0, 0, 0, 1, } };
96
97	for( Int_t i=0; i<6; i++){
98	fParam[i] = T0[i];
99	for( Int_t j=0; j<6; j++) fParam[i] += mJ[i][j]*d[j];
100	}
101
102	Double_t mA[6][6];
103	for( Int_t k=0,i=0; i<6; i++)
104	for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = fCov[k];
105
106	Double_t mJC[6][6];
107	for( Int_t i=0; i<6; i++ )
108	for( Int_t j=0; j<6; j++ ){
109	mJC[i][j]=0;
110	for( Int_t k=0; k<6; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
111	}
112
113	for( Int_t k=0,i=0; i<6; i++)
114	for( Int_t j=0; j<=i; j++, k++ ){
115	fCov[k] = 0;
116	for( Int_t l=0; l<6; l++ ) fCov[k]+=mJC[i][l]*mJ[j][l];
117	}
118	}
119
120
121	//* Fit utilities
122
123	void AliHLT3DTrackParam::InitializeCovarianceMatrix()
124	{
125	//* Initialization of covariance matrix
126
127	for( Int_t i=0; i<21; i++ ) fCov[i] = 0;
128	fSignQ = 0;
129	fCov[0] = fCov[ 2] = fCov[ 5] = 100.;
130	fCov[9] = fCov[14] = fCov[20] = 10000.;
131	fChi2 = 0;
132	fNDF = -5;
133	}
134
135	void AliHLT3DTrackParam::GetGlueMatrix( const Double_t xyz[3],
136	Double_t G[6], const Double_t *T0 ) const
137	{
138	//* !
139
140	if( !T0 ) T0 = fParam;
141
142	Double_t dx = xyz[0]-T0[0], dy = xyz[1]-T0[1], dz = xyz[2]-T0[2];
143	Double_t px2= T0[3]T0[3], py2= T0[4]T0[4], pz2= T0[5]*T0[5];
144	Double_t s2 = (dxdx + dydy + dz*dz);
145	Double_t p2 = px2 + py2 + pz2;
146	if( p2>1.e-4 ) s2/=p2;
147	Double_t x = T0[3]*s2;
148	Double_t xx= px2s2, xy= xT0[4], xz= xT0[5], yy= py2s2, yz= T0[4]T0[5]s2;
149	G[ 0]= xx;
150	G[ 1]= xy; G[ 2]= yy;
151	G[ 3]= xz; G[ 4]= yz; G[ 5]= pz2*s2;
152	}
153
154
155
156	void AliHLT3DTrackParam::Filter( const Double_t m[3], const Double_t V[6], const Double_t G[6] )
157	{
158	//* !
159
160	Double_t
161	c00 = fCov[ 0],
162	c10 = fCov[ 1], c11 = fCov[ 2],
163	c20 = fCov[ 3], c21 = fCov[ 4], c22 = fCov[ 5],
164	c30 = fCov[ 6], c31 = fCov[ 7], c32 = fCov[ 8],
165	c40 = fCov[10], c41 = fCov[11], c42 = fCov[12],
166	c50 = fCov[15], c51 = fCov[16], c52 = fCov[17];
167
168	double
169	z0 = m[0]-fParam[0],
170	z1 = m[1]-fParam[1],
171	z2 = m[2]-fParam[2];
172
173	Double_t mS[6] = { c00+V[0]+G[0], c10+V[1]+G[1], c11+V[2]+G[2],
174	c20+V[3]+G[3], c21+V[4]+G[4], c22+V[5]+G[5] };
175	Double_t mSi[6];
176	mSi[0] = mS[4]mS[4] - mS[2]mS[5];
177	mSi[1] = mS[1]mS[5] - mS[3]mS[4];
178	mSi[3] = mS[2]mS[3] - mS[1]mS[4];
179	Double_t det = 1./(mS[0]mSi[0] + mS[1]mSi[1] + mS[3]*mSi[3]);
180	mSi[0] *= det;
181	mSi[1] *= det;
182	mSi[3] *= det;
183	mSi[2] = ( mS[3]mS[3] - mS[0]mS[5] )*det;
184	mSi[4] = ( mS[0]mS[4] - mS[1]mS[3] )*det;
185	mSi[5] = ( mS[1]mS[1] - mS[0]mS[2] )*det;
186
187	fNDF += 2;
188	fChi2 += ( +(mSi[0]z0 + mSi[1]z1 + mSi[3]z2)z0
189	+(mSi[1]z0 + mSi[2]z1 + mSi[4]z2)z1
190	+(mSi[3]z0 + mSi[4]z1 + mSi[5]z2)z2 );
191
192	Double_t k0, k1, k2 ; // k = CHtS
193
194	k0 = c00mSi[0] + c10mSi[1] + c20*mSi[3];
195	k1 = c00mSi[1] + c10mSi[2] + c20*mSi[4];
196	k2 = c00mSi[3] + c10mSi[4] + c20*mSi[5];
197
198	fParam[ 0]+= k0z0 + k1z1 + k2*z2 ;
199	fCov [ 0]-= k0c00 + k1c10 + k2*c20;
200
201	k0 = c10mSi[0] + c11mSi[1] + c21*mSi[3];
202	k1 = c10mSi[1] + c11mSi[2] + c21*mSi[4];
203	k2 = c10mSi[3] + c11mSi[4] + c21*mSi[5];
204
205	fParam[ 1]+= k0z0 + k1z1 + k2*z2 ;
206	fCov [ 1]-= k0c00 + k1c10 + k2*c20;
207	fCov [ 2]-= k0c10 + k1c11 + k2*c21;
208
209	k0 = c20mSi[0] + c21mSi[1] + c22*mSi[3];
210	k1 = c20mSi[1] + c21mSi[2] + c22*mSi[4];
211	k2 = c20mSi[3] + c21mSi[4] + c22*mSi[5];
212
213	fParam[ 2]+= k0z0 + k1z1 + k2*z2 ;
214	fCov [ 3]-= k0c00 + k1c10 + k2*c20;
215	fCov [ 4]-= k0c10 + k1c11 + k2*c21;
216	fCov [ 5]-= k0c20 + k1c21 + k2*c22;
217
218	k0 = c30mSi[0] + c31mSi[1] + c32*mSi[3];
219	k1 = c30mSi[1] + c31mSi[2] + c32*mSi[4];
220	k2 = c30mSi[3] + c31mSi[4] + c32*mSi[5];
221
222	fParam[ 3]+= k0z0 + k1z1 + k2*z2 ;
223	fCov [ 6]-= k0c00 + k1c10 + k2*c20;
224	fCov [ 7]-= k0c10 + k1c11 + k2*c21;
225	fCov [ 8]-= k0c20 + k1c21 + k2*c22;
226	fCov [ 9]-= k0c30 + k1c31 + k2*c32;
227
228	k0 = c40mSi[0] + c41mSi[1] + c42*mSi[3];
229	k1 = c40mSi[1] + c41mSi[2] + c42*mSi[4];
230	k2 = c40mSi[3] + c41mSi[4] + c42*mSi[5];
231
232	fParam[ 4]+= k0z0 + k1z1 + k2*z2 ;
233	fCov [10]-= k0c00 + k1c10 + k2*c20;
234	fCov [11]-= k0c10 + k1c11 + k2*c21;
235	fCov [12]-= k0c20 + k1c21 + k2*c22;
236	fCov [13]-= k0c30 + k1c31 + k2*c32;
237	fCov [14]-= k0c40 + k1c41 + k2*c42;
238
239	k0 = c50mSi[0] + c51mSi[1] + c52*mSi[3];
240	k1 = c50mSi[1] + c51mSi[2] + c52*mSi[4];
241	k2 = c50mSi[3] + c51mSi[4] + c52*mSi[5];
242
243	fParam[ 5]+= k0z0 + k1z1 + k2*z2 ;
244	fCov [15]-= k0c00 + k1c10 + k2*c20;
245	fCov [16]-= k0c10 + k1c11 + k2*c21;
246	fCov [17]-= k0c20 + k1c21 + k2*c22;
247	fCov [18]-= k0c30 + k1c31 + k2*c32;
248	fCov [19]-= k0c40 + k1c41 + k2*c42;
249	fCov [20]-= k0c50 + k1c51 + k2*c52;
250
251	// fit charge
252
253	Double_t px = fParam[3];
254	Double_t py = fParam[4];
255	Double_t pz = fParam[5];
256
257	Double_t p = TMath::Sqrt( pxpx + pypy + pz*pz );
258	Double_t pi = 1./p;
259	Double_t qp = fSignQ*pi;
260	Double_t qp3 = qppipi;
261	Double_t
262	c60 = qp3*(c30+c40+c50),
263	c61 = qp3*(c31+c41+c51),
264	c62 = qp3*(c32+c42+c52);
265
266	k0 = c60mSi[0] + c61mSi[1] + c62*mSi[3];
267	k1 = c60mSi[1] + c61mSi[2] + c62*mSi[4];
268	k2 = c60mSi[3] + c61mSi[4] + c62*mSi[5];
269
270	qp+= k0z0 + k1z1 + k2*z2 ;
271	if( qp>0 ) fSignQ = 1;
272	else if(qp<0 ) fSignQ = -1;
273	else fSignQ = 0;
274	}
275
276
277	//* Other utilities
278
279	void AliHLT3DTrackParam::SetDirection( Double_t Direction[3] )
280	{
281	//* Change track direction
282
283	if( fParam[3]Direction[0] + fParam[4]Direction[1] + fParam[5]*Direction[2] >= 0 ) return;
284
285	fParam[3] = -fParam[3];
286	fParam[4] = -fParam[4];
287	fParam[5] = -fParam[5];
288	fSignQ = -fSignQ;
289
290	fCov[ 6]=-fCov[ 6]; fCov[ 7]=-fCov[ 7]; fCov[ 8]=-fCov[ 8];
291	fCov[10]=-fCov[10]; fCov[11]=-fCov[11]; fCov[12]=-fCov[12];
292	fCov[15]=-fCov[15]; fCov[16]=-fCov[16]; fCov[17]=-fCov[17];
293	}
294
295
296	void AliHLT3DTrackParam::RotateCoordinateSystem( Double_t alpha )
297	{
298	//* !
299
300	Double_t cA = TMath::Cos( alpha );
301	Double_t sA = TMath::Sin( alpha );
302	Double_t x= fParam[0], y= fParam[1], px= fParam[3], py= fParam[4];
303	fParam[0] = xcA + ysA;
304	fParam[1] =-xsA + ycA;
305	fParam[2] = fParam[2];
306	fParam[3] = pxcA + pysA;
307	fParam[4] =-pxsA + pycA;
308	fParam[5] = fParam[5];
309
310	Double_t mJ[6][6] = { { cA,sA, 0, 0, 0, 0 },
311	{-sA,cA, 0, 0, 0, 0 },
312	{ 0, 0, 1, 0, 0, 0 },
313	{ 0, 0, 0, cA, sA, 0 },
314	{ 0, 0, 0,-sA, cA, 0 },
315	{ 0, 0, 0, 0, 0, 1 } };
316
317	Double_t mA[6][6];
318	for( Int_t k=0,i=0; i<6; i++)
319	for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = fCov[k];
320
321	Double_t mJC[6][6];
322	for( Int_t i=0; i<6; i++ )
323	for( Int_t j=0; j<6; j++ ){
324	mJC[i][j]=0;
325	for( Int_t k=0; k<6; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
326	}
327
328	for( Int_t k=0,i=0; i<6; i++)
329	for( Int_t j=0; j<=i; j++, k++ ){
330	fCov[k] = 0;
331	for( Int_t l=0; l<6; l++ ) fCov[k]+=mJC[i][l]*mJ[j][l];
332	}
333	}
334
335
336	void AliHLT3DTrackParam::Get5Parameters( Double_t alpha, Double_t T[6], Double_t C[15] ) const
337	{
338	//* !
339
340	AliHLT3DTrackParam t = *this;
341	t.RotateCoordinateSystem(alpha);
342	Double_t
343	x= t.fParam[0], y= t.fParam[1], z = t.fParam[2],
344	px= t.fParam[3], py= t.fParam[4], pz = t.fParam[5], q = t.fSignQ;
345
346	Double_t p2 = pxpx+pypy+pz*pz;
347	if( p2<1.e-8 ) p2 = 1;
348	Double_t n2 = 1./p2;
349	Double_t n = sqrt(n2);
350
351	T[5] = x;
352	T[0] = y;
353	T[1] = z;
354	T[2] = py/px;
355	T[3] = pz/px;
356	T[4] = q*n;
357
358	Double_t mJ[5][6] = { { -T[2], 1, 0, 0, 0, 0 },
359	{ -T[3], 0, 1, 0, 0, 0 },
360	{ 0, 0, 0, -T[2]/px, 1./px, 0 },
361	{ 0, 0, 0, -T[3]/px, 0, 1./px },
362	{ 0, 0, 0, -T[4]n2px, -T[4]n2py, -T[4]n2pz} };
363
364	Double_t mA[6][6];
365	for( Int_t k=0,i=0; i<6; i++)
366	for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = t.fCov[k];
367
368	Double_t mJC[5][6];
369	for( Int_t i=0; i<5; i++ )
370	for( Int_t j=0; j<6; j++ ){
371	mJC[i][j]=0;
372	for( Int_t k=0; k<6; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
373	}
374
375	for( Int_t k=0,i=0; i<5; i++)
376	for( Int_t j=0; j<=i; j++, k++ ){
377	C[k] = 0;
378	for( Int_t l=0; l<6; l++ ) C[k]+=mJC[i][l]*mJ[j][l];
379	}
380	}