CbmKFMath.cxx

Go to the documentation of this file.

/* Copyright (C) 2006-2019 GSI Helmholtzzentrum fuer Schwerionenforschung, Darmstadt
   SPDX-License-Identifier: GPL-3.0-only
   Authors: Denis Bertini [committer] */
 
#include "CbmKFMath.h"
 
#include "FairField.h"
#include "FairTrackParam.h"
 
#include <cmath>
 
using std::exp;
using std::fabs;
using std::log;
using std::sqrt;
 
ClassImp(CbmKFMath)
 
  Bool_t CbmKFMath::intersectCone(Double_t zCone, Double_t ZCone, Double_t rCone, Double_t RCone, const Double_t x[],
                                  Double_t* z1, Double_t* z2)
{
  Double_t t  = (RCone - rCone) / (ZCone - zCone);
  Double_t A  = (RCone * zCone - rCone * ZCone) / (ZCone - zCone);
  Double_t x0 = x[0] - x[5] * x[2];
  Double_t y0 = x[1] - x[5] * x[3];
  Double_t tx = x[2];
  Double_t ty = x[3];
  Double_t a  = tx * tx + ty * ty - t * t;
  Double_t b  = 2 * (tx * x0 + ty * y0 + t * A);
  Double_t c  = x0 * x0 + y0 * y0 - A * A;
 
  if (fabs(a) < 1.E-4) {
    return 1;
  }
  Double_t D = b * b - 4 * a * c;
  if (D < 0.) {
    return 1;
  }
  D   = sqrt(D);
  *z1 = (-b + D) / (2 * a);
  *z2 = (-b - D) / (2 * a);
  return 0;
}
 
 
void CbmKFMath::multQSQt(Int_t N, const Double_t Q[], const Double_t S[], Double_t S_out[])
{
  Double_t A[N * N];
 
  for (Int_t i = 0, n = 0; i < N; i++) {
    for (Int_t j = 0; j < N; j++, ++n) {
      A[n] = 0;
      for (Int_t k = 0; k < N; ++k) {
        A[n] += S[indexS(i, k)] * Q[j * N + k];
      }
    }
  }
 
  for (Int_t i = 0; i < N; i++) {
    for (Int_t j = 0; j <= i; j++) {
      Int_t n  = indexS(i, j);
      S_out[n] = 0;
      for (Int_t k = 0; k < N; k++) {
        S_out[n] += Q[i * N + k] * A[k * N + j];
      }
    }
  }
}
 
void CbmKFMath::multQtSQ(Int_t N, const Double_t Q[], const Double_t S[], Double_t S_out[])
{
  Double_t A[N * N];
 
  for (Int_t i = 0, n = 0; i < N; i++) {
    for (Int_t j = 0; j < N; j++, ++n) {
      A[n] = 0;
      for (Int_t k = 0; k < N; ++k) {
        A[n] += S[indexS(i, k)] * Q[k * N + j];
      }
    }
  }
 
  for (Int_t i = 0; i < N; i++) {
    for (Int_t j = 0; j <= i; j++) {
      Int_t n  = indexS(i, j);
      S_out[n] = 0;
      for (Int_t k = 0; k < N; k++) {
        S_out[n] += Q[k * N + i] * A[k * N + j];
      }
    }
  }
}
 
void CbmKFMath::multSSQ(const Double_t* A, const Double_t* B, Double_t* C, Int_t n)
{
  for (Int_t i = 0; i < n; ++i) {
    for (Int_t j = 0; j < n; ++j) {
      Int_t ind = i * n + j;
      C[ind]    = 0;
      for (Int_t k = 0; k < n; ++k) {
        C[ind] += A[indexS(i, k)] * B[indexS(k, j)];
      }
    }
  }
}
 
void CbmKFMath::four_dim_inv(Double_t a[4][4])
{
  /**** Gaussian algorithm for 4x4 matrix inversion ****/
  Int_t i, j, k, l;
  Double_t factor;
  Double_t temp[4];
  Double_t b[4][4];
 
  // Set b to I
  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++) {
      b[i][j] = (i == j) ? 1.0 : 0.0;
    }
  }
 
  for (i = 0; i < 4; i++) {
    for (j = i + 1; j < 4; j++) {
      if (fabs(a[i][i]) < fabs(a[j][i])) {
        for (l = 0; l < 4; l++) {
          temp[l] = a[i][l];
        }
        for (l = 0; l < 4; l++) {
          a[i][l] = a[j][l];
        }
        for (l = 0; l < 4; l++) {
          a[j][l] = temp[l];
        }
        for (l = 0; l < 4; l++) {
          temp[l] = b[i][l];
        }
        for (l = 0; l < 4; l++) {
          b[i][l] = b[j][l];
        }
        for (l = 0; l < 4; l++) {
          b[j][l] = temp[l];
        }
      }
    }
    factor = a[i][i];
    for (j = 4 - 1; j > -1; j--) {
      b[i][j] /= factor;
      a[i][j] /= factor;
    }
    for (j = i + 1; j < 4; j++) {
      factor = -a[j][i];
      for (k = 0; k < 4; k++) {
        a[j][k] += a[i][k] * factor;
        b[j][k] += b[i][k] * factor;
      }
    }
  }
  for (i = 3; i > 0; i--) {
    for (j = i - 1; j > -1; j--) {
      factor = -a[j][i];
      for (k = 0; k < 4; k++) {
        a[j][k] += a[i][k] * factor;
        b[j][k] += b[i][k] * factor;
      }
    }
  }
 
  // copy b to a and return
  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++) {
      a[i][j] = b[i][j];
    }
  }
}
 
void CbmKFMath::five_dim_inv(Double_t a[5][5])
{
  /**** Gaussian algorithm for 5x5 matrix inversion ****/
  Int_t i, j, k, l;
  Double_t factor;
  Double_t temp[5];
  Double_t b[5][5];
 
  // Set b to I
  for (i = 0; i < 5; i++) {
    for (j = 0; j < 5; j++) {
      b[i][j] = (i == j) ? 1. : 0.;
    }
  }
 
 
  for (i = 0; i < 5; i++) {
    for (j = i + 1; j < 5; j++) {
      if (fabs(a[i][i]) < fabs(a[j][i])) {
        for (l = 0; l < 5; l++) {
          temp[l] = a[i][l];
        }
        for (l = 0; l < 5; l++) {
          a[i][l] = a[j][l];
        }
        for (l = 0; l < 5; l++) {
          a[j][l] = temp[l];
        }
        for (l = 0; l < 5; l++) {
          temp[l] = b[i][l];
        }
        for (l = 0; l < 5; l++) {
          b[i][l] = b[j][l];
        }
        for (l = 0; l < 5; l++) {
          b[j][l] = temp[l];
        }
      }
    }
    factor = a[i][i];
    // cout<<"Highest element "<<a[i][i]<<endl;
    for (j = 5 - 1; j > -1; j--) {
      b[i][j] /= factor;
      a[i][j] /= factor;
    }
    for (j = i + 1; j < 5; j++) {
      factor = -a[j][i];
      for (k = 0; k < 5; k++) {
        a[j][k] += a[i][k] * factor;
        b[j][k] += b[i][k] * factor;
      }
    }
  }
  for (i = 4; i > 0; i--) {
    for (j = i - 1; j > -1; j--) {
      factor = -a[j][i];
      for (k = 0; k < 5; k++) {
        a[j][k] += a[i][k] * factor;
        b[j][k] += b[i][k] * factor;
      }
    }
  }
 
  // copy b to a and return
  for (i = 0; i < 5; i++) {
    for (j = 0; j < 5; j++) {
      a[i][j] = b[i][j];
    }
  }
}
 
Bool_t CbmKFMath::invS(Double_t A[], Int_t N)
{
 
  Bool_t ret = 0;
 
  const Double_t ZERO = 1.E-20;
 
  // input: simmetric > 0 NxN matrix A = {a11,a21,a22,a31..a33,..}
  // output: inverse A, in case of problems fill zero and return 1
 
  // A->low triangular Anew : A = Anew x Anew^T
  // method:
  // for(j=1,N) for(i=j,N) Aij=(Aii-sum_{k=1}^{j-1}Aik*Ajk )/Ajj
  //
 
  {
    Double_t *j1 = A, *jj = A;
    for (Int_t j = 1; j <= N; j1 += j++, jj += j) {
      Double_t *ik = j1, x = 0;
      while (ik != jj) {
        x -= (*ik) * (*ik);
        ik++;
      }
      x += *ik;
      if (x > ZERO) {
        x   = sqrt(x);
        *ik = x;
        ik++;
        x = 1 / x;
        for (Int_t step = 1; step <= N - j; ik += ++step) {  // ik==Ai1
          Double_t sum = 0;
          for (Double_t* jk = j1; jk != jj; sum += *(jk++) * *(ik++)) {
            ;
          }
          *ik = (*ik - sum) * x;  // ik == Aij
        }
      }
      else {
        Double_t* ji = jj;
        for (Int_t i = j; i < N; i++) {
          *(ji += i) = 0.;
        }
        ret = 1;
      }
    }
  }
 
  // A -> Ainv
  // method :
  // for(i=1,N){
  //   Aii = 1/Aii;
  //   for(j=1,i-1) Aij=-(sum_{k=j}^{i-1} Aik * Akj) / Aii ;
  // }
 
  {
    Double_t *ii = A, *ij = A;
    for (Int_t i = 1; i <= N; ij = ii + 1, ii += ++i) {
      if (*ii > ZERO) {
        Double_t x   = -(*ii = 1. / *ii);
        Double_t* jj = A;
        for (Int_t j = 1; j < i; jj += ++j, ij++) {
          Double_t *ik = ij, *kj = jj, sum = 0.;
          for (Int_t k = j; ik != ii; kj += k++, ik++) {
            sum += *ik * *kj;
          }
          *kj = sum * x;
        }
      }
      else {
        for (Double_t* ik = ij; ik != ii + 1; ik++) {
          *ik = 0.;
        }
        ret = 1;
      }
    }
  }
 
  // A -> A^T x A
  // method:
  // Aij = sum_{k=i}^N Aki * Akj
 
  {
    Double_t *ii = A, *ij = A;
    for (Int_t i = 1; i <= N; ii += ++i) {
      do {
        Double_t *ki = ii, *kj = ij, sum = 0.;
        for (Int_t k = i; k <= N; ki += k, kj += k++) {
          sum += (*ki) * (*kj);
        }
        *ij = sum;
      } while ((ij++) != ii);
    }
  }
  return ret;
}
 
Double_t CbmKFMath::getDeviation(Double_t x, Double_t y, Double_t C[], Double_t vx, Double_t vy, Double_t Cv[])
{
  Double_t dx   = x - vx;
  Double_t dy   = y - vy;
  Double_t c[3] = {0, 0, 0};
  if (C) {
    c[0] = C[0];
    c[1] = C[1];
    c[2] = C[2];
  }
  if (Cv) {
    c[0] += Cv[0];
    c[1] += Cv[1];
    c[2] += Cv[2];
  }
  Double_t d = c[0] * c[2] - c[1] * c[1];
  if (fabs(d) < 1.e-20) {
    return 0;
  }
  return sqrt(fabs(0.5 * (dx * dx * c[0] - 2 * dx * dy * c[1] + dy * dy * c[2]) / d));
}
 
Double_t CbmKFMath::AnalyticQP(const Double_t T[],       // track parameters (x,y,tx,ty,Q/p,z)
                               const Double_t V[],       // vertex parameters (x,y,z)
                               FairField* MagneticField  // magnetic field
)
{
 
  const Double_t c_light = 0.000299792458;
 
  Double_t x = T[0], y = T[1], tx = T[2], ty = T[3], qp0 = T[4], z = T[5],
 
           vx = V[0],
           //vy   = V[1],
    vz = V[2],
 
           txtx = tx * tx, tyty = ty * ty, txty = tx * ty;
 
  Double_t Ax = txty, Ay = -txtx - 1, Az = ty, Ayy = tx * (txtx * 3 + 3), Ayz = -2 * txty, Azy = Ayz,
           Ayyy = -(15 * txtx * txtx + 18 * txtx + 3),
 
           Bx = tyty + 1, By = -txty, Bz = -tx, Byy = ty * (txtx * 3 + 1), Byz = 2 * txtx + 1, Bzy = txtx - tyty,
           Byyy = -txty * (txtx * 15 + 9);
 
  Double_t t = c_light * sqrt(1. + txtx + tyty), h = t * qp0;
 
  // integrals
 
  Double_t Sx = 0, Sy = 0, Sz = 0, Syy = 0, Syz = 0, Szy = 0, Syyy = 0;
 
  {
    Double_t sx = 0, sy = 0, sz = 0, syy = 0, syz = 0, szy = 0, syyy = 0;
 
    Double_t r[3] = {x, y, z};
    Double_t B[3][3];
 
    Int_t n = int(fabs(vz - z) / 5.);
    if (n < 2) {
      n = 2;
    }
    Double_t dz = (vz - z) / n;
 
    {
      MagneticField->GetFieldValue(r, B[0]);
      r[0] += tx * dz;
      r[1] += ty * dz;
      r[2] += dz;
      MagneticField->GetFieldValue(r, B[1]);
      r[0] += tx * dz;
      r[1] += ty * dz;
      r[2] += dz;
      MagneticField->GetFieldValue(r, B[2]);
 
      sx = (B[0][0] + 4 * B[1][0] + B[2][0]) * dz * 2 / 6.;
      sy = (B[0][1] + 4 * B[1][1] + B[2][1]) * dz * 2 / 6.;
      sz = (B[0][2] + 4 * B[1][2] + B[2][2]) * dz * 2 / 6.;
 
      Sx = (B[0][0] + 2 * B[1][0]) * dz * dz * 4 / 6.;
      Sy = (B[0][1] + 2 * B[1][1]) * dz * dz * 4 / 6.;
      Sz = (B[0][2] + 2 * B[1][2]) * dz * dz * 4 / 6.;
 
      Double_t c2[3][3] = {{5, -4, -1}, {44, 80, -4}, {11, 44, 5}};    // /=360.
      Double_t C2[3][3] = {{38, 8, -4}, {148, 208, -20}, {3, 36, 3}};  // /=2520.
      for (Int_t k = 0; k < 3; k++) {
        for (Int_t m = 0; m < 3; m++) {
          syz += c2[k][m] * B[k][1] * B[m][2];
          Syz += C2[k][m] * B[k][1] * B[m][2];
          szy += c2[k][m] * B[k][2] * B[m][1];
          Szy += C2[k][m] * B[k][2] * B[m][1];
        }
      }
 
      syz *= dz * dz * 4 / 360.;
      Syz *= dz * dz * dz * 8 / 2520.;
 
      szy *= dz * dz * 4 / 360.;
      Szy *= dz * dz * dz * 8 / 2520.;
 
      syy  = (B[0][1] + 4 * B[1][1] + B[2][1]) * dz * 2;
      syyy = syy * syy * syy / 1296;
      syy  = syy * syy / 72;
 
      Syy = (B[0][1] * (38 * B[0][1] + 156 * B[1][1] - B[2][1]) + B[1][1] * (208 * B[1][1] + 16 * B[2][1])
             + B[2][1] * (3 * B[2][1]))
            * dz * dz * dz * 8 / 2520.;
      Syyy = (B[0][1]
                * (B[0][1] * (85 * B[0][1] + 526 * B[1][1] - 7 * B[2][1]) + B[1][1] * (1376 * B[1][1] + 84 * B[2][1])
                   + B[2][1] * (19 * B[2][1]))
              + B[1][1] * (B[1][1] * (1376 * B[1][1] + 256 * B[2][1]) + B[2][1] * (62 * B[2][1]))
              + B[2][1] * B[2][1] * (3 * B[2][1]))
             * dz * dz * dz * dz * 16 / 90720.;
 
      x += tx * 2 * dz + h * (Sx * Ax + Sy * Ay + Sz * Az) + h * h * (Syy * Ayy + Syz * Ayz + Szy * Azy)
           + h * h * h * Syyy * Ayyy;
      y += ty * 2 * dz + h * (Sx * Bx + Sy * By + Sz * Bz) + h * h * (Syy * Byy + Syz * Byz + Szy * Bzy)
           + h * h * h * Syyy * Byyy;
      tx += h * (sx * Ax + sy * Ay + sz * Az) + h * h * (syy * Ayy + syz * Ayz + szy * Azy) + h * h * h * syyy * Ayyy;
      ty += h * (sx * Bx + sy * By + sz * Bz) + h * h * (syy * Byy + syz * Byz + szy * Bzy) + h * h * h * syyy * Byyy;
      z += 2 * dz;
      txtx = tx * tx;
      tyty = ty * ty;
      txty = tx * ty;
 
      Ax   = txty;
      Ay   = -txtx - 1;
      Az   = ty;
      Ayy  = tx * (txtx * 3 + 3);
      Ayz  = -2 * txty;
      Azy  = Ayz;
      Ayyy = -(15 * txtx * txtx + 18 * txtx + 3);
 
      Bx   = tyty + 1;
      By   = -txty;
      Bz   = -tx;
      Byy  = ty * (txtx * 3 + 1);
      Byz  = 2 * txtx + 1;
      Bzy  = txtx - tyty;
      Byyy = -txty * (txtx * 15 + 9);
 
      t = c_light * sqrt(1. + txtx + tyty);
      h = t * qp0;
    }
 
    for (Int_t i = 2; i < n; i++) {
 
      Double_t B0[3] = {B[0][0], B[0][1], B[0][2]};
 
      B[0][0] = B[1][0];
      B[0][1] = B[1][1];
      B[0][2] = B[1][2];
 
      B[1][0] = B[2][0];
      B[1][1] = B[2][1];
      B[1][2] = B[2][2];
 
      // estimate B[2] at +dz
 
      B[2][0] = B0[0] - 3 * B[0][0] + 3 * B[1][0];
      B[2][1] = B0[1] - 3 * B[0][1] + 3 * B[1][1];
      B[2][2] = B0[2] - 3 * B[0][2] + 3 * B[1][2];
 
      Double_t Sx_, Sy_, Sz_, Syy_, Syz_, Szy_, Syyy_;
 
      Sx_ = (-B[0][0] + 10 * B[1][0] + 3 * B[2][0]) * dz * dz * 4 / 96.;
      Sy_ = (-B[0][1] + 10 * B[1][1] + 3 * B[2][1]) * dz * dz * 4 / 96.;
      Sz_ = (-B[0][2] + 10 * B[1][2] + 3 * B[2][2]) * dz * dz * 4 / 96.;
 
      Syz_ = Szy_ = 0;
 
      Double_t c2[3][3] = {{5, -52, -13}, {-28, 320, 68}, {-37, 332, 125}};      // /=5760
      Double_t C2[3][3] = {{13, -152, -29}, {-82, 1088, 170}, {-57, 576, 153}};  // /=80640
 
      for (Int_t k = 0; k < 3; k++) {
        for (Int_t m = 0; m < 3; m++) {
          Syz_ += C2[k][m] * B[k][1] * B[m][2];
          Szy_ += C2[k][m] * B[k][2] * B[m][1];
        }
      }
 
      Syz_ *= dz * dz * dz * 8 / 80640.;
      Szy_ *= dz * dz * dz * 8 / 80640.;
 
      Syy_ = (B[0][1] * (13 * B[0][1] - 234 * B[1][1] - 86 * B[2][1]) + B[1][1] * (1088 * B[1][1] + 746 * B[2][1])
              + B[2][1] * (153 * B[2][1]))
             * dz * dz * dz * 8 / 80640.;
 
      Syyy_ = (B[0][1]
                 * (B[0][1] * (-43 * B[0][1] + 1118 * B[1][1] + 451 * B[2][1])
                    + B[1][1] * (-9824 * B[1][1] - 7644 * B[2][1]) + B[2][1] * (-1669 * B[2][1]))
               + B[1][1] * (B[1][1] * (29344 * B[1][1] + 32672 * B[2][1]) + B[2][1] * (13918 * B[2][1]))
               + B[2][1] * B[2][1] * (2157 * B[2][1]))
              * dz * dz * dz * dz * 16 / 23224320.;
 
      r[2] += dz;
      r[0] = x + tx * dz + h * (Sx_ * Ax + Sy_ * Ay + Sz_ * Az) + h * h * (Syy_ * Ayy + Syz_ * Ayz + Szy_ * Azy)
             + h * h * h * Syyy_ * Ayyy;
      r[1] = y + ty * dz + h * (Sx_ * Bx + Sy_ * By + Sz_ * Bz) + h * h * (Syy_ * Byy + Syz_ * Byz + Szy_ * Bzy)
             + h * h * h * Syyy_ * Byyy;
      /* 
        Syyy_+= dz*syyy+Sy_*syy+Syy_*sy+Syyy;
        Syy_ += dz*syy + sy*Sy_ + Syy;
        Syz_ += dz*syz + sz*Sy_ + Syz;
        Szy_ += dz*szy + sy*Sz_ + Szy;
        Sx_  += dz*sx + Sx;
        Sy_  += dz*sy + Sy;
        Sz_  += dz*sz + Sz;
 
        r[2] += dz;
        r[0] = 
          x + tx*(r[2]-z) 
          + h*( Sx_*Ax + Sy_*Ay + Sz_*Az ) 
          + h*h*( Syy_*Ayy + Syz_*Ayz + Szy_*Azy )
          + h*h*h*Syyy_*Ayyy;
        r[1] = y + ty*(r[2]-z) 
          + h*( Sx_*Bx + Sy_*By + Sz_*Bz ) 
          + h*h*( Syy_*Byy + Syz_*Byz + Szy_*Bzy )
          + h*h*h*Syyy_*Byyy;
        */
      MagneticField->GetFieldValue(r, B[2]);
 
      Double_t sx_, sy_, sz_, syy_, syz_, szy_, syyy_;
 
      sx_ = (-B[0][0] + 8 * B[1][0] + 5 * B[2][0]) * dz * 2 / 24.;
      sy_ = (-B[0][1] + 8 * B[1][1] + 5 * B[2][1]) * dz * 2 / 24.;
      sz_ = (-B[0][2] + 8 * B[1][2] + 5 * B[2][2]) * dz * 2 / 24.;
 
      Sx_ = (-B[0][0] + 10 * B[1][0] + 3 * B[2][0]) * dz * dz * 4 / 96.;
      Sy_ = (-B[0][1] + 10 * B[1][1] + 3 * B[2][1]) * dz * dz * 4 / 96.;
      Sz_ = (-B[0][2] + 10 * B[1][2] + 3 * B[2][2]) * dz * dz * 4 / 96.;
 
      syz_ = Syz_ = szy_ = Szy_ = 0;
 
      for (Int_t k = 0; k < 3; k++) {
        for (Int_t m = 0; m < 3; m++) {
          syz_ += c2[k][m] * B[k][1] * B[m][2];
          Syz_ += C2[k][m] * B[k][1] * B[m][2];
          szy_ += c2[k][m] * B[k][2] * B[m][1];
          Szy_ += C2[k][m] * B[k][2] * B[m][1];
        }
      }
 
      syz_ *= dz * dz * 4 / 5760.;
      Syz_ *= dz * dz * dz * 8 / 80640.;
 
      szy_ *= dz * dz * 4 / 5760.;
      Szy_ *= dz * dz * dz * 8 / 80640.;
 
      syy_  = (B[0][1] - 8 * B[1][1] - 5 * B[2][1]) * dz * 2;
      syyy_ = -syy_ * syy_ * syy_ / 82944;
      syy_  = syy_ * syy_ / 1152;
 
      Syy_ = (B[0][1] * (13 * B[0][1] - 234 * B[1][1] - 86 * B[2][1]) + B[1][1] * (1088 * B[1][1] + 746 * B[2][1])
              + B[2][1] * (153 * B[2][1]))
             * dz * dz * dz * 8 / 80640.;
 
      Syyy_ = (B[0][1]
                 * (B[0][1] * (-43 * B[0][1] + 1118 * B[1][1] + 451 * B[2][1])
                    + B[1][1] * (-9824 * B[1][1] - 7644 * B[2][1]) + B[2][1] * (-1669 * B[2][1]))
               + B[1][1] * (B[1][1] * (29344 * B[1][1] + 32672 * B[2][1]) + B[2][1] * (13918 * B[2][1]))
               + B[2][1] * B[2][1] * (2157 * B[2][1]))
              * dz * dz * dz * dz * 16 / 23224320.;
 
      x += tx * dz + h * (Sx_ * Ax + Sy_ * Ay + Sz_ * Az) + h * h * (Syy_ * Ayy + Syz_ * Ayz + Szy_ * Azy)
           + h * h * h * Syyy_ * Ayyy;
      y += ty * dz + h * (Sx_ * Bx + Sy_ * By + Sz_ * Bz) + h * h * (Syy_ * Byy + Syz_ * Byz + Szy_ * Bzy)
           + h * h * h * Syyy_ * Byyy;
      tx += h * (sx_ * Ax + sy_ * Ay + sz_ * Az) + h * h * (syy_ * Ayy + syz_ * Ayz + szy_ * Azy)
            + h * h * h * syyy_ * Ayyy;
      ty += h * (sx_ * Bx + sy_ * By + sz_ * Bz) + h * h * (syy_ * Byy + syz_ * Byz + szy_ * Bzy)
            + h * h * h * syyy_ * Byyy;
      z += dz;
      txtx = tx * tx;
      tyty = ty * ty;
      txty = tx * ty;
 
      Ax   = txty;
      Ay   = -txtx - 1;
      Az   = ty;
      Ayy  = tx * (txtx * 3 + 3);
      Ayz  = -2 * txty;
      Azy  = Ayz;
      Ayyy = -(15 * txtx * txtx + 18 * txtx + 3);
 
      Bx   = tyty + 1;
      By   = -txty;
      Bz   = -tx;
      Byy  = ty * (txtx * 3 + 1);
      Byz  = 2 * txtx + 1;
      Bzy  = txtx - tyty;
      Byyy = -txty * (txtx * 15 + 9);
 
      t = c_light * sqrt(1. + txtx + tyty);
      h = t * qp0;
 
 
      Syyy += dz * syyy + Sy_ * syy + Syy_ * sy + Syyy_;
      Syy += dz * syy + sy * Sy_ + Syy_;
      Syz += dz * syz + sz * Sy_ + Syz_;
      Szy += dz * szy + sy * Sz_ + Szy_;
      Sx += dz * sx + Sx_;
      Sy += dz * sy + Sy_;
      Sz += dz * sz + Sz_;
 
      syyy += sy_ * syy + syy_ * sy + syyy_;
      syy += sy * sy_ + syy_;
      syz += sz * sy_ + syz_;
      szy += sz * sz_ + szy_;
      sx += sx_;
      sy += sy_;
      sz += sz_;
    }
    //cout<<x-vx<<" "<<y-vy<<" "<<z-vz<<" "<<1./qp0<<endl;
  }
 
  x    = T[0];
  y    = T[1];
  tx   = T[2];
  ty   = T[3];
  z    = T[5];
  txtx = tx * tx;
  tyty = ty * ty;
  txty = tx * ty;
 
  Ax   = txty;
  Ay   = -txtx - 1;
  Az   = ty;
  Ayy  = tx * (txtx * 3 + 3);
  Ayz  = -2 * txty;
  Azy  = Ayz;
  Ayyy = -(15 * txtx * txtx + 18 * txtx + 3);
 
  Bx   = tyty + 1;
  By   = -txty;
  Bz   = -tx;
  Byy  = ty * (txtx * 3 + 1);
  Byz  = 2 * txtx + 1;
  Bzy  = txtx - tyty;
  Byyy = -txty * (txtx * 15 + 9);
 
  t = c_light * sqrt(1. + txtx + tyty);
  h = t * qp0;
 
  Double_t
 
    c = (x - vx) + tx * (vz - z),
    b = t * (Sx * Ax + Sy * Ay + Sz * Az), a = t * t * (Syy * Ayy + Syz * Ayz + Szy * Azy),
    d = t * t * t * (Syyy * Ayyy);
 
  Double_t C = d * qp0 * qp0 * qp0 + a * qp0 * qp0 + b * qp0 + c, B = 3 * d * qp0 * qp0 + 2 * a * qp0 + b,
           A = 3 * d + a;
 
  Double_t D = B * B - 4 * A * C;
  if (D < 0.) {
    D = 0.;
  }
  D = sqrt(D);
 
  Double_t dqp1 = (-B + D) / 2 / A;
  Double_t dqp2 = (-B - D) / 2 / A;
  Double_t dqp  = (fabs(dqp1) < fabs(dqp2)) ? dqp1 : dqp2;
 
  return qp0 + dqp;
}
 
 
Bool_t CbmKFMath::GetThickness(Double_t z1, Double_t z2, Double_t mz, Double_t mthick, Double_t* mz_out,
                               Double_t* mthick_out)
{
  Double_t z, Z;
 
  if (z1 < z2) {
    z = z1;
    Z = z2;
  }
  else {
    Z = z1;
    z = z2;
  }
 
  Double_t tmin = mz - mthick / 2, tmax = mz + mthick / 2;
  if (tmin >= Z || z >= tmax) {
    return 1;
  }
  if (z <= tmin && tmax <= Z)  // case  z |**| Z
  {
    *mz_out     = mz;
    *mthick_out = mthick;
  }
  else if (z <= tmin && tmin < Z && Z <= tmax)  // case  z |*Z*|
  {
    *mz_out     = (tmin + Z) / 2;
    *mthick_out = Z - tmin;
  }
  else if (tmax <= Z && tmin <= z && z < tmax)  // case |*z*| Z
  {
    *mz_out     = (tmax + z) / 2;
    *mthick_out = tmax - z;
  }
  else if (tmin <= z && Z <= tmax)  // case |*zZ*|
  {
    *mz_out     = (Z + z) / 2;
    *mthick_out = Z - z;
  }
  return 0;
}
 
 
Int_t CbmKFMath::GetNoise(Double_t Lrl, Double_t F, Double_t Fe, Double_t tx, Double_t ty, Double_t qp, Double_t mass,
                          Bool_t is_electron, Bool_t downstream_direction, Double_t* Q5, Double_t* Q8, Double_t* Q9,
                          Double_t* Ecor)
{
  *Q5        = 0;
  *Q8        = 0;
  *Q9        = 0;
  *Ecor      = 1.;
  Double_t t = sqrt(1. + tx * tx + ty * ty);
  if (!std::isfinite(t) || t > 1.e4) {
    return 1;
  }
  t          = sqrt(t);
  Double_t l = t * Lrl;
  if (l > 1.) {
    l = 1.;  // protect against l too big
  }
  Double_t s0 = (l > exp(-1. / 0.038)) ? F * .0136 * (1 + 0.038 * log(l)) * qp : 0.;
  Double_t a  = (1. + mass * mass * qp * qp) * s0 * s0 * t * t * l;
 
  *Q5 = a * (1. + tx * tx);
  *Q8 = a * tx * ty;
  *Q9 = a * (1. + ty * ty);
 
  // Apply correction for dE/dx energy loss
 
  Double_t L = downstream_direction ? l : -l;
 
  if (0 && is_electron)  // Bremsstrahlung effect for e+,e-
  {
    *Ecor = exp(L);
  }
 
  if (1)  // ionisation energy loss
  {
    Double_t m_energyLoss = Fe;
    //      Double_t m_energyLoss = 0.02145;//0.060;
    Double_t corr = (1. - fabs(qp) * L * m_energyLoss);
    if (corr > 0.001 * fabs(qp)) {
      *Ecor *= 1. / corr;
    }
    else {
      *Ecor = 1000. / fabs(qp);
    }
  }
  return 0;
}
 
 
void CbmKFMath::CopyTC2TrackParam(FairTrackParam* par, Double_t T[], Double_t C[])
{
  if (T) {
    par->SetX(T[0]);
    par->SetY(T[1]);
    par->SetZ(T[5]);
    par->SetTx(T[2]);
    par->SetTy(T[3]);
    par->SetQp(T[4]);
  }
  if (C) {
    for (Int_t i = 0, iCov = 0; i < 5; i++) {
      for (Int_t j = 0; j <= i; j++, iCov++) {
        par->SetCovariance(i, j, C[iCov]);
      }
    }
  }
}
 
void CbmKFMath::CopyTrackParam2TC(const FairTrackParam* par, Double_t T[], Double_t C[])
{
  if (T) {
    T[0] = par->GetX();
    T[1] = par->GetY();
    T[2] = par->GetTx();
    T[3] = par->GetTy();
    T[4] = par->GetQp();
    T[5] = par->GetZ();
  }
  if (C) {
    for (Int_t i = 0, iCov = 0; i < 5; i++) {
      for (Int_t j = 0; j <= i; j++, iCov++) {
        C[iCov] = par->GetCovariance(i, j);
      }
    }
  }
}