[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 AliHLT3DTrackParam::GetDStoPoint( double Bz, const double xyz[3], const double *T0 ) const
{
  //* Get DS = Path/Momentum to a certain space point for Bz field

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

  const double kCLight = 0.000299792458;
  double bq = Bz * q * kCLight;
  double pt2 = T0[3] * T0[3] + T0[4] * T0[4];
  if ( pt2 < 1.e-4 ) return 0;
  double dx = xyz[0] - T0[0];
  double dy = xyz[1] - T0[1];
  double a = dx * T0[3] + dy * T0[4];
  double 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 Bz, double DS, double *T0 )
{
  //* Transport the particle on DS = Path/Momentum, for Bz field

  double 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 kCLight = 0.000299792458;
  Bz = Bz * T0[6] * kCLight;
  double bs = Bz * DS;
  double s = TMath::Sin( bs ), c = TMath::Cos( bs );
  double sB, cB;
  if ( TMath::Abs( bs ) > 1.e-10 ) {
    sB = s / Bz;
    cB = ( 1 - c ) / Bz;
  } else {
    const Double_t kOvSqr6=1./TMath::Sqrt(6.);
    sB = (1.-bs*kOvSqr6)*(1.+bs*kOvSqr6) * DS;
    cB = .5 * sB * bs;
  }

  double px = T0[3];
  double py = T0[4];
  double pz = T0[5];

  double 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 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 i = 0; i < 6; i++ ) {
    fParam[i] = T0[i];
    for ( int j = 0; j < 6; j++ ) fParam[i] += mJ[i][j] * d[j];
  }

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

  double mJC[6][6];
  for ( int i = 0; i < 6; i++ )
    for ( int j = 0; j < 6; j++ ) {
      mJC[i][j] = 0;
      for ( int k = 0; k < 6; k++ ) mJC[i][j] += mJ[i][k] * mA[k][j];
    }

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


//* Fit utilities

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

  for ( int 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 xyz[3],
                                        double G[6], const double *T0  ) const
{
  //* !

  if ( !T0 ) T0 = fParam;

  double dx = xyz[0] - T0[0], dy = xyz[1] - T0[1], dz = xyz[2] - T0[2];
  double px2 = T0[3] * T0[3], py2 = T0[4] * T0[4], pz2 = T0[5] * T0[5];
  double s2 = ( dx * dx + dy * dy + dz * dz );
  double p2 = px2 + py2 + pz2;
  if ( p2 > 1.e-4 ) s2 /= p2;
  double x = T0[3] * s2;
  double 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 m[3], const double V[6], const double G[6] )
{
  //* !

  double
  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 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 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 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 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 px = fParam[3];
  double py = fParam[4];
  double pz = fParam[5];

  double p = TMath::Sqrt( px * px + py * py + pz * pz );
  double pi = 1. / p;
  double qp = fSignQ * pi;
  double qp3 = qp * pi * pi;
  double
  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 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 alpha )
{
  //* !

  double cA = TMath::Cos( alpha );
  double sA = TMath::Sin( alpha );
  double 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 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 mA[6][6];
  for ( int k = 0, i = 0; i < 6; i++ )
    for ( int j = 0; j <= i; j++, k++ ) mA[i][j] = mA[j][i] = fCov[k];

  double mJC[6][6];
  for ( int i = 0; i < 6; i++ )
    for ( int j = 0; j < 6; j++ ) {
      mJC[i][j] = 0;
      for ( int k = 0; k < 6; k++ ) mJC[i][j] += mJ[i][k] * mA[k][j];
    }

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


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

  AliHLT3DTrackParam t = *this;
  t.RotateCoordinateSystem( alpha );
  double
  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 p2 = px * px + py * py + pz * pz;
  if ( p2 < 1.e-8 ) p2 = 1;
  double n2 = 1. / p2;
  double 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 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 mA[6][6];
  for ( int k = 0, i = 0; i < 6; i++ )
    for ( int j = 0; j <= i; j++, k++ ) mA[i][j] = mA[j][i] = t.fCov[k];

  double mJC[5][6];
  for ( int i = 0; i < 5; i++ )
    for ( int j = 0; j < 6; j++ ) {
      mJC[i][j] = 0;
      for ( int k = 0; k < 6; k++ ) mJC[i][j] += mJ[i][k] * mA[k][j];
    }

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