CbmHelix.cxx

Go to the documentation of this file.

/*
 * CbmHelix.cxx
 *
 *  Created on: 27 sie 2019
 *      Author: Daniel Wielanek
 *              E-mail: daniel.wielanek@gmail.com
 *              Warsaw University of Technology, Faculty of Physics
 */
#include "CbmHelix.h"
#include <TMath.h>
#include <vector>
// matrix x, y, tx,ty, qp, z
FairField* CbmHelix::fgField = NULL;
CbmHelix::CbmHelix() {}
 
void CbmHelix::Build(const CbmGlobalTrack* tr) {
  const FairTrackParam* parameters = tr->GetParamVertex();
  SetParameters(parameters);
}
 
void CbmHelix::Build(const CbmStsTrack* tr, Bool_t firstPoint) {
  if (firstPoint) {
    SetParameters(tr->GetParamFirst());
  } else {
    SetParameters(tr->GetParamLast());
  }
}
 
TVector3 CbmHelix::Eval(Double_t z) {
  Propagate(z);
  return TVector3(GetTrack()[0], GetTrack()[1], GetTrack()[5]);
}
 
TVector3 CbmHelix::Eval(Double_t z, TVector3& mom) {
  Propagate(z);
  Double_t p  = (TMath::Abs(Qp()) > 1.e-4) ? 1. / TMath::Abs(Qp()) : 1.e4;
  Double_t pz = TMath::Sqrt(p * p / (Tx() * Tx() + Ty() * Ty() + 1));
  Double_t px = Tx() * pz;
  Double_t py = Ty() * pz;
  mom.SetXYZ(px, py, pz);
  return TVector3(GetTrack()[0], GetTrack()[1], GetTrack()[5]);
}
 
void CbmHelix::SetParameters(const FairTrackParam* param) {
  fT[0] = param->GetX();
  fT[1] = param->GetY();
  fT[2] = param->GetTx();
  fT[3] = param->GetTy();
  fT[4] = param->GetQp();
  fT[5] = param->GetZ();
  for (Int_t i = 0, iCov = 0; i < 5; i++)
    for (Int_t j = 0; j <= i; j++, iCov++)
      fC[iCov] = param->GetCovariance(i, j);
}
 
CbmHelix::~CbmHelix() {
  // TODO Auto-generated destructor stub
}
 
CbmHelix::CbmHelix(const CbmHelix& other) : TObject() {
  for (int i = 0; i < 6; i++)
    fT[i] = other.fT[i];
  for (int i = 0; i < 15; i++)
    fC[i] = other.fC[i];
}
 
CbmHelix& CbmHelix::operator=(const CbmHelix& other) {
  if (&other == this) return *this;
  for (int i = 0; i < 6; i++)
    fT[i] = other.fT[i];
  for (int i = 0; i < 15; i++)
    fC[i] = other.fC[i];
  return *this;
}
 
Int_t CbmHelix::Propagate(Double_t z) {
  Int_t fMethod = 1;
  Bool_t err    = 0;
  if (fabs(fT[5] - z) < 1.e-5) return 0;
 
  if (!fgField || (300 <= z && 300 <= fT[5])) {
    // ExtrapolateLine(Z );
    //  return 0;
  }
  Double_t zz = z;
  if (z < 300. && 300 <= fT[5]) ExtrapolateLine(300.);
 
  if (fT[5] < 300. && 300. < z) { zz = 300.; }
  Bool_t repeat = 1;
  while (!err && repeat) {
    const Double_t max_step = 5.;
    Double_t zzz;
    if (fabs(fT[5] - zz) > max_step)
      zzz = fT[5] + ((zz > fT[5]) ? max_step : -max_step);
    else {
      zzz    = zz;
      repeat = 0;
    }
    switch (fMethod) {
      case 0: {
        ExtrapolateLine(zzz);
        break;
      }
      case 1: {
        err = err || ExtrapolateALight(zzz);
        break;
      }
      case 2: {
        err = err || ExtrapolateRK4(zzz);
        break;
      }
    }
  }
  if (fT[5] != z) ExtrapolateLine(z);
  return err;
}
 
void CbmHelix::ExtrapolateLine(Double_t z_out) {
  Double_t dz = z_out - fT[5];
 
  fT[0] += dz * fT[2];
  fT[1] += dz * fT[3];
  fT[5] = z_out;
 
  const Double_t dzC_in8 = dz * fC[8];
 
  fC[4] = fC[4] + dzC_in8;
  fC[1] = fC[1] + dz * (fC[4] + fC[6]);
 
  const Double_t C_in3 = fC[3];
 
  fC[3] = C_in3 + dz * fC[5];
  fC[0] = fC[0] + dz * (fC[3] + C_in3);
 
  const Double_t C_in7 = fC[7];
 
  fC[7] = C_in7 + dz * fC[9];
  fC[2] = fC[2] + dz * (fC[7] + C_in7);
  fC[6] = fC[6] + dzC_in8;
}
 
Int_t CbmHelix::ExtrapolateRK4(Double_t z_out) {
  const Double_t c_light = 0.000299792458;
 
  static Double_t a[4] = {0.0, 0.5, 0.5, 1.0};
  static Double_t c[4] = {1.0 / 6.0, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 6.0};
  static Double_t b[4] = {0.0, 0.5, 0.5, 1.0};
 
  Int_t step4;
  Double_t k[16], x0[4], x[4], k1[16];
  Double_t Ax[4], Ay[4], Ax_tx[4], Ay_tx[4], Ax_ty[4], Ay_ty[4];
 
  //----------------------------------------------------------------
 
  Double_t qp_in = fT[4];
  Double_t z_in  = fT[5];
  Double_t h     = z_out - z_in;
  Double_t hC    = h * c_light;
  x0[0]          = fT[0];
  x0[1]          = fT[1];
  x0[2]          = fT[2];
  x0[3]          = fT[3];
  //
  //   Runge-Kutta step
  //
 
  Int_t step;
  Int_t i;
 
  for (step = 0; step < 4; ++step) {
    for (i = 0; i < 4; ++i) {
      if (step == 0) {
        x[i] = x0[i];
      } else {
        x[i] = x0[i] + b[step] * k[step * 4 - 4 + i];
      }
    }
 
    Double_t point[3] = {x[0], x[1], z_in + a[step] * h};
    Double_t B[3];
    if (fgField)
      fgField->GetFieldValue(point, B);
    else {
      B[0] = B[1] = B[2] = 0.;
    }
 
    Double_t tx      = x[2];
    Double_t ty      = x[3];
    Double_t tx2     = tx * tx;
    Double_t ty2     = ty * ty;
    Double_t txty    = tx * ty;
    Double_t tx2ty21 = 1.0 + tx2 + ty2;
    if (tx2ty21 > 1.e4) return 1;
    Double_t I_tx2ty21 = 1.0 / tx2ty21 * Qp();
    Double_t tx2ty2    = sqrt(tx2ty21);
    //   Double_t I_tx2ty2 = qp0 * hC / tx2ty2 ; unsused ???
    tx2ty2 *= hC;
    Double_t tx2ty2qp = tx2ty2 * Qp();
    Ax[step]          = (txty * B[0] + ty * B[2] - (1.0 + tx2) * B[1]) * tx2ty2;
    Ay[step] = (-txty * B[1] - tx * B[2] + (1.0 + ty2) * B[0]) * tx2ty2;
 
    Ax_tx[step] =
      Ax[step] * tx * I_tx2ty21 + (ty * B[0] - 2.0 * tx * B[1]) * tx2ty2qp;
    Ax_ty[step] = Ax[step] * ty * I_tx2ty21 + (tx * B[0] + B[2]) * tx2ty2qp;
    Ay_tx[step] = Ay[step] * tx * I_tx2ty21 + (-ty * B[1] - B[2]) * tx2ty2qp;
    Ay_ty[step] =
      Ay[step] * ty * I_tx2ty21 + (-tx * B[1] + 2.0 * ty * B[0]) * tx2ty2qp;
 
    step4        = step * 4;
    k[step4]     = tx * h;
    k[step4 + 1] = ty * h;
    k[step4 + 2] = Ax[step] * Qp();
    k[step4 + 3] = Ay[step] * Qp();
 
  }  // end of Runge-Kutta steps
 
  for (i = 0; i < 4; ++i) {
    fT[i] = x0[i] + c[0] * k[i] + c[1] * k[4 + i] + c[2] * k[8 + i]
            + c[3] * k[12 + i];
  }
  fT[5] = z_out;
  //
  //     Derivatives    dx/dqp
  //
 
  x0[0] = 0.0;
  x0[1] = 0.0;
  x0[2] = 0.0;
  x0[3] = 0.0;
 
  //
  //   Runge-Kutta step for derivatives dx/dqp
 
  for (step = 0; step < 4; ++step) {
    for (i = 0; i < 4; ++i) {
      if (step == 0) {
        x[i] = x0[i];
      } else {
        x[i] = x0[i] + b[step] * k1[step * 4 - 4 + i];
      }
    }
    step4         = step * 4;
    k1[step4]     = x[2] * h;
    k1[step4 + 1] = x[3] * h;
    k1[step4 + 2] = Ax[step] + Ax_tx[step] * x[2] + Ax_ty[step] * x[3];
    k1[step4 + 3] = Ay[step] + Ay_tx[step] * x[2] + Ay_ty[step] * x[3];
 
  }  // end of Runge-Kutta steps for derivatives dx/dqp
 
  Double_t J[25];
 
  for (i = 0; i < 4; ++i) {
    J[20 + i] = x0[i] + c[0] * k1[i] + c[1] * k1[4 + i] + c[2] * k1[8 + i]
                + c[3] * k1[12 + i];
  }
  J[24] = 1.;
  //
  //      end of derivatives dx/dqp
  //
 
  //     Derivatives    dx/tx
  //
 
  x0[0] = 0.0;
  x0[1] = 0.0;
  x0[2] = 1.0;
  x0[3] = 0.0;
 
  //
  //   Runge-Kutta step for derivatives dx/dtx
  //
 
  for (step = 0; step < 4; ++step) {
    for (i = 0; i < 4; ++i) {
      if (step == 0) {
        x[i] = x0[i];
      } else if (i != 2) {
        x[i] = x0[i] + b[step] * k1[step * 4 - 4 + i];
      }
    }
    step4         = step * 4;
    k1[step4]     = x[2] * h;
    k1[step4 + 1] = x[3] * h;
    //    k1[step4+2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3];
    k1[step4 + 3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3];
 
  }  // end of Runge-Kutta steps for derivatives dx/dtx
 
  for (i = 0; i < 4; ++i) {
    if (i != 2) {
      J[10 + i] = x0[i] + c[0] * k1[i] + c[1] * k1[4 + i] + c[2] * k1[8 + i]
                  + c[3] * k1[12 + i];
    }
  }
  //      end of derivatives dx/dtx
  J[12] = 1.0;
  J[14] = 0.0;
 
  //     Derivatives    dx/ty
  //
 
  x0[0] = 0.0;
  x0[1] = 0.0;
  x0[2] = 0.0;
  x0[3] = 1.0;
 
  //
  //   Runge-Kutta step for derivatives dx/dty
  //
 
  for (step = 0; step < 4; ++step) {
    for (i = 0; i < 4; ++i) {
      if (step == 0) {
        x[i] = x0[i];  // ty fixed
      } else if (i != 3) {
        x[i] = x0[i] + b[step] * k1[step * 4 - 4 + i];
      }
    }
    step4         = step * 4;
    k1[step4]     = x[2] * h;
    k1[step4 + 1] = x[3] * h;
    k1[step4 + 2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3];
    //    k1[step4+3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3];
 
  }  // end of Runge-Kutta steps for derivatives dx/dty
 
  for (i = 0; i < 3; ++i) {
    J[15 + i] = x0[i] + c[0] * k1[i] + c[1] * k1[4 + i] + c[2] * k1[8 + i]
                + c[3] * k1[12 + i];
  }
  //      end of derivatives dx/dty
  J[18] = 1.;
  J[19] = 0.;
 
  //
  //    derivatives dx/dx and dx/dy
 
  for (i = 0; i < 10; ++i) {
    J[i] = 0.;
  }
  J[0] = 1.;
  J[6] = 1.;
 
  // extrapolate inverse momentum
 
  fT[4] = qp_in;
 
  Double_t dqp = qp_in - Qp();
 
  {
    for (Int_t ip = 0; ip < 4; ip++) {
      fT[ip] += J[5 * 4 + ip] * dqp;
    }
  }
 
  //          covariance matrix transport
 
  multQtSQ(5, J);
  return 0;
}
 
Int_t CbmHelix::ExtrapolateALight(Double_t z_out) {
  //
  //  Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
  //
  {
    bool ok = 1;
    for (int i = 0; i < 6; i++)
      ok = ok && !TMath::IsNaN(fT[i]) && (fT[i] < 1.e5);
    for (int i = 0; i < 15; i++)
      ok = ok && !TMath::IsNaN(fC[i]);
    if (!ok) {
      for (int i = 0; i < 15; i++)
        fC[i] = 0;
      fC[0] = fC[2] = fC[5] = fC[9] = fC[14] = 100.;
      return 1;
    }
  }
  const Double_t c_light = 0.000299792458;
 
  //Double_t qp_in = fT[4];
  Double_t z_in = fT[5];
  Double_t dz   = z_out - z_in;
 
  // construct coefficients
 
  Double_t x = fT[2],  // tx !!
    y        = fT[3],  // ty !!
 
    xx = x * x, xy = x * y, yy = y * y, x2 = x * 2, x4 = x * 4,
           xx31 = xx * 3 + 1, xx159 = xx * 15 + 9, y2 = y * 2;
 
  Double_t Ax = xy, Ay = -xx - 1, Az = y, Ayy = x * (xx * 3 + 3), Ayz = -2 * xy,
           Ayyy = -(15 * xx * xx + 18 * xx + 3),
 
           Ax_x = y, Ay_x = -x2, Az_x = 0, Ayy_x = 3 * xx31, Ayz_x = -y2,
           Ayyy_x = -x4 * xx159,
 
           Ax_y = x, Ay_y = 0, Az_y = 1, Ayy_y = 0, Ayz_y = -x2, Ayyy_y = 0,
 
           Bx = yy + 1, By = -xy, Bz = -x, Byy = y * xx31, Byz = 2 * xx + 1,
           Byyy = -xy * xx159,
 
           Bx_x = 0, By_x = -y, Bz_x = -1, Byy_x = 6 * xy, Byz_x = x4,
           Byyy_x = -y * (45 * xx + 9),
 
           Bx_y = y2, By_y = -x, Bz_y = 0, Byy_y = xx31, Byz_y = 0,
           Byyy_y = -x * xx159;
 
  // end of coefficients calculation
 
  Double_t t2 = 1. + xx + yy;
  if (t2 > 1.e4) return 1;
  Double_t t = sqrt(t2), h = Qp() * c_light, ht = h * t;
 
  Double_t sx = 0, sy = 0, sz = 0, syy = 0, syz = 0, syyy = 0, Sx = 0, Sy = 0,
           Sz = 0, Syy = 0, Syz = 0, Syyy = 0;
 
  {  // get field integrals
 
    Double_t B[3][3];
    Double_t r0[3], r1[3], r2[3];
 
    // first order track approximation
 
    r0[0] = fT[0];
    r0[1] = fT[1];
    r0[2] = fT[5];
 
    r2[0] = fT[0] + fT[2] * dz;
    r2[1] = fT[1] + fT[3] * dz;
    r2[2] = z_out;
 
    r1[0] = 0.5 * (r0[0] + r2[0]);
    r1[1] = 0.5 * (r0[1] + r2[1]);
    r1[2] = 0.5 * (r0[2] + r2[2]);
 
    fgField->GetFieldValue(r0, B[0]);
    fgField->GetFieldValue(r1, B[1]);
    fgField->GetFieldValue(r2, B[2]);
 
    Sy    = (7 * B[0][1] + 6 * B[1][1] - B[2][1]) * dz * dz / 96.;
    r1[0] = fT[0] + x * dz / 2 + ht * Sy * Ay;
    r1[1] = fT[1] + y * dz / 2 + ht * Sy * By;
 
    Sy    = (B[0][1] + 2 * B[1][1]) * dz * dz / 6.;
    r2[0] = fT[0] + x * dz + ht * Sy * Ay;
    r2[1] = fT[1] + y * dz + ht * Sy * By;
 
    Sy = 0;
 
    // integrals
 
    fgField->GetFieldValue(r0, B[0]);
    fgField->GetFieldValue(r1, B[1]);
    fgField->GetFieldValue(r2, B[2]);
 
    sx = (B[0][0] + 4 * B[1][0] + B[2][0]) * dz / 6.;
    sy = (B[0][1] + 4 * B[1][1] + B[2][1]) * dz / 6.;
    sz = (B[0][2] + 4 * B[1][2] + B[2][2]) * dz / 6.;
 
    Sx = (B[0][0] + 2 * B[1][0]) * dz * dz / 6.;
    Sy = (B[0][1] + 2 * B[1][1]) * dz * dz / 6.;
    Sz = (B[0][2] + 2 * B[1][2]) * dz * dz / 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 n = 0; n < 3; n++)
      for (Int_t m = 0; m < 3; m++) {
        syz += c2[n][m] * B[n][1] * B[m][2];
        Syz += C2[n][m] * B[n][1] * B[m][2];
      }
 
    syz *= dz * dz / 360.;
    Syz *= dz * dz * dz / 2520.;
 
    syy  = (B[0][1] + 4 * B[1][1] + B[2][1]) * dz;
    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 / 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 / 90720.;
  }
 
  Double_t
 
    sA1   = sx * Ax + sy * Ay + sz * Az,
    sA1_x = sx * Ax_x + sy * Ay_x + sz * Az_x,
    sA1_y = sx * Ax_y + sy * Ay_y + sz * Az_y,
 
    sB1   = sx * Bx + sy * By + sz * Bz,
    sB1_x = sx * Bx_x + sy * By_x + sz * Bz_x,
    sB1_y = sx * Bx_y + sy * By_y + sz * Bz_y,
 
    SA1   = Sx * Ax + Sy * Ay + Sz * Az,
    SA1_x = Sx * Ax_x + Sy * Ay_x + Sz * Az_x,
    SA1_y = Sx * Ax_y + Sy * Ay_y + Sz * Az_y,
 
    SB1   = Sx * Bx + Sy * By + Sz * Bz,
    SB1_x = Sx * Bx_x + Sy * By_x + Sz * Bz_x,
    SB1_y = Sx * Bx_y + Sy * By_y + Sz * Bz_y,
 
    sA2 = syy * Ayy + syz * Ayz, sA2_x = syy * Ayy_x + syz * Ayz_x,
    sA2_y = syy * Ayy_y + syz * Ayz_y, sB2 = syy * Byy + syz * Byz,
    sB2_x = syy * Byy_x + syz * Byz_x, sB2_y = syy * Byy_y + syz * Byz_y,
 
    SA2 = Syy * Ayy + Syz * Ayz, SA2_x = Syy * Ayy_x + Syz * Ayz_x,
    SA2_y = Syy * Ayy_y + Syz * Ayz_y, SB2 = Syy * Byy + Syz * Byz,
    SB2_x = Syy * Byy_x + Syz * Byz_x, SB2_y = Syy * Byy_y + Syz * Byz_y,
 
    sA3 = syyy * Ayyy, sA3_x = syyy * Ayyy_x, sA3_y = syyy * Ayyy_y,
    sB3 = syyy * Byyy, sB3_x = syyy * Byyy_x, sB3_y = syyy * Byyy_y,
 
    SA3 = Syyy * Ayyy, SA3_x = Syyy * Ayyy_x, SA3_y = Syyy * Ayyy_y,
    SB3 = Syyy * Byyy, SB3_x = Syyy * Byyy_x, SB3_y = Syyy * Byyy_y;
#ifdef SKIP_LOSSES
  fT[0] = fT[0] + x * dz + ht * (SA1 + ht * (SA2 + ht * SA3));
  fT[1] = fT[1] + y * dz + ht * (SB1 + ht * (SB2 + ht * SB3));
  fT[2] = fT[2] + ht * (sA1 + ht * (sA2 + ht * sA3));
  fT[3] = fT[3] + ht * (sB1 + ht * (sB2 + ht * sB3));
  fT[5] = z_out;
#else
  T_out[0] = fT[0] + x * dz + ht * (SA1 + ht * (SA2 + ht * SA3));
  T_out[1] = fT[1] + y * dz + ht * (SB1 + ht * (SB2 + ht * SB3));
  T_out[2] = fT[2] + ht * (sA1 + ht * (sA2 + ht * sA3));
  T_out[3] = fT[3] + ht * (sB1 + ht * (sB2 + ht * sB3));
  T_out[4] = fT[4];
  T_out[5] = z_out;
#endif
  Double_t J[25];
 
  // derivatives '_x
 
  J[0] = 1;
  J[1] = 0;
  J[2] = 0;
  J[3] = 0;
  J[4] = 0;
 
  // derivatives '_y
 
  J[5] = 0;
  J[6] = 1;
  J[7] = 0;
  J[8] = 0;
  J[9] = 0;
 
  // derivatives '_tx
 
  J[10] = dz + h * x * (1. / t * SA1 + h * (2 * SA2 + 3 * ht * SA3))
          + ht * (SA1_x + ht * (SA2_x + ht * SA3_x));
  J[11] = h * x * (1. / t * SB1 + h * (2 * SB2 + 3 * ht * SB3))
          + ht * (SB1_x + ht * (SB2_x + ht * SB3_x));
  J[12] = 1 + h * x * (1. / t * sA1 + h * (2 * sA2 + 3 * ht * sA3))
          + ht * (sA1_x + ht * (sA2_x + ht * sA3_x));
  J[13] = h * x * (1. / t * sB1 + h * (2 * sB2 + 3 * ht * sB3))
          + ht * (sB1_x + ht * (sB2_x + ht * sB3_x));
  J[14] = 0;
 
  // derivatives '_ty
 
  J[15] = h * y * (1. / t * SA1 + h * (2 * SA2 + 3 * ht * SA3))
          + ht * (SA1_y + ht * (SA2_y + ht * SA3_y));
  J[16] = dz + h * y * (1. / t * SB1 + h * (2 * SB2 + 3 * ht * SB3))
          + ht * (SB1_y + ht * (SB2_y + ht * SB3_y));
  J[17] = h * y * (1. / t * sA1 + h * (2 * sA2 + 3 * ht * sA3))
          + ht * (sA1_y + ht * (sA2_y + ht * sA3_y));
  J[18] = 1 + h * y * (1. / t * sB1 + h * (2 * sB2 + 3 * ht * sB3))
          + ht * (sB1_y + ht * (sB2_y + ht * sB3_y));
  J[19] = 0;
 
  // derivatives '_qp
 
  J[20] = c_light * t * (SA1 + ht * (2 * SA2 + 3 * ht * SA3));
  J[21] = c_light * t * (SB1 + ht * (2 * SB2 + 3 * ht * SB3));
  J[22] = c_light * t * (sA1 + ht * (2 * sA2 + 3 * ht * sA3));
  J[23] = c_light * t * (sB1 + ht * (2 * sB2 + 3 * ht * sB3));
  J[24] = 1;
 
  // extrapolate inverse momentum
#ifdef SKIP_LOSSES
#else
  T_out[4] = qp_in;
 
  Double_t dqp = qp_in - Qp();
 
 
  for (Int_t i = 0; i < 4; i++) {
    fT[i] = T_out[i] + J[5 * 4 + i] * dqp;
  }
  fT[4] = T_out[4];
  fT[5] = T_out[5];
#endif
  //          covariance matrix transport
 
  multQtSQ(5, J);
  return 0;
}
 
void CbmHelix::multQtSQ(const Int_t N, Double_t Q[]) {
  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] += fC[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);
      fC[n]   = 0;
      for (Int_t k = 0; k < N; k++)
        fC[n] += Q[k * N + i] * A[k * N + j];
    }
  }
}