HLT/TPCLib/tracking-ca/AliHLT3DTrackParam.cxx

   1 // @(#) $Id$
   2 // **************************************************************************
   3 // This file is property of and copyright by the ALICE HLT Project          *
   4 // ALICE Experiment at CERN, All rights reserved.                           *
   5 //                                                                          *
   6 // Primary Authors: Sergey Gorbunov <sergey.gorbunov@kip.uni-heidelberg.de> *
   7 //                  Ivan Kisel <kisel@kip.uni-heidelberg.de>                *
   8 //                  for The ALICE HLT Project.                              *
   9 //                                                                          *
  10 // Permission to use, copy, modify and distribute this software and its     *
  11 // documentation strictly for non-commercial purposes is hereby granted     *
  12 // without fee, provided that the above copyright notice appears in all     *
  13 // copies and that both the copyright notice and this permission notice     *
  14 // appear in the supporting documentation. The authors make no claims       *
  15 // about the suitability of this software for any purpose. It is            *
  16 // provided "as is" without express or implied warranty.                    *
  17 //***************************************************************************
  18
  19 #include "AliHLT3DTrackParam.h"
  20 #include "TMath.h"
  21
  22 ClassImp( AliHLT3DTrackParam )
  23
  24 //* Transport utilities
  25
  26 double AliHLT3DTrackParam::GetDStoPoint( double Bz, const double xyz[3], const double *T0 ) const
  27 {
  28   //* Get DS = Path/Momentum to a certain space point for Bz field
  29
  30   double q = fSignQ;
  31   if ( !T0 ) T0 = fParam;
  32   else q = T0[6];
  33
  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;
  44   return dS;
  45 }
  46
  47
  48 void AliHLT3DTrackParam::TransportToDS( double Bz, double DS, double *T0 )
  49 {
  50   //* Transport the particle on DS = Path/Momentum, for Bz field
  51
  52   double 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 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 {
  72     const Double_t kOvSqr6=1./TMath::Sqrt(6.);
  73     sB = (1.-bs*kOvSqr6)*(1.+bs*kOvSqr6) * DS;
  74     cB = .5 * sB * bs;
  75   }
  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],
  82                   fParam[3] - T0[3], fParam[4] - T0[4], fParam[5] - T0[5]
  83                 };
  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;
  90   T0[5] = T0[5];
  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++ ) {
 101     fParam[i] = T0[i];
 102     for ( int j = 0; j < 6; j++ ) fParam[i] += mJ[i][j] * d[j];
 103   }
 104
 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];
 108
 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];
 114     }
 115
 116   for ( int k = 0, i = 0; i < 6; i++ )
 117     for ( int j = 0; j <= i; j++, k++ ) {
 118       fCov[k] = 0;
 119       for ( int l = 0; l < 6; l++ ) fCov[k] += mJC[i][l] * mJ[j][l];
 120     }
 121 }
 122
 123
 124 //* Fit utilities
 125
 126 void AliHLT3DTrackParam::InitializeCovarianceMatrix()
 127 {
 128   //* Initialization of covariance matrix
 129
 130   for ( int i = 0; i < 21; i++ ) fCov[i] = 0;
 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
 138 void AliHLT3DTrackParam::GetGlueMatrix( const double xyz[3],
 139                                         double G[6], const double *T0  ) const
 140 {
 141   //* !
 142
 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;
 155 }
 156
 157
 158
 159 void AliHLT3DTrackParam::Filter( const double m[3], const double V[6], const double G[6] )
 160 {
 161   //* !
 162
 163   double
 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],
 177                    c20 + V[3] + G[3], c21 + V[4] + G[4], c22 + V[5] + G[5]
 178                  };
 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] );
 184   mSi[0] *= det;
 185   mSi[1] *= det;
 186   mSi[3] *= det;
 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
 191   fNDF  += 2;
 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;
 254
 255   // fit charge
 256
 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;
 277   else fSignQ = 0;
 278 }
 279
 280
 281 //* Other utilities
 282
 283 void AliHLT3DTrackParam::SetDirection( double Direction[3] )
 284 {
 285   //* Change track direction
 286
 287   if ( fParam[3]*Direction[0] + fParam[4]*Direction[1] + fParam[5]*Direction[2] >= 0 ) return;
 288
 289   fParam[3] = -fParam[3];
 290   fParam[4] = -fParam[4];
 291   fParam[5] = -fParam[5];
 292   fSignQ    = -fSignQ;
 293
 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];
 297 }
 298
 299
 300 void AliHLT3DTrackParam::RotateCoordinateSystem( double alpha )
 301 {
 302   //* !
 303
 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;
 309   fParam[2] = fParam[2];
 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];
 331     }
 332
 333   for ( int k = 0, i = 0; i < 6; i++ )
 334     for ( int j = 0; j <= i; j++, k++ ) {
 335       fCov[k] = 0;
 336       for ( int l = 0; l < 6; l++ ) fCov[k] += mJC[i][l] * mJ[j][l];
 337     }
 338 }
 339
 340
 341 void AliHLT3DTrackParam::Get5Parameters( double alpha, double T[6], double C[15] ) const
 342 {
 343   //* !
 344
 345   AliHLT3DTrackParam t = *this;
 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;
 350
 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 );
 355
 356   T[5] = x;
 357   T[0] = y;
 358   T[1] = z;
 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]*n2*px, -T[4]*n2*py, -T[4]*n2*pz}
 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];
 379     }
 380
 381   for ( int k = 0, i = 0; i < 5; i++ )
 382     for ( int j = 0; j <= i; j++, k++ ) {
 383       C[k] = 0;
 384       for ( int l = 0; l < 6; l++ ) C[k] += mJC[i][l] * mJ[j][l];
 385     }
 386 }