beamtime/cern-oct12/go4/RICH/CbmRichRingFitterEllipseTau.cxx (r4864/r3568)

Go to the documentation of this file.

// 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::endl;
using std::cout;

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;
        }

        //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 (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;
                        }
                }
        }
}