CbmRichRingFitterEllipseTau.cxx

Go to the documentation of this file.

/* Copyright (C) 2009-2016 UGiessen/JINR-LIT, Giessen/Dubna
   SPDX-License-Identifier: GPL-3.0-only
   Authors: Alexander Ayriyan, Semen Lebedev [committer] */
 
// GMij are indices for a 5x5 matrix.
#define GM00 0
#define GM01 1
#define GM02 2
#define GM03 3
#define GM04 4
 
#define GM10 5
#define GM11 6
#define GM12 7
#define GM13 8
#define GM14 9
 
#define GM20 10
#define GM21 11
#define GM22 12
#define GM23 13
#define GM24 14
 
#define GM30 15
#define GM31 16
#define GM32 17
#define GM33 18
#define GM34 19
 
#define GM40 20
#define GM41 21
#define GM42 22
#define GM43 23
#define GM44 24
 
 
// Aij are indices for a 6x6 matrix.
 
#define GA00 0
#define GA01 1
#define GA02 2
#define GA03 3
#define GA04 4
#define GA05 5
 
#define GA10 6
#define GA11 7
#define GA12 8
#define GA13 9
#define GA14 10
#define GA15 11
 
#define GA20 12
#define GA21 13
#define GA22 14
#define GA23 15
#define GA24 16
#define GA25 17
 
#define GA30 18
#define GA31 19
#define GA32 20
#define GA33 21
#define GA34 22
#define GA35 23
 
#define GA40 24
#define GA41 25
#define GA42 26
#define GA43 27
#define GA44 28
#define GA45 29
 
#define GA50 30
#define GA51 31
#define GA52 32
#define GA53 33
#define GA54 34
#define GA55 35
 
#include "CbmRichRingFitterEllipseTau.h"
 
 
using std::cout;
using std::endl;
 
CbmRichRingFitterEllipseTau::CbmRichRingFitterEllipseTau() {}
 
CbmRichRingFitterEllipseTau::~CbmRichRingFitterEllipseTau() {}
 
void CbmRichRingFitterEllipseTau::DoFit(CbmRichRingLight* ring)
{
  int nofHits = ring->GetNofHits();
 
  if (nofHits <= 5) {
    ring->SetXYABP(-1., -1., -1., -1., -1.);
    ring->SetRadius(-1.);
    return;
  }
 
  if (nofHits >= MAX_NOF_HITS_IN_RING) {
    cout << "-E- CbmRichRingFitterEllipseTau::DoFit(), too many hits in the ring:" << nofHits << endl;
    ring->SetXYABP(-1., -1., -1., -1., -1.);
    ring->SetRadius(-1.);
    return;
  }
 
  //for (int i = 0; i < nofHits; i++) {
  //    CbmRichHit* hit = (CbmRichHit*) fHitsArray->At(ring->GetHit(i));
  //    fX.push_back(hit->GetX());
  //    fY.push_back(hit->GetY());
  //}
 
  InitMatrices(ring);
  Taubin();
  TransformEllipse(ring);
  CalcChi2(ring);
}
 
void CbmRichRingFitterEllipseTau::Taubin()
{
  //    TMatrixD PQ(5,5); // fPQ = P^(-1) * Q
 
  Inv5x5();
  Double_t PQa[25];
  AMultB(fP, 25, 5, fQ, 25, 5, PQa);  //PQ = fP*fQ;
  TMatrixD PQ(5, 5, PQa);
 
  //    Double_t PQa2[5][5];
  //    Double_t d[5];
  //    Double_t v[5][5];
  //    for(Int_t i = 0; i<5; i++){
  //            for (Int_t j = 0; j<5;j++){
  //                    PQa2[i][j] = PQa[5*i+j];
  //                    cout << PQa2[i][j] << "  ";
  //            }
  //            cout << endl;
  //    }
  //    cout << endl << endl;
  //
  //    Jacobi(PQa2, d, v);
  //    Eigsrt(d, v);
 
  TMatrixDEigen eig(PQ);
  TMatrixD evc = eig.GetEigenVectors();
 
  Double_t AlgParF = 0.;
  AlgParF -= evc(0, 0) * fM[GA05];
  fAlgPar[0] = evc(0, 0);
 
  AlgParF -= evc(1, 0) * fM[GA15];
  fAlgPar[1] = evc(1, 0);
 
  AlgParF -= evc(2, 0) * fM[GA25];
  fAlgPar[2] = evc(2, 0);
 
  AlgParF -= evc(3, 0) * fM[GA35];
  fAlgPar[3] = evc(3, 0);
 
  AlgParF -= evc(4, 0) * fM[GA45];
  fAlgPar[4] = evc(4, 0);
 
  fAlgPar[5] = AlgParF;
}
 
void CbmRichRingFitterEllipseTau::InitMatrices(CbmRichRingLight* ring)
{
  const unsigned int numHits  = ring->GetNofHits();
  const unsigned int numHits2 = 2 * numHits;
  const unsigned int numHits3 = 3 * numHits;
  const unsigned int numHits4 = 4 * numHits;
  const unsigned int numHits5 = 5 * numHits;
  const unsigned int numHits6 = 6 * numHits;
  const double oneOverNumHits = 1. / numHits;
  unsigned int i6;
  for (unsigned int i = 0; i < numHits; i++) {
    double x = ring->GetHit(i).fX;
    double y = ring->GetHit(i).fY;
    i6       = i * 6;
    fZ[i6] = fZT[i] = x * x;
    fZ[i6 + 1] = fZT[i + numHits] = x * y;
    fZ[i6 + 2] = fZT[i + numHits2] = y * y;
    fZ[i6 + 3] = fZT[i + numHits3] = x;
    fZ[i6 + 4] = fZT[i + numHits4] = y;
    fZ[i6 + 5] = fZT[i + numHits5] = 1.;
  }
  AMultB(fZT, numHits6, numHits, fZ, numHits6, 6, fM);
  for (unsigned int i = 0; i < numHits6; i++)
    fM[i] = oneOverNumHits * fM[i];
 
  for (int i = 0; i < 25; i++)
    fP[i] = 0.;
 
  fP[GM00] = fM[GA00] - fM[GA05] * fM[GA05];
  fP[GM01] = fP[GM10] = fM[GA01] - fM[GA05] * fM[GA15];
  fP[GM02] = fP[GM20] = fM[GA02] - fM[GA05] * fM[GA25];
  fP[GM03] = fP[GM30] = fM[GA03] - fM[GA05] * fM[GA35];
  fP[GM04] = fP[GM40] = fM[GA04] - fM[GA05] * fM[GA45];
 
  fP[GM11] = fM[GA11] - fM[GA15] * fM[GA15];
  fP[GM12] = fP[GM21] = fM[GA12] - fM[GA15] * fM[GA25];
  fP[GM13] = fP[GM31] = fM[GA13] - fM[GA15] * fM[GA35];
  fP[GM14] = fP[GM41] = fM[GA14] - fM[GA15] * fM[GA45];
 
  fP[GM22] = fM[GA22] - fM[GA25] * fM[GA25];
  fP[GM23] = fP[GM32] = fM[GA23] - fM[GA25] * fM[GA35];
  fP[GM24] = fP[GM42] = fM[GA24] - fM[GA25] * fM[GA45];
 
  fP[GM33] = fM[GA33] - fM[GA35] * fM[GA35];
  fP[GM34] = fP[GM43] = fM[GA34] - fM[GA35] * fM[GA45];
 
  fP[GM44] = fM[GA44] - fM[GA45] * fM[GA45];
 
 
  for (int i = 0; i < 25; i++)
    fQ[i] = 0.;
  fQ[GM00] = 4. * fM[GA50];
  fQ[GM01] = fQ[GM10] = 2. * fM[GA51];
  fQ[GM03] = fQ[GM30] = 2. * fM[GA53];
  fQ[GM11]            = fM[GA50] + fM[GA52];
  fQ[GM12] = fQ[GM21] = 2. * fM[GA51];
  fQ[GM13] = fQ[GM31] = fM[GA54];
  fQ[GM14] = fQ[GM41] = fM[GA53];
  fQ[GM22]            = 4. * fM[GA52];
  fQ[GM24] = fQ[GM42] = 2. * fM[GA54];
  fQ[GM33] = fQ[GM44] = 1.;
}
 
void CbmRichRingFitterEllipseTau::TransformEllipse(CbmRichRingLight* ring)
{
  double Pxx = fAlgPar[0];
  double Pxy = fAlgPar[1];
  double Pyy = fAlgPar[2];
  double Px  = fAlgPar[3];
  double Py  = fAlgPar[4];
  double P   = fAlgPar[5];
  CalcChi2(Pxx, Pxy, Pyy, Px, Py, P, ring);
  ring->SetABCDEF(Pxx, Pxy, Pyy, Px, Py, P);
 
  double alpha;
  double QQx, QQy, Qxx, Qyy, Qx, Qy, Q;
  double cosa, sina, cca, ssa, sin2a;
  double xc, yc;
  if (fabs(Pxx - Pyy) > 0.1e-10) {
    alpha = atan(Pxy / (Pxx - Pyy));
    alpha = alpha / 2.0;
  }
  else
    alpha = 1.57079633;
 
  cosa  = cos(alpha);
  sina  = sin(alpha);
  cca   = cosa * cosa;
  ssa   = sina * sina;
  sin2a = sin(2. * alpha);
  Pxy   = Pxy * sin2a / 2.;
  Qx    = Px * cosa + Py * sina;
  Qy    = -Px * sina + Py * cosa;
  QQx   = Qx * Qx;
  QQy   = Qy * Qy;
  Qxx   = 1. / (Pxx * cca + Pxy + Pyy * ssa);
  Qyy   = 1. / (Pxx * ssa - Pxy + Pyy * cca);
  Q     = -P + Qxx * QQx / 4. + Qyy * QQy / 4.;
  xc    = Qx * Qxx;
  yc    = Qy * Qyy;
 
  double axisA   = TMath::Sqrt(Q * Qxx);
  double axisB   = TMath::Sqrt(Q * Qyy);
  double centerX = -xc * cosa / 2. + yc * sina / 2.;
  double centerY = -xc * sina / 2. - yc * cosa / 2.;
 
  ring->SetXYABP(centerX, centerY, axisA, axisB, alpha);
  ring->SetRadius((axisA + axisB) / 2.);
 
  if (ring->GetAaxis() < ring->GetBaxis()) {
    double tmp = ring->GetAaxis();
    ring->SetAaxis(ring->GetBaxis());
    ring->SetBaxis(tmp);
 
    tmp = ring->GetPhi();
    if (ring->GetPhi() <= 0)
      ring->SetPhi(ring->GetPhi() + 1.57079633);
    else
      ring->SetPhi(ring->GetPhi() - 1.57079633);
  }
}
 
void CbmRichRingFitterEllipseTau::Inv5x5()
{
  // Find all NECESSARY 2x2 dets:  (30 of them)
  const double det2_23_01 = fP[GM20] * fP[GM31] - fP[GM21] * fP[GM30];
  const double det2_23_02 = fP[GM20] * fP[GM32] - fP[GM22] * fP[GM30];
  const double det2_23_03 = fP[GM20] * fP[GM33] - fP[GM23] * fP[GM30];
  const double det2_23_04 = fP[GM20] * fP[GM34] - fP[GM24] * fP[GM30];
  const double det2_23_12 = fP[GM21] * fP[GM32] - fP[GM22] * fP[GM31];
  const double det2_23_13 = fP[GM21] * fP[GM33] - fP[GM23] * fP[GM31];
  const double det2_23_14 = fP[GM21] * fP[GM34] - fP[GM24] * fP[GM31];
  const double det2_23_23 = fP[GM22] * fP[GM33] - fP[GM23] * fP[GM32];
  const double det2_23_24 = fP[GM22] * fP[GM34] - fP[GM24] * fP[GM32];
  const double det2_23_34 = fP[GM23] * fP[GM34] - fP[GM24] * fP[GM33];
  const double det2_24_01 = fP[GM20] * fP[GM41] - fP[GM21] * fP[GM40];
  const double det2_24_02 = fP[GM20] * fP[GM42] - fP[GM22] * fP[GM40];
  const double det2_24_03 = fP[GM20] * fP[GM43] - fP[GM23] * fP[GM40];
  const double det2_24_04 = fP[GM20] * fP[GM44] - fP[GM24] * fP[GM40];
  const double det2_24_12 = fP[GM21] * fP[GM42] - fP[GM22] * fP[GM41];
  const double det2_24_13 = fP[GM21] * fP[GM43] - fP[GM23] * fP[GM41];
  const double det2_24_14 = fP[GM21] * fP[GM44] - fP[GM24] * fP[GM41];
  const double det2_24_23 = fP[GM22] * fP[GM43] - fP[GM23] * fP[GM42];
  const double det2_24_24 = fP[GM22] * fP[GM44] - fP[GM24] * fP[GM42];
  const double det2_24_34 = fP[GM23] * fP[GM44] - fP[GM24] * fP[GM43];
  const double det2_34_01 = fP[GM30] * fP[GM41] - fP[GM31] * fP[GM40];
  const double det2_34_02 = fP[GM30] * fP[GM42] - fP[GM32] * fP[GM40];
  const double det2_34_03 = fP[GM30] * fP[GM43] - fP[GM33] * fP[GM40];
  const double det2_34_04 = fP[GM30] * fP[GM44] - fP[GM34] * fP[GM40];
  const double det2_34_12 = fP[GM31] * fP[GM42] - fP[GM32] * fP[GM41];
  const double det2_34_13 = fP[GM31] * fP[GM43] - fP[GM33] * fP[GM41];
  const double det2_34_14 = fP[GM31] * fP[GM44] - fP[GM34] * fP[GM41];
  const double det2_34_23 = fP[GM32] * fP[GM43] - fP[GM33] * fP[GM42];
  const double det2_34_24 = fP[GM32] * fP[GM44] - fP[GM34] * fP[GM42];
  const double det2_34_34 = fP[GM33] * fP[GM44] - fP[GM34] * fP[GM43];
 
  // Find all NECESSARY 3x3 dets:   (40 of them)
  const double det3_123_012 = fP[GM10] * det2_23_12 - fP[GM11] * det2_23_02 + fP[GM12] * det2_23_01;
  const double det3_123_013 = fP[GM10] * det2_23_13 - fP[GM11] * det2_23_03 + fP[GM13] * det2_23_01;
  const double det3_123_014 = fP[GM10] * det2_23_14 - fP[GM11] * det2_23_04 + fP[GM14] * det2_23_01;
  const double det3_123_023 = fP[GM10] * det2_23_23 - fP[GM12] * det2_23_03 + fP[GM13] * det2_23_02;
  const double det3_123_024 = fP[GM10] * det2_23_24 - fP[GM12] * det2_23_04 + fP[GM14] * det2_23_02;
  const double det3_123_034 = fP[GM10] * det2_23_34 - fP[GM13] * det2_23_04 + fP[GM14] * det2_23_03;
  const double det3_123_123 = fP[GM11] * det2_23_23 - fP[GM12] * det2_23_13 + fP[GM13] * det2_23_12;
  const double det3_123_124 = fP[GM11] * det2_23_24 - fP[GM12] * det2_23_14 + fP[GM14] * det2_23_12;
  const double det3_123_134 = fP[GM11] * det2_23_34 - fP[GM13] * det2_23_14 + fP[GM14] * det2_23_13;
  const double det3_123_234 = fP[GM12] * det2_23_34 - fP[GM13] * det2_23_24 + fP[GM14] * det2_23_23;
  const double det3_124_012 = fP[GM10] * det2_24_12 - fP[GM11] * det2_24_02 + fP[GM12] * det2_24_01;
  const double det3_124_013 = fP[GM10] * det2_24_13 - fP[GM11] * det2_24_03 + fP[GM13] * det2_24_01;
  const double det3_124_014 = fP[GM10] * det2_24_14 - fP[GM11] * det2_24_04 + fP[GM14] * det2_24_01;
  const double det3_124_023 = fP[GM10] * det2_24_23 - fP[GM12] * det2_24_03 + fP[GM13] * det2_24_02;
  const double det3_124_024 = fP[GM10] * det2_24_24 - fP[GM12] * det2_24_04 + fP[GM14] * det2_24_02;
  const double det3_124_034 = fP[GM10] * det2_24_34 - fP[GM13] * det2_24_04 + fP[GM14] * det2_24_03;
  const double det3_124_123 = fP[GM11] * det2_24_23 - fP[GM12] * det2_24_13 + fP[GM13] * det2_24_12;
  const double det3_124_124 = fP[GM11] * det2_24_24 - fP[GM12] * det2_24_14 + fP[GM14] * det2_24_12;
  const double det3_124_134 = fP[GM11] * det2_24_34 - fP[GM13] * det2_24_14 + fP[GM14] * det2_24_13;
  const double det3_124_234 = fP[GM12] * det2_24_34 - fP[GM13] * det2_24_24 + fP[GM14] * det2_24_23;
  const double det3_134_012 = fP[GM10] * det2_34_12 - fP[GM11] * det2_34_02 + fP[GM12] * det2_34_01;
  const double det3_134_013 = fP[GM10] * det2_34_13 - fP[GM11] * det2_34_03 + fP[GM13] * det2_34_01;
  const double det3_134_014 = fP[GM10] * det2_34_14 - fP[GM11] * det2_34_04 + fP[GM14] * det2_34_01;
  const double det3_134_023 = fP[GM10] * det2_34_23 - fP[GM12] * det2_34_03 + fP[GM13] * det2_34_02;
  const double det3_134_024 = fP[GM10] * det2_34_24 - fP[GM12] * det2_34_04 + fP[GM14] * det2_34_02;
  const double det3_134_034 = fP[GM10] * det2_34_34 - fP[GM13] * det2_34_04 + fP[GM14] * det2_34_03;
  const double det3_134_123 = fP[GM11] * det2_34_23 - fP[GM12] * det2_34_13 + fP[GM13] * det2_34_12;
  const double det3_134_124 = fP[GM11] * det2_34_24 - fP[GM12] * det2_34_14 + fP[GM14] * det2_34_12;
  const double det3_134_134 = fP[GM11] * det2_34_34 - fP[GM13] * det2_34_14 + fP[GM14] * det2_34_13;
  const double det3_134_234 = fP[GM12] * det2_34_34 - fP[GM13] * det2_34_24 + fP[GM14] * det2_34_23;
  const double det3_234_012 = fP[GM20] * det2_34_12 - fP[GM21] * det2_34_02 + fP[GM22] * det2_34_01;
  const double det3_234_013 = fP[GM20] * det2_34_13 - fP[GM21] * det2_34_03 + fP[GM23] * det2_34_01;
  const double det3_234_014 = fP[GM20] * det2_34_14 - fP[GM21] * det2_34_04 + fP[GM24] * det2_34_01;
  const double det3_234_023 = fP[GM20] * det2_34_23 - fP[GM22] * det2_34_03 + fP[GM23] * det2_34_02;
  const double det3_234_024 = fP[GM20] * det2_34_24 - fP[GM22] * det2_34_04 + fP[GM24] * det2_34_02;
  const double det3_234_034 = fP[GM20] * det2_34_34 - fP[GM23] * det2_34_04 + fP[GM24] * det2_34_03;
  const double det3_234_123 = fP[GM21] * det2_34_23 - fP[GM22] * det2_34_13 + fP[GM23] * det2_34_12;
  const double det3_234_124 = fP[GM21] * det2_34_24 - fP[GM22] * det2_34_14 + fP[GM24] * det2_34_12;
  const double det3_234_134 = fP[GM21] * det2_34_34 - fP[GM23] * det2_34_14 + fP[GM24] * det2_34_13;
  const double det3_234_234 = fP[GM22] * det2_34_34 - fP[GM23] * det2_34_24 + fP[GM24] * det2_34_23;
 
  // Find all NECESSARY 4x4 dets:   (25 of them)
  const double det4_0123_0123 =
    fP[GM00] * det3_123_123 - fP[GM01] * det3_123_023 + fP[GM02] * det3_123_013 - fP[GM03] * det3_123_012;
  const double det4_0123_0124 =
    fP[GM00] * det3_123_124 - fP[GM01] * det3_123_024 + fP[GM02] * det3_123_014 - fP[GM04] * det3_123_012;
  const double det4_0123_0134 =
    fP[GM00] * det3_123_134 - fP[GM01] * det3_123_034 + fP[GM03] * det3_123_014 - fP[GM04] * det3_123_013;
  const double det4_0123_0234 =
    fP[GM00] * det3_123_234 - fP[GM02] * det3_123_034 + fP[GM03] * det3_123_024 - fP[GM04] * det3_123_023;
  const double det4_0123_1234 =
    fP[GM01] * det3_123_234 - fP[GM02] * det3_123_134 + fP[GM03] * det3_123_124 - fP[GM04] * det3_123_123;
  const double det4_0124_0123 =
    fP[GM00] * det3_124_123 - fP[GM01] * det3_124_023 + fP[GM02] * det3_124_013 - fP[GM03] * det3_124_012;
  const double det4_0124_0124 =
    fP[GM00] * det3_124_124 - fP[GM01] * det3_124_024 + fP[GM02] * det3_124_014 - fP[GM04] * det3_124_012;
  const double det4_0124_0134 =
    fP[GM00] * det3_124_134 - fP[GM01] * det3_124_034 + fP[GM03] * det3_124_014 - fP[GM04] * det3_124_013;
  const double det4_0124_0234 =
    fP[GM00] * det3_124_234 - fP[GM02] * det3_124_034 + fP[GM03] * det3_124_024 - fP[GM04] * det3_124_023;
  const double det4_0124_1234 =
    fP[GM01] * det3_124_234 - fP[GM02] * det3_124_134 + fP[GM03] * det3_124_124 - fP[GM04] * det3_124_123;
  const double det4_0134_0123 =
    fP[GM00] * det3_134_123 - fP[GM01] * det3_134_023 + fP[GM02] * det3_134_013 - fP[GM03] * det3_134_012;
  const double det4_0134_0124 =
    fP[GM00] * det3_134_124 - fP[GM01] * det3_134_024 + fP[GM02] * det3_134_014 - fP[GM04] * det3_134_012;
  const double det4_0134_0134 =
    fP[GM00] * det3_134_134 - fP[GM01] * det3_134_034 + fP[GM03] * det3_134_014 - fP[GM04] * det3_134_013;
  const double det4_0134_0234 =
    fP[GM00] * det3_134_234 - fP[GM02] * det3_134_034 + fP[GM03] * det3_134_024 - fP[GM04] * det3_134_023;
  const double det4_0134_1234 =
    fP[GM01] * det3_134_234 - fP[GM02] * det3_134_134 + fP[GM03] * det3_134_124 - fP[GM04] * det3_134_123;
  const double det4_0234_0123 =
    fP[GM00] * det3_234_123 - fP[GM01] * det3_234_023 + fP[GM02] * det3_234_013 - fP[GM03] * det3_234_012;
  const double det4_0234_0124 =
    fP[GM00] * det3_234_124 - fP[GM01] * det3_234_024 + fP[GM02] * det3_234_014 - fP[GM04] * det3_234_012;
  const double det4_0234_0134 =
    fP[GM00] * det3_234_134 - fP[GM01] * det3_234_034 + fP[GM03] * det3_234_014 - fP[GM04] * det3_234_013;
  const double det4_0234_0234 =
    fP[GM00] * det3_234_234 - fP[GM02] * det3_234_034 + fP[GM03] * det3_234_024 - fP[GM04] * det3_234_023;
  const double det4_0234_1234 =
    fP[GM01] * det3_234_234 - fP[GM02] * det3_234_134 + fP[GM03] * det3_234_124 - fP[GM04] * det3_234_123;
  const double det4_1234_0123 =
    fP[GM10] * det3_234_123 - fP[GM11] * det3_234_023 + fP[GM12] * det3_234_013 - fP[GM13] * det3_234_012;
  const double det4_1234_0124 =
    fP[GM10] * det3_234_124 - fP[GM11] * det3_234_024 + fP[GM12] * det3_234_014 - fP[GM14] * det3_234_012;
  const double det4_1234_0134 =
    fP[GM10] * det3_234_134 - fP[GM11] * det3_234_034 + fP[GM13] * det3_234_014 - fP[GM14] * det3_234_013;
  const double det4_1234_0234 =
    fP[GM10] * det3_234_234 - fP[GM12] * det3_234_034 + fP[GM13] * det3_234_024 - fP[GM14] * det3_234_023;
  const double det4_1234_1234 =
    fP[GM11] * det3_234_234 - fP[GM12] * det3_234_134 + fP[GM13] * det3_234_124 - fP[GM14] * det3_234_123;
 
  // Find the 5x5 det:
  const double det = fP[GM00] * det4_1234_1234 - fP[GM01] * det4_1234_0234 + fP[GM02] * det4_1234_0134
                     - fP[GM03] * det4_1234_0124 + fP[GM04] * det4_1234_0123;
  //    if (determ)
  //            *determ = det;
  //
  //    if (det == 0) {
  //            Error("Inv5x5", "matrix is singular");
  //            return kFALSE;
  //    }
  const double oneOverDet = 1.0 / det;
  const double mn1OverDet = -oneOverDet;
 
  fP[GM00] = det4_1234_1234 * oneOverDet;
  fP[GM01] = det4_0234_1234 * mn1OverDet;
  fP[GM02] = det4_0134_1234 * oneOverDet;
  fP[GM03] = det4_0124_1234 * mn1OverDet;
  fP[GM04] = det4_0123_1234 * oneOverDet;
 
  fP[GM10] = det4_1234_0234 * mn1OverDet;
  fP[GM11] = det4_0234_0234 * oneOverDet;
  fP[GM12] = det4_0134_0234 * mn1OverDet;
  fP[GM13] = det4_0124_0234 * oneOverDet;
  fP[GM14] = det4_0123_0234 * mn1OverDet;
 
  fP[GM20] = det4_1234_0134 * oneOverDet;
  fP[GM21] = det4_0234_0134 * mn1OverDet;
  fP[GM22] = det4_0134_0134 * oneOverDet;
  fP[GM23] = det4_0124_0134 * mn1OverDet;
  fP[GM24] = det4_0123_0134 * oneOverDet;
 
  fP[GM30] = det4_1234_0124 * mn1OverDet;
  fP[GM31] = det4_0234_0124 * oneOverDet;
  fP[GM32] = det4_0134_0124 * mn1OverDet;
  fP[GM33] = det4_0124_0124 * oneOverDet;
  fP[GM34] = det4_0123_0124 * mn1OverDet;
 
  fP[GM40] = det4_1234_0123 * oneOverDet;
  fP[GM41] = det4_0234_0123 * mn1OverDet;
  fP[GM42] = det4_0134_0123 * oneOverDet;
  fP[GM43] = det4_0124_0123 * mn1OverDet;
  fP[GM44] = det4_0123_0123 * oneOverDet;
}
 
void CbmRichRingFitterEllipseTau::AMultB(const double* const ap, int na, int ncolsa, const double* const bp, int nb,
                                         int ncolsb, double* cp)
{
  // Elementary routine to calculate matrix multiplication A*B
 
  const double* arp0 = ap;  // Pointer to  A[i,0];
  while (arp0 < ap + na) {
    for (const double* bcp = bp; bcp < bp + ncolsb;) {  // Pointer to the j-th column of B, Start bcp = B[0,0]
      const double* arp = arp0;                         // Pointer to the i-th row of A, reset to A[i,0]
      double cij        = 0;
      while (bcp < bp + nb) {  // Scan the i-th row of A and
        cij += *arp++ * *bcp;  // the j-th col of B
        bcp += ncolsb;
      }
      *cp++ = cij;
      bcp -= nb - 1;  // Set bcp to the (j+1)-th col
    }
    arp0 += ncolsa;  // Set ap to the (i+1)-th row
  }
}
 
#define ROTATE(a, i, j, k, l)                                                                                          \
  g       = a[i][j];                                                                                                   \
  h       = a[k][l];                                                                                                   \
  a[i][j] = g - s * (h + g * tau);                                                                                     \
  a[k][l] = h + s * (g - h * tau)
#define MAXSWEEP 50
void CbmRichRingFitterEllipseTau::Jacobi(double a[5][5], double d[5], double v[5][5])
{
  double tresh, theta, tau, t, sm, s, h, g, c;
 
  double b[5], z[5];
  unsigned int ip, iq, i, j;
  for (ip = 0; ip < 5; ip++) {
    for (iq = 0; iq < 5; iq++)
      v[ip][iq] = 0.;
    v[ip][ip] = 1.;
  }
 
  for (ip = 0; ip < 5; ip++) {
    b[ip] = a[ip][ip];
    z[ip] = 0.;
  }
 
  int nrot = 0;
 
  for (i = 1; i <= MAXSWEEP; i++) {
 
    for (sm = 0., ip = 0; ip < 5; ip++)
      for (iq = ip + 1; iq < 5; iq++)
        sm += fabs(a[ip][iq]);
    if (sm == 0.) {
      return;
    }
 
    tresh = (i < 4 ? 0.2 * sm / (5 * 5) : 0.);
 
    for (ip = 0; ip < 4; ip++)
      for (iq = ip + 1; iq < 5; iq++) {
 
        g = 100. * fabs(a[ip][iq]);
        if (i > 4 && (float) fabs(d[ip] + g) == (float) fabs(d[ip]) && (float) fabs(d[iq] + g) == (float) fabs(d[iq]))
          a[ip][iq] = 0.;
 
        else if (fabs(a[ip][iq]) > tresh) {
          h = d[ip] - d[iq];
 
          if ((float) (fabs(h) + g) == (float) fabs(h))
            t = a[ip][iq] / h;
          else {
            theta = 0.5 * h / a[ip][iq];
            t     = 1. / (fabs(theta) + sqrt(1. + theta * theta));
            if (theta < 0.) t = -t;
          }
          c   = 1. / sqrt(1 + t * t);
          s   = t * c;
          tau = s / (1. + c);
          h   = t * a[ip][iq];
          z[ip] -= h;
          z[iq] += h;
          d[ip] -= h;
          d[iq] += h;
          a[ip][iq] = 0.;
          for (j = 0; j < ip; j++) {
            ROTATE(a, j, ip, j, iq);
          }
          for (j = ip + 1; j < iq; j++) {
            ROTATE(a, ip, j, iq, j);
          }
          for (j = iq + 1; j < 5; j++) {
            ROTATE(a, ip, j, j, iq);
          }
          for (j = 0; j < 5; j++) {
            ROTATE(v, j, ip, j, iq);
          }
          ++nrot;
        }
      }
    for (ip = 0; ip < 5; ip++) {
      b[ip] += z[ip];
      d[ip] = b[ip];
      z[ip] = 0.;
    }
  }  //i rot
}
 
void CbmRichRingFitterEllipseTau::Eigsrt(double d[5], double v[5][5])
{
  double p;
  int i, k, j;
  for (i = 0; i < 5; i++) {
    p = d[k = i];
    for (j = i + 1; j < 5; j++)
      if (d[j] >= p) p = d[k = j];
    if (k != i) {
      d[k] = d[i];
      d[i] = p;
      for (j = 0; j < 5; j++) {
        p       = v[j][i];
        v[j][i] = v[j][k];
        v[j][k] = p;
      }
    }
  }
}