Fork me on GitHub

Changeset 64294db in git for external


Ignore:
Timestamp:
Apr 17, 2021, 10:26:04 AM (4 years ago)
Author:
GitHub <noreply@…>
Branches:
master
Children:
3b3071a
Parents:
3cfe61d (diff), 4df491e (diff)
Note: this is a merge changeset, the changes displayed below correspond to the merge itself.
Use the (diff) links above to see all the changes relative to each parent.
git-author:
Michele Selvaggi <michele.selvaggi@…> (04/17/21 10:26:04)
git-committer:
GitHub <noreply@…> (04/17/21 10:26:04)
Message:

Merge pull request #94 from fbedesch/master

VertexFit.cc and TrkUtil.cc improvements

Location:
external/TrackCovariance
Files:
4 edited

Legend:

Unmodified
Added
Removed

  • external/TrackCovariance/TrkUtil.cc

    r3cfe61d r64294db  
    11#include "TrkUtil.h"
    2 #include <TMath.h>
    32#include <iostream>
     3#include <algorithm>
    44
    55// Constructor
    … …  
    77{
    88        fBz = Bz;
     9        fGasSel = 0;                            // Default is He-Isobuthane (90-10)
     10        fRmin = 0.0;                            // Lower                DCH radius
     11        fRmax = 0.0;                            // Higher       DCH radius
     12        fZmin = 0.0;                            // Lower                DCH z
     13        fZmax = 0.0;                            // Higher       DCH z
    914}
    1015TrkUtil::TrkUtil()
    1116{
    1217        fBz = 0.0;
     18        fGasSel = 0;                            // Default is He-Isobuthane (90-10)
     19        fRmin = 0.0;                            // Lower                DCH radius
     20        fRmax = 0.0;                            // Higher       DCH radius
     21        fZmin = 0.0;                            // Lower                DCH z
     22        fZmax = 0.0;                            // Higher       DCH z
    1323}
    1424//
    … …  
    1727{
    1828        fBz = 0.0;
     29        fGasSel = 0;                            // Default is He-Isobuthane (90-10)
     30        fRmin = 0.0;                            // Lower                DCH radius
     31        fRmax = 0.0;                            // Higher       DCH radius
     32        fZmin = 0.0;                            // Lower                DCH z
     33        fZmax = 0.0;                            // Higher       DCH z
    1934}
    2035//
    … …  
    2944        Double_t pt = p.Pt();
    3045        Double_t C = a / (2 * pt);                      // Half curvature
    31         //cout << "ObsTrk::XPtoPar: fB = " << fB << ", a = " << a << ", pt = " << pt << ", C = " << C << endl;
     46        //std::cout << "ObsTrk::XPtoPar: fB = " << fB << ", a = " << a << ", pt = " << pt << ", C = " << C << std::endl;
    3247        Double_t r2 = x.Perp2();
    3348        Double_t cross = x(0) * p(1) - x(1) * p(0);
    34         Double_t T = TMath::Sqrt(pt * pt - 2 * a * cross + a * a * r2);
    35         Double_t phi0 = TMath::ATan2((p(1) - a * x(0)) / T, (p(0) + a * x(1)) / T);     // Phi0
     49        Double_t T = sqrt(pt * pt - 2 * a * cross + a * a * r2);
     50        Double_t phi0 = atan2((p(1) - a * x(0)) / T, (p(0) + a * x(1)) / T);    // Phi0
    3651        Double_t D;                                                     // Impact parameter D
    3752        if (pt < 10.0) D = (T - pt) / a;
    … …  
    4257        Par(2) = C;             // Store C
    4358        //Longitudinal parameters
    44         Double_t B = C * TMath::Sqrt(TMath::Max(r2 - D * D, 0.0) / (1 + 2 * C * D));
    45         Double_t st = TMath::ASin(B) / C;
     59        Double_t B = C * sqrt(TMath::Max(r2 - D * D, 0.0) / (1 + 2 * C * D));
     60        Double_t st = asin(B) / C;
    4661        Double_t ct = p(2) / pt;
    4762        Double_t z0 = x(2) - ct * st;
    … …  
    7085        //
    7186        TVector3 Xval;
    72         Xval(0) = -D * TMath::Sin(phi0);
    73         Xval(1) = D * TMath::Cos(phi0);
     87        Xval(0) = -D * sin(phi0);
     88        Xval(1) = D * cos(phi0);
    7489        Xval(2) = z0;
    7590        //
    … …  
    7994TVector3 TrkUtil::ParToP(TVectorD Par)
    8095{
    81         if (fBz == 0.0)
    82 std::cout << "TrkUtil::ParToP: Warning Bz not set" << std::endl;
     96        if (fBz == 0.0)std::cout << "TrkUtil::ParToP: Warning Bz not set" << std::endl;
    8397        //
    8498        return ParToP(Par,fBz);
    … …  
    93107        TVector3 Pval;
    94108        Double_t pt = Bz * cSpeed() / TMath::Abs(2 * C);
    95         Pval(0) = pt * TMath::Cos(phi0);
    96         Pval(1) = pt * TMath::Sin(phi0);
     109        Pval(0) = pt * cos(phi0);
     110        Pval(1) = pt * sin(phi0);
    97111        Pval(2) = pt * ct;
    98112        //
    … …  
    113127        Double_t b = -cSpeed() * fBz / 2.;
    114128        pACTS(0) = 1000 * Par(0);               // D from m to mm
    115         pACTS(1) = 1000 * Par(3);               // z0 from m to mm
     129        pACTS(1) = 1000 * Par(3);       // z0 from m to mm
    116130        pACTS(2) = Par(1);                      // Phi0 is unchanged
    117         pACTS(3) = TMath::ATan2(1.0, Par(4));           // Theta in [0, pi] range
    118         pACTS(4) = Par(2) / (b * TMath::Sqrt(1 + Par(4) * Par(4)));             // q/p in GeV
     131        pACTS(3) = atan2(1.0, Par(4));          // Theta in [0, pi] range
     132        pACTS(4) = Par(2) / (b * sqrt(1 + Par(4) * Par(4)));            // q/p in GeV
    119133        pACTS(5) = 0.0;                         // Time: currently undefined
    120134        //
    … …  
    133147        A(0, 0) = 1000.;                // D-D  conversion to mm
    134148        A(1, 2) = 1.0;          // phi0-phi0
    135         A(2, 4) = 1.0 / (TMath::Sqrt(1.0 + ct * ct) * b);       // q/p-C
     149        A(2, 4) = 1.0 / (sqrt(1.0 + ct * ct) * b);      // q/p-C
    136150        A(3, 1) = 1000.;                // z0-z0 conversion to mm
    137151        A(4, 3) = -1.0 / (1.0 + ct * ct); // theta - cot(theta)
    138         A(4, 4) = -C * ct / (b * TMath::Power(1.0 + ct * ct, 3.0 / 2.0)); // q/p-cot(theta)
     152        A(4, 4) = -C * ct / (b * pow(1.0 + ct * ct, 3.0 / 2.0)); // q/p-cot(theta)
    139153        //
    140154        TMatrixDSym Cv = Cov;
    … …  
    218232        return Cmm;
    219233}
     234//
     235// Setup chamber volume
     236void TrkUtil::SetDchBoundaries(Double_t Rmin, Double_t Rmax, Double_t Zmin, Double_t Zmax)
     237{
     238        fRmin = Rmin;                           // Lower                DCH radius
     239        fRmax = Rmax;                           // Higher       DCH radius
     240        fZmin = Zmin;                           // Lower                DCH z
     241        fZmax = Zmax;                           // Higher       DCH z
     242}
     243//
     244// Get Trakck length inside DCH volume
     245Double_t TrkUtil::TrkLen(TVectorD Par)
     246{
     247        Double_t tLength = 0.0;
     248        // Check if geometry is initialized
     249        if (fZmin == 0.0 && fZmax == 0.0)
     250        {
     251                // No geometry set so send a warning and return 0
     252                std::cout << "TrkUtil::TrkLen() called without a DCH volume defined" << std::endl;
     253        }
     254        else
     255        {
     256                //******************************************************************
     257                // Determine the track length inside the chamber   ****
     258                //******************************************************************
     259                //
     260                // Track pararameters
     261                Double_t D = Par(0);            // Transverse impact parameter
     262                Double_t phi0 = Par(1);         // Transverse direction at minimum approach
     263                Double_t C = Par(2);            // Half curvature
     264                Double_t z0 = Par(3);           // Z at minimum approach
     265                Double_t ct = Par(4);           // cot(theta)
     266                //std::cout << "TrkUtil:: parameters: D= " << D << ", phi0= " << phi0
     267                //      << ", C= " << C << ", z0= " << z0 << ", ct= " << ct << std::endl;
     268                //
     269                // Track length per unit phase change
     270                Double_t Scale = sqrt(1.0 + ct*ct) / (2.0*TMath::Abs(C));
     271                //
     272                // Find intersections with chamber boundaries
     273                //
     274                Double_t phRin = 0.0;                   // phase of inner cylinder
     275                Double_t phRin2= 0.0;                   // phase of inner cylinder intersection (2nd branch)
     276                Double_t phRhi = 0.0;                   // phase of outer cylinder intersection
     277                Double_t phZmn = 0.0;                   // phase of left wall intersection
     278                Double_t phZmx = 0.0;                   // phase of right wall intersection
     279                //  ... with inner cylinder
     280                Double_t Rtop = TMath::Abs((1.0 + C*D) / C);
     281
     282                if (Rtop > fRmin && TMath::Abs(D) < fRmin) // *** don't treat large D tracks for the moment ***
     283                {
     284                        Double_t ph = 2 * asin(C*sqrt((fRmin*fRmin - D*D) / (1.0 + 2.0*C*D)));
     285                        Double_t z = z0 + ct*ph / (2.0*C);
     286
     287                        //std::cout << "Rin intersection: ph = " << ph<<", z= "<<z << std::endl;
     288
     289                        if (z < fZmax && z > fZmin)     phRin = TMath::Abs(ph); // Intersection inside chamber volume   
     290                        //
     291                        // Include second branch of loopers
     292                        Double_t Pi = 3.14159265358979323846;
     293                        Double_t ph2 = 2*Pi - TMath::Abs(ph);
     294                        if (ph < 0)ph2 = -ph2;
     295                        z = z0 + ct * ph2 / (2.0 * C);
     296                        if (z < fZmax && z > fZmin)     phRin2 = TMath::Abs(ph2);       // Intersection inside chamber volume
     297                }
     298                //  ... with outer cylinder
     299                if (Rtop > fRmax && TMath::Abs(D) < fRmax) // *** don't treat large D tracks for the moment ***
     300                {
     301                        Double_t ph = 2 * asin(C*sqrt((fRmax*fRmax - D*D) / (1.0 + 2.0*C*D)));
     302                        Double_t z = z0 + ct*ph / (2.0*C);
     303                        if (z < fZmax && z > fZmin)     phRhi = TMath::Abs(ph); // Intersection inside chamber volume   
     304                }
     305                //  ... with left wall
     306                Double_t Zdir = (fZmin - z0) / ct;
     307                if (Zdir > 0.0)
     308                {
     309                        Double_t ph = 2.0*C*Zdir;
     310                        Double_t Rint = sqrt(D*D + (1.0 + 2.0*C*D)*pow(sin(ph / 2), 2) / (C*C));
     311                        if (Rint < fRmax && Rint > fRmin)       phZmn = TMath::Abs(ph); // Intersection inside chamber volume   
     312                }
     313                //  ... with right wall
     314                Zdir = (fZmax - z0) / ct;
     315                if (Zdir > 0.0)
     316                {
     317                        Double_t ph = 2.0*C*Zdir;
     318                        Double_t Rint = sqrt(D*D + (1.0 + 2.0*C*D)*pow(sin(ph / 2), 2) / (C*C));
     319                        if (Rint < fRmax && Rint > fRmin)       phZmx = TMath::Abs(ph); // Intersection inside chamber volume   
     320                }
     321                //
     322                // Order phases and keep the lowest two non-zero ones
     323                //
     324                const Int_t Nint = 5;
     325                Double_t dPhase = 0.0;  // Phase difference between two close intersections
     326                Double_t ph_arr[Nint] = { phRin, phRin2, phRhi, phZmn, phZmx };
     327                std::sort(ph_arr, ph_arr + Nint);
     328                Int_t iPos = -1;                // First element > 0
     329                for (Int_t i = 0; i < Nint; i++)
     330                {
     331                        if (ph_arr[i] <= 0.0) iPos = i;
     332                }
     333
     334                if (iPos < Nint - 2)
     335                {
     336                        dPhase = ph_arr[iPos + 2] - ph_arr[iPos + 1];
     337                        tLength = dPhase*Scale;
     338                }
     339        }
     340        return tLength;
     341}
     342//
     343// Return number of ionization clusters
     344Bool_t TrkUtil::IonClusters(Double_t &Ncl, Double_t mass, TVectorD Par)
     345{
     346        //
     347        // Units are meters/Tesla/GeV
     348        //
     349        Ncl = 0.0;
     350        Bool_t Signal = kFALSE;
     351        Double_t tLen = 0;
     352        // Check if geometry is initialized
     353        if (fZmin == 0.0 && fZmax == 0.0)
     354        {
     355                // No geometry set so send a warning and return 0
     356                std::cout << "TrkUtil::IonClusters() called without a volume defined" << std::endl;
     357        }
     358        else tLen = TrkLen(Par);
     359
     360        //******************************************************************
     361        // Now get the number of clusters                       ****
     362        //******************************************************************
     363        //
     364        Double_t muClu = 0.0;   // mean number of clusters
     365        Double_t bg = 0.0;              // beta*gamma
     366        Ncl = 0.0;
     367        if (tLen > 0.0)
     368        {
     369                Signal = kTRUE;
     370                //
     371                // Find beta*gamma
     372                if (fBz == 0.0)
     373                {
     374                        Signal = kFALSE;
     375                        std::cout << "TrkUtil::IonClusters: Please set Bz!!!" << std::endl;
     376                }
     377                else
     378                {
     379                        TVector3 p = ParToP(Par);
     380                        bg = p.Mag() / mass;
     381                        muClu = Nclusters(bg)*tLen;                             // Avg. number of clusters
     382
     383                        Ncl = gRandom->PoissonD(muClu);                 // Actual number of clusters
     384                }
     385
     386        }
     387//
     388        return Signal;
     389}
     390//
     391//
     392Double_t TrkUtil::Nclusters(Double_t begam)
     393{
     394        Int_t Opt = fGasSel;
     395        Double_t Nclu = Nclusters(begam, Opt);
     396        //
     397        return Nclu;
     398}
     399//
     400Double_t TrkUtil::Nclusters(Double_t begam, Int_t Opt) {
     401        //
     402        // Opt = 0: He 90 - Isobutane 10
     403        //     = 1: pure He
     404        //     = 2: Argon 50 - Ethane 50
     405        //     = 3: pure Argon
     406        //
     407        //
     408        /*
     409        std::vector<double> bg{ 0.5, 0.8, 1., 2., 3., 4., 5., 8., 10.,
     410        12., 15., 20., 50., 100., 200., 500., 1000. };
     411        // He 90 - Isobutane 10
     412        std::vector<double> ncl_He_Iso{ 42.94, 23.6,18.97,12.98,12.2,12.13,
     413        12.24,12.73,13.03,13.29,13.63,14.08,15.56,16.43,16.8,16.95,16.98 };
     414        //
     415        // pure He
     416        std::vector<double> ncl_He{ 11.79,6.5,5.23,3.59,3.38,3.37,3.4,3.54,3.63,
     417        3.7,3.8,3.92,4.33,4.61,4.78,4.87,4.89 };
     418        //
     419        // Argon 50 - Ethane 50
     420        std::vector<double> ncl_Ar_Eth{ 130.04,71.55,57.56,39.44,37.08,36.9,
     421        37.25,38.76,39.68,40.49,41.53,42.91,46.8,48.09,48.59,48.85,48.93 };
     422        //
     423        // pure Argon
     424        std::vector<double> ncl_Ar{ 88.69,48.93,39.41,27.09,25.51,25.43,25.69,
     425        26.78,27.44,28.02,28.77,29.78,32.67,33.75,34.24,34.57,34.68 };
     426        //
     427        Int_t nPoints = (Int_t)bg.size();
     428        bg.push_back(10000.);
     429        std::vector<double> ncl;
     430        switch (Opt)
     431        {
     432        case 0: ncl = ncl_He_Iso;                       // He-Isobutane
     433                break;
     434        case 1: ncl = ncl_He;                           // pure He
     435                break;
     436        case 2: ncl = ncl_Ar_Eth;                       // Argon - Ethane
     437                break;
     438        case 3: ncl = ncl_Ar;                           // pure Argon
     439                break;
     440        }
     441        ncl.push_back(ncl[nPoints - 1]);
     442        */
     443        const Int_t Npt = 18;
     444        Double_t bg[Npt] = { 0.5, 0.8, 1., 2., 3., 4., 5., 8., 10.,
     445        12., 15., 20., 50., 100., 200., 500., 1000., 10000. };
     446        //
     447        // He 90 - Isobutane 10
     448        Double_t ncl_He_Iso[Npt] = { 42.94, 23.6,18.97,12.98,12.2,12.13,
     449        12.24,12.73,13.03,13.29,13.63,14.08,15.56,16.43,16.8,16.95,16.98, 16.98 };
     450        //
     451        // pure He
     452        Double_t ncl_He[Npt] = { 11.79,6.5,5.23,3.59,3.38,3.37,3.4,3.54,3.63,
     453                                3.7,3.8,3.92,4.33,4.61,4.78,4.87,4.89, 4.89 };
     454        //
     455        // Argon 50 - Ethane 50
     456        Double_t ncl_Ar_Eth[Npt] = { 130.04,71.55,57.56,39.44,37.08,36.9,
     457        37.25,38.76,39.68,40.49,41.53,42.91,46.8,48.09,48.59,48.85,48.93,48.93 };
     458        //
     459        // pure Argon
     460        Double_t ncl_Ar[Npt] = { 88.69,48.93,39.41,27.09,25.51,25.43,25.69,
     461        26.78,27.44,28.02,28.77,29.78,32.67,33.75,34.24,34.57,34.68, 34.68 };
     462        //
     463        Double_t ncl[Npt];
     464        switch (Opt)
     465        {
     466                case 0: std::copy(ncl_He_Iso, ncl_He_Iso + Npt, ncl);   // He-Isobutane
     467                break;                                                 
     468                case 1: std::copy(ncl_He, ncl_He + Npt, ncl);           // pure He
     469                break;
     470                case 2: std::copy(ncl_Ar_Eth, ncl_Ar_Eth + Npt, ncl);   // Argon - Ethane
     471                break;
     472                case 3: std::copy(ncl_Ar, ncl_Ar + Npt, ncl);           // pure Argon
     473                break;
     474        }
     475        //
     476        Int_t ilow = 0;
     477        while (begam > bg[ilow])ilow++;
     478        ilow--;
     479        //std::cout << "ilow= " << ilow << ", low = " << bg[ilow] << ", val = " << begam
     480        //      << ", high = " << bg[ilow + 1] << std::endl;
     481        //
     482        Int_t ind[3] = { ilow, ilow + 1, ilow + 2 };
     483        TVectorD y(3);
     484        for (Int_t i = 0; i < 3; i++)y(i) = ncl[ind[i]];
     485        TVectorD x(3);
     486        for (Int_t i = 0; i < 3; i++)x(i) = bg[ind[i]];
     487        TMatrixD Xval(3, 3);
     488        for (Int_t i = 0; i < 3; i++)Xval(i, 0) = 1.0;
     489        for (Int_t i = 0; i < 3; i++)Xval(i, 1) = x(i);
     490        for (Int_t i = 0; i < 3; i++)Xval(i, 2) = x(i) * x(i);
     491        //std::cout << "Xval:" << std::endl; Xval.Print();
     492        Xval.Invert();
     493        TVectorD coeff = Xval * y;
     494        Double_t interp = coeff[0] + coeff[1] * begam + coeff[2] * begam * begam;
     495        //std::cout << "val1= (" <<x(0)<<", "<< y(0) << "), val2= ("
     496        //      <<x(1)<<", "<< y(1) << "), val3= ("
     497        //      <<x(2)<<", "<< y(2)
     498        //      << "), result= (" <<begam<<", "<< interp<<")" << std::endl;
     499        //
     500        //if (TMath::IsNaN(interp))std::cout << "NaN found: bg= " << begam << ", Opt= " << Opt << std::endl;
     501        if (begam < bg[0]) interp = 0.0;
     502        //std::cout << "bg= " << begam << ", Opt= " << Opt <<", interp = "<<interp<< std::endl;
     503        return 100*interp;
     504}
     505//
     506Double_t TrkUtil::funcNcl(Double_t *xp, Double_t *par){
     507        Double_t bg = xp[0];
     508        return Nclusters(bg);
     509}
     510//
     511void TrkUtil::SetGasMix(Int_t Opt)
     512{
     513        if (Opt < 0 || Opt > 3)
     514        {
     515                std::cout << "TrkUtil::SetGasMix Gas option not allowed. No action."
     516                        << std::endl;
     517        }
     518        else fGasSel = Opt;
     519}

  • external/TrackCovariance/TrkUtil.h

    r3cfe61d r64294db  
    66#include <TVectorD.h>
    77#include <TMatrixDSym.h>
     8#include <TRandom.h>
    89//
    910//
    … …  
    1516protected:
    1617        Double_t fBz;                                                   // Solenoid magnetic field
     18        //
     19        Int_t fGasSel;                                                  // Gas selection: 0: He-Iso, 1: He, 2:Ar-Eth, 3: Ar
     20        Double_t fRmin;                                                 // Lower                DCH radius
     21        Double_t fRmax;                                                 // Higher       DCH radius
     22        Double_t fZmin;                                                 // Lower                DCH z
     23        Double_t fZmax;                                                 // Higher       DCH z
    1724        //
    1825        // Service routines
    … …  
    3138        TVectorD ParToILC(TVectorD Par);                // Parameter conversion
    3239        TMatrixDSym CovToILC(TMatrixDSym Cov);  // Covariance conversion
     40        //
    3341
    3442public:
    … …  
    6068        static TVectorD ParToMm(TVectorD Par);                  // Parameter conversion
    6169        static TMatrixDSym CovToMm(TMatrixDSym Cov);    // Covariance conversion
    62 
     70        //
     71        // Cluster counting in gas
     72        //
     73        // Define gas volume (units = meters)
     74        void SetDchBoundaries(Double_t Rmin, Double_t Rmax, Double_t Zmin, Double_t Zmax);
     75        // Gas mixture selection
     76        void SetGasMix(Int_t Opt);
     77        // Get number of ionization clusters
     78        Bool_t IonClusters(Double_t &Ncl, Double_t mass, TVectorD Par);
     79        Double_t Nclusters(Double_t bgam);      // mean clusters/meter vs beta*gamma
     80        static Double_t Nclusters(Double_t bgam, Int_t Opt);    // mean clusters/meter vs beta*gamma
     81        Double_t funcNcl(Double_t *xp, Double_t *par);
     82        Double_t TrkLen(TVectorD Par);                                  // Track length inside chamber
    6383};
    6484

  • external/TrackCovariance/VertexFit.cc

    r3cfe61d r64294db  
    88// Constructors
    99//
    10 // Empty
     10//
     11// Empty construction (to be used when adding tracks later with AddTrk() )
    1112VertexFit::VertexFit()
    1213{
    1314        fNtr = 0;
     15        fRold = -1.0;
    1416        fVtxDone = kFALSE;
    1517        fVtxCst = kFALSE;
    … …  
    1921        fcovXv.ResizeTo(3, 3);
    2022}
    21 // Parameters and covariances
     23//
     24// Build from list of parameters and covariances
    2225VertexFit::VertexFit(Int_t Ntr, TVectorD** trkPar, TMatrixDSym** trkCov)
    2326{
    2427        fNtr = Ntr;
     28        fRold = -1.0;
    2529        fVtxDone = kFALSE;
    2630        fVtxCst = kFALSE;
    … …  
    3034        fcovXv.ResizeTo(3, 3);
    3135        //
    32         fPar = trkPar;
    33         fCov = trkCov;
    34         fChi2List.ResizeTo(Ntr);
    35         //
    36         ffi = new Double_t[Ntr];                                // Fit phases
    37         fx0i = new TVectorD * [Ntr];                    // Track expansion points
    38         for (Int_t i = 0; i < Ntr; i++) fx0i[i] = new TVectorD(3);
    39         fai = new TVectorD * [Ntr];                     // dx/dphi
    40         for (Int_t i = 0; i < Ntr; i++) fai[i] = new TVectorD(3);
    41         fa2i = new Double_t[Ntr];                               // a'Wa
    42         fDi = new TMatrixDSym * [Ntr];          // W-WBW
    43         for (Int_t i = 0; i < Ntr; i++) fDi[i] = new TMatrixDSym(3);
    44         fWi = new TMatrixDSym * [Ntr];  // (ACA')^-1
    45         for (Int_t i = 0; i < Ntr; i++) fWi[i] = new TMatrixDSym(3);
    46         fWinvi = new TMatrixDSym * [Ntr];       // ACA'
    47         for (Int_t i = 0; i < Ntr; i++) fWinvi[i] = new TMatrixDSym(3);
    48 }
    49 // ObsTrk list
     36        for (Int_t i = 0; i < fNtr; i++)
     37        {
     38                TVectorD pr = *trkPar[i];
     39                fPar.push_back(new TVectorD(pr));
     40                TMatrixDSym cv = *trkCov[i];
     41                fCov.push_back(new TMatrixDSym(cv));
     42        }
     43        fChi2List.ResizeTo(fNtr);
     44        //
     45}
     46//
     47// Build from ObsTrk list of tracks
    5048VertexFit::VertexFit(Int_t Ntr, ObsTrk** track)
    5149{
    5250        fNtr = Ntr;
     51        fRold = -1.0;
    5352        fVtxDone = kFALSE;
    5453        fVtxCst = kFALSE;
    … …  
    5857        fcovXv.ResizeTo(3, 3);
    5958        //
    60         fPar = new TVectorD * [Ntr];
    61         fCov = new TMatrixDSym * [Ntr];
    62         fChi2List.ResizeTo(Ntr);
    63         for (Int_t i = 0; i < Ntr; i++)
    64         {
    65                 fPar[i] = new TVectorD(track[i]->GetObsPar());
    66                 fCov[i] = new TMatrixDSym(track[i]->GetCov());
    67         }
    68         //
    69         ffi = new Double_t[Ntr];                                // Fit phases
    70         fx0i = new TVectorD * [Ntr];                    // Track expansion points
    71         for (Int_t i = 0; i < Ntr; i++) fx0i[i] = new TVectorD(3);
    72         fai = new TVectorD * [Ntr];                     // dx/dphi
    73         for (Int_t i = 0; i < Ntr; i++) fai[i] = new TVectorD(3);
    74         fa2i = new Double_t[Ntr];                               // a'Wa
    75         fDi = new TMatrixDSym * [Ntr];  // W-WBW
    76         for (Int_t i = 0; i < Ntr; i++) fDi[i] = new TMatrixDSym(3);
    77         fWi = new TMatrixDSym * [Ntr];  // (ACA')^-1
    78         for (Int_t i = 0; i < Ntr; i++) fWi[i] = new TMatrixDSym(3);
    79         fWinvi = new TMatrixDSym * [Ntr];       // ACA'
    80         for (Int_t i = 0; i < Ntr; i++) fWinvi[i] = new TMatrixDSym(3);
     59        fChi2List.ResizeTo(fNtr);
     60        for (Int_t i = 0; i < fNtr; i++)
     61        {
     62                fPar.push_back(new TVectorD(track[i]->GetObsPar()));
     63                fCov.push_back(new TMatrixDSym(track[i]->GetCov()));
     64        }
    8165}
    8266//
    8367// Destructor
     68//
     69void VertexFit::ResetWrkArrays()
     70{
     71        Int_t N = (Int_t)ffi.size();
     72        for (Int_t i = 0; i < N; i++)
     73        {
     74                if (fx0i[i])  { fx0i[i]->Clear();               delete fx0i[i]; }
     75                if (fai[i])   { fai[i]->Clear();                        delete fai[i]; }
     76                if (fDi[i])   { fDi[i]->Clear();                        delete fDi[i]; }
     77                if (fWi[i])   { fWi[i]->Clear();                        delete fWi[i]; }
     78                if (fWinvi[i]){ fWinvi[i]->Clear();     delete fWinvi[i]; }
     79        }
     80        fa2i.clear();
     81        fx0i.clear();
     82        fai.clear();
     83        fDi.clear();
     84        fWi.clear();
     85        fWinvi.clear();
     86}
    8487VertexFit::~VertexFit()
    8588{
    86         fxCst.Clear();
    87         fCovCst.Clear();
    88         fXv.Clear();
    89         fcovXv.Clear();
    90         fChi2List.Clear();
     89        fxCst.Clear();         
     90        fCovCst.Clear();               
     91        fXv.Clear();           
     92        fcovXv.Clear();         
     93        fChi2List.Clear();     
    9194        //
    9295        for (Int_t i = 0; i < fNtr; i++)
    9396        {
    94                 fPar[i]->Clear();
    95                 fCov[i]->Clear();
    96                 //
    97                 fx0i[i]->Clear(); delete fx0i[i];
    98                 fai[i]->Clear(); delete fai[i];
    99                 fDi[i]->Clear(); delete fDi[i];
    100                 fWi[i]->Clear(); delete fWi[i];
    101                 fWinvi[i]->Clear();     delete fWinvi[i];
    102         }
     97                fPar[i]->Clear();       delete fPar[i];
     98                fCov[i]->Clear();       delete fCov[i];
     99        }
     100        fPar.clear();
     101        fCov.clear();   
     102        //
     103        ResetWrkArrays();
     104        ffi.clear();
    103105        fNtr = 0;
    104         delete[] fPar;
    105         delete[] fCov;
    106         delete[] ffi;
    107         delete[] fa2i;
    108         delete[] fx0i;
    109         delete[] fai;
    110         delete[] fDi;
    111         delete[] fWi;
    112         delete[] fWinvi;
    113 }
    114 //
    115 Double_t VertexFit::FastRv1(TVectorD p1, TVectorD p2)
    116 {
    117         //
    118         // Find radius of intersection between two tracks in the transverse plane
     106}
     107//
     108Double_t VertexFit::FastRv(TVectorD p1, TVectorD p2)
     109{
     110        //
     111        // Find radius of minimum distance between two tracks
    119112        //
    120113        // p = (D,phi, C, z0, ct)
    … …  
    122115        // Define arrays
    123116        //
    124         Double_t r1 = 1.0 / p1(2);
    125         Double_t r2 = 1.0 / p2(2);
     117        Double_t C1 = p1(2);
     118        Double_t C2 = p2(2);
     119        Double_t ph1 = p1(1);
     120        Double_t ph2 = p2(1);
    126121        TVectorD x0 = Fill_x0(p1);
    127122        TVectorD y0 = Fill_x0(p2);
    128123        TVectorD n = Fill_a(p1, 0.0);
    129         n *= r1;
     124        n *= (2*C1);
    130125        TVectorD k = Fill_a(p2, 0.0);
    131         k *= r2;
     126        k *= (2*C2);
    132127        //
    133128        // Setup and solve linear system
    … …  
    150145        TVectorD X = x0 + smin(0) * n;
    151146        TVectorD Y = y0 + smin(1) * k;
    152         Double_t R1 = TMath::Sqrt(X(0) * X(0) + X(1) * X(1));
    153         Double_t R2 = TMath::Sqrt(Y(0) * Y(0) + Y(1) * Y(1));
    154         //
    155         return 0.5 * (R1 + R2);
    156 }
    157 Double_t VertexFit::FastRv(TVectorD p1, TVectorD p2)
    158 {
    159         //
    160         // Find radius of minimum distance
    161         //
    162         // p = (D,phi, C)
    163         //
    164         // Solving matrix
    165         TMatrixDSym H(2);
    166         H(0, 0) = -TMath::Cos(p2(1));
    167         H(0, 1) = TMath::Cos(p1(1));
    168         H(1, 0) = -TMath::Sin(p2(1));
    169         H(1, 1) = TMath::Sin(p1(1));
    170         Double_t Det = TMath::Sin(p2(1) - p1(1));
    171         H *= 1.0 / Det;
    172         //
    173         // Convergence parameters
    174         Int_t Ntry = 0;
    175         Int_t NtryMax = 100;
    176         Double_t eps = 1000.;
    177         Double_t epsMin = 1.0e-6;
    178         //
    179         // Vertex finding loop
    180         //
    181         TVectorD cterm(2);
    182         cterm(0) = p1(0);
    183         cterm(1) = p2(0);
    184         TVectorD xv(2);
    185         Double_t R = 1000.;
    186         while (eps > epsMin)
    187         {
    188                 xv = H * cterm;
    189                 Ntry++;
    190                 if (Ntry > NtryMax)
     147        //
     148        // Higher order corrections
     149        X(0) += -C1 * smin(0) * smin(0) * TMath::Sin(ph1);
     150        X(1) +=  C1 * smin(0) * smin(0) * TMath::Cos(ph1);
     151        Y(0) += -C2 * smin(1) * smin(1) * TMath::Sin(ph2);
     152        Y(1) +=  C2 * smin(1) * smin(1) * TMath::Cos(ph2);
     153        //
     154        TVectorD Xavg = 0.5 * (X + Y);
     155        //
     156        //
     157        return TMath::Sqrt(Xavg(0)*Xavg(0)+Xavg(1)*Xavg(1));
     158}
     159//
     160// Starting radius determination
     161Double_t VertexFit::StartRadius()
     162{
     163        //
     164        // Maximum impact parameter
     165        Double_t Rd = 0;
     166        for (Int_t i = 0; i < fNtr; i++)
     167        {
     168                TVectorD par = *fPar[i];
     169                Double_t Dabs = TMath::Abs(par(0));
     170                if (Dabs > Rd)Rd = Dabs;
     171        }
     172        //-----------------------------
     173        //
     174        // Find track pair with phi difference closest to pi/2
     175        Int_t isel = 0; Int_t jsel = 0;         // selected track indices
     176        Double_t dSinMax = 0.0;                         // Max phi difference
     177        for (Int_t i = 0; i < fNtr - 1; i++)
     178        {
     179                TVectorD pari = *fPar[i];
     180                Double_t phi1 = pari(1);
     181
     182                for (Int_t j = i + 1; j < fNtr; j++)
    191183                {
    192                         std::cout << "FastRv: maximum number of iteration reached" << std::endl;
    193                         break;
     184                        TVectorD parj = *fPar[j];
     185                        Double_t phi2 = parj(1);
     186                        Double_t Sindphi = TMath::Abs(TMath::Sin(phi2 - phi1));
     187                        if (Sindphi > dSinMax)
     188                        {
     189                                isel = i; jsel = j;
     190                                dSinMax = Sindphi;
     191                        }
    194192                }
    195                 Double_t Rnew = TMath::Sqrt(xv(0) * xv(0) + xv(1) * xv(1));
    196                 eps = Rnew - R;
    197                 R = Rnew;
    198                 cterm(0) = p1(2) * R * R;
    199                 cterm(1) = p2(2) * R * R;
    200         }
     193        }
     194        //
     195        //------------------------------------------
     196        //
     197        // Find radius of minimum distrance between tracks
     198        TVectorD p1 = *fPar[isel];
     199        TVectorD p2 = *fPar[jsel];
     200        Double_t R = FastRv(p1, p2);
     201        //
     202        R = 0.9 * R + 0.1 * Rd;         // Protect for overshoot
    201203        //
    202204        return R;
    203205}
    204 
    205 TMatrixDSym VertexFit::RegInv3(TMatrixDSym& Smat0)
    206 {
    207         //
    208         // Regularized inversion of symmetric 3x3 matrix with positive diagonal elements
    209         //
    210         TMatrixDSym Smat = Smat0;
    211         Int_t N = Smat.GetNrows();
    212         if (N != 3)
    213         {
    214                 std::cout << "RegInv3 called with  matrix size != 3. Abort & return standard inversion." << std::endl;
    215                 return Smat.Invert();
    216         }
    217         TMatrixDSym D(N); D.Zero();
    218         Bool_t dZero = kTRUE;   // No elements less or equal 0 on the diagonal
    219         for (Int_t i = 0; i < N; i++) if (Smat(i, i) <= 0.0)dZero = kFALSE;
    220         if (dZero)
    221         {
    222                 for (Int_t i = 0; i < N; i++) D(i, i) = 1.0 / TMath::Sqrt(Smat(i, i));
    223                 TMatrixDSym RegMat = Smat.Similarity(D);
    224                 TMatrixDSym Q(2);
    225                 for (Int_t i = 0; i < 2; i++)
     206//
     207// Regularized symmetric matrix inversion
     208//
     209TMatrixDSym VertexFit::RegInv(TMatrixDSym& Min)
     210{
     211        TMatrixDSym M = Min;                            // Decouple from input
     212        Int_t N = M.GetNrows();                 // Matrix size
     213        TMatrixDSym D(N); D.Zero();             // Normaliztion matrix
     214        TMatrixDSym R(N);                               // Normarized matrix
     215        TMatrixDSym Rinv(N);                            // Inverse of R
     216        TMatrixDSym Minv(N);                            // Inverse of M
     217        //
     218        // Check for 0's and normalize
     219        for (Int_t i = 0; i < N; i++)
     220        {
     221                if (M(i, i) != 0.0) D(i, i) = 1. / TMath::Sqrt(TMath::Abs(M(i, i)));
     222                else D(i, i) = 1.0;
     223        }
     224        R = M.Similarity(D);
     225        //
     226        // Recursive algorithms stops when N = 2
     227        //
     228        //****************
     229        // case N = 2  ***
     230        //****************
     231        if (N == 2)
     232        {
     233                Double_t det = R(0, 0) * R(1, 1) - R(0, 1) * R(1, 0);
     234                if (det == 0)
    226235                {
    227                         for (Int_t j = 0; j < 2; j++)Q(i, j) = RegMat(i, j);
     236                        std::cout << "VertexFit::RegInv: null determinant for N = 2" << std::endl;
     237                        Rinv.Zero();    // Return null matrix
    228238                }
    229                 Double_t Det = 1 - Q(0, 1) * Q(1, 0);
    230                 TMatrixDSym H(2);
    231                 H = Q;
    232                 H(0, 1) = -Q(0, 1);
    233                 H(1, 0) = -Q(1, 0);
    234                 TVectorD p(2);
    235                 p(0) = RegMat(0, 2);
    236                 p(1) = RegMat(1, 2);
    237                 Double_t pHp = H.Similarity(p);
    238                 Double_t h = pHp - Det;
    239                 //
    240                 TMatrixDSym pp(2); pp.Rank1Update(p);
    241                 TMatrixDSym F = (h * H) - pp.Similarity(H);
    242                 F *= 1.0 / Det;
    243                 TVectorD b = H * p;
    244                 TMatrixDSym InvReg(3);
    245                 for (Int_t i = 0; i < 2; i++)
     239                else
    246240                {
    247                         InvReg(i, 2) = b(i);
    248                         InvReg(2, i) = b(i);
    249                         for (Int_t j = 0; j < 2; j++) InvReg(i, j) = F(i, j);
     241                        // invert matrix
     242                        Rinv(0, 0) = R(1, 1);
     243                        Rinv(0, 1) = -R(0, 1);
     244                        Rinv(1, 0) = Rinv(0, 1);
     245                        Rinv(1, 1) = R(0, 0);
     246                        Rinv *= 1. / det;
    250247                }
    251                 InvReg(2, 2) = -Det;
    252                 //
    253                 InvReg *= 1.0 / h;
    254                 //
    255                 //
    256                 return InvReg.Similarity(D);
    257         }
     248        }
     249        //****************
     250        // case N > 2  ***
     251        //****************
    258252        else
    259253        {
    260                 D.Zero();
    261                 for (Int_t i = 0; i < N; i++) D(i, i) = 1.0 / TMath::Sqrt(TMath::Abs(Smat(i, i)));
    262                 TMatrixDSym RegMat = Smat.Similarity(D);
    263                 RegMat.Invert();
    264                 return RegMat.Similarity(D);
    265         }
     254                // Break up matrix
     255                TMatrixDSym Q = R.GetSub(0, N - 2, 0, N - 2);   // Upper left
     256                TVectorD p(N - 1);
     257                for (Int_t i = 0; i < N - 1; i++)p(i) = R(N - 1, i);
     258                Double_t q = R(N - 1, N - 1);
     259                //Invert pieces and re-assemble
     260                TMatrixDSym Ainv(N - 1);
     261                TMatrixDSym A(N - 1);
     262                if (TMath::Abs(q) > 1.0e-15)
     263                {
     264                        // Case |q| > 0
     265                        Ainv.Rank1Update(p, -1.0 / q);
     266                        Ainv += Q;
     267                        A = RegInv(Ainv);               // Recursive call
     268                        TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
     269                        //
     270                        TVectorD b = (-1.0 / q) * (A * p);
     271                        for (Int_t i = 0; i < N - 1; i++)
     272                        {
     273                                Rinv(N - 1, i) = b(i);
     274                                Rinv(i, N - 1) = b(i);
     275                        }
     276                        //
     277                        Double_t pdotb = 0.;
     278                        for (Int_t i = 0; i < N - 1; i++)pdotb += p(i) * b(i);
     279                        Double_t c = (1.0 - pdotb) / q;
     280                        Rinv(N - 1, N - 1) = c;
     281                }
     282                else
     283                {
     284                        // case q = 0
     285                        TMatrixDSym Qinv = RegInv(Q);           // Recursive call
     286                        Double_t a = Qinv.Similarity(p);
     287                        Double_t c = -1.0 / a;
     288                        Rinv(N - 1, N - 1) = c;
     289                        //
     290                        TVectorD b = (1.0 / a) * (Qinv * p);
     291                        for (Int_t i = 0; i < N - 1; i++)
     292                        {
     293                                Rinv(N - 1, i) = b(i);
     294                                Rinv(i, N - 1) = b(i);
     295                        }
     296                        //
     297                        A.Rank1Update(p, -1 / a);
     298                        A += Q;
     299                        A.Similarity(Qinv);
     300                        TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
     301                }
     302        }
     303        Minv = Rinv.Similarity(D);
     304        return Minv;
    266305}
    267306//
    … …  
    381420}
    382421//
    383 void  VertexFit::VertexFinder()
    384 {
    385         //
    386         // Vertex fit (units are meters)
     422void VertexFit::UpdateTrkArrays(Int_t i)
     423{
     424        //
     425        // Get track parameters and their covariance
     426        TVectorD par = *fPar[i];
     427        TMatrixDSym Cov = *fCov[i];
     428        //
     429        // Fill all track related work arrays arrays
     430        Double_t fs = ffi[i];                                           // Get phase
     431        TVectorD xs = Fill_x(par, fs);
     432        fx0i.push_back(new TVectorD(xs));                       // Start helix position
     433        //
     434        TMatrixD A = Fill_A(par, fs);                           // A = dx/da = derivatives wrt track parameters
     435        TMatrixDSym Winv = Cov.Similarity(A);           // W^-1 = A*C*A'
     436        fWinvi.push_back(new TMatrixDSym(Winv));                // Store W^-1 matrix
     437        //
     438        TMatrixDSym W = RegInv(Winv);                           // W = (A*C*A')^-1
     439        fWi.push_back(new TMatrixDSym(W));                      // Store W matrix
     440        //
     441        TVectorD a = Fill_a(par, fs);                           // a = dx/ds = derivatives wrt phase
     442        fai.push_back(new TVectorD(a));                         // Store a
     443        //
     444        Double_t a2 = W.Similarity(a);
     445        fa2i.push_back(a2);                                                     // Store a2
     446        //
     447        // Build D matrix
     448        TMatrixDSym B(3);
     449        B.Rank1Update(a, -1. / a2);
     450        B.Similarity(W);
     451        TMatrixDSym Ds = W + B;                                         // D matrix
     452        fDi.push_back(new TMatrixDSym(Ds));                     // Store D matrix
     453}
     454//
     455void  VertexFit::VertexFitter()
     456{
     457        //std::cout << "VertexFitter: just in" << std::endl;
     458        if (fNtr < 2)
     459        {
     460                std::cout << "VertexFit::VertexFitter - Method called with less than 2 tracks - Aborting " << std::endl;
     461                std::exit(1);
     462        }
     463        //
     464        // Vertex fit
    387465        //
    388466        // Initial variable definitions
    389467        TVectorD x(3);
    390468        TMatrixDSym covX(3);
    391         TVectorD x0(3); for (Int_t v = 0; v < 3; v++)x0(v) = 100.; // set to large value
    392469        Double_t Chi2 = 0;
    393         //
    394         // Stored quantities
    395         Double_t* fi = new Double_t[fNtr];                              // Phases
    396         TVectorD** x0i = new TVectorD * [fNtr];                 // Track expansion point
    397         TVectorD** ai = new TVectorD * [fNtr];                          // dx/dphi
    398         Double_t* a2i = new Double_t[fNtr];                             // a'Wa
    399         TMatrixDSym** Di = new TMatrixDSym * [fNtr];            // W-WBW
    400         TMatrixDSym** Wi = new TMatrixDSym * [fNtr];            // (ACA')^-1
    401         TMatrixDSym** Winvi = new TMatrixDSym * [fNtr]; // ACA'
    402         //
    403         // vertex radius approximation
    404         // Maximum impact parameter
    405         Double_t Rd = 0;
    406         for (Int_t i = 0; i < fNtr; i++)
    407         {
    408                 //ObsTrk* t = tracks[i];
    409                 TVectorD par = *fPar[i];
    410                 Double_t Dabs = TMath::Abs(par(0));
    411                 if (Dabs > Rd)Rd = Dabs;
    412         }
    413         //
    414         // Find track pair with largest phi difference
    415         Int_t isel = 0; Int_t jsel = 0; // selected track indices
    416         Double_t dphiMax = 0.0; // Max phi difference
    417 
    418         for (Int_t i = 0; i < fNtr - 1; i++)
    419         {
    420                 //ObsTrk* ti = tracks[i];
    421                 TVectorD pari = *fPar[i];
    422                 Double_t phi1 = pari(1);
    423 
    424                 for (Int_t j = i + 1; j < fNtr; j++)
    425                 {
    426                         //ObsTrk* tj = tracks[j];
    427                         TVectorD parj = *fPar[j];
    428                         Double_t phi2 = parj(1);
    429                         Double_t dphi = TMath::Abs(phi2 - phi1);
    430                         if (dphi > TMath::Pi())dphi = TMath::TwoPi() - dphi;
    431                         if (dphi > dphiMax)
    432                         {
    433                                 isel = i; jsel = j;
    434                                 dphiMax = dphi;
    435                         }
    436                 }
    437         }
    438         //
    439         TVectorD p1 = *fPar[isel];
    440         TVectorD p2 = *fPar[jsel];
    441         Double_t R = FastRv1(p1, p2);
    442         if (R > 1000.0) R = Rd;
    443         R = 0.9 * R + 0.1 * Rd;
     470        TVectorD x0 = fXv;      // If previous fit done
     471        if (fRold < 0.0)for (Int_t i = 0; i < 3; i++)x0(i) = 1000.;     // Set to arbitrary large value if not
     472        //
     473        // Starting vertex radius approximation
     474        //
     475        Double_t R = fRold;                                             // Use previous fit if available
     476        if (R < 0.0) R = StartRadius();                 // Rough vertex estimate
    444477        //
    445478        // Iteration properties
    … …  
    450483        Double_t epsi = 1000.;
    451484        //
     485        // Iteration loop
    452486        while (epsi > eps && Ntry < TryMax)             // Iterate until found vertex is stable
    453487        {
     488                // Initialize arrays
    454489                x.Zero();
    455490                TVectorD cterm(3); TMatrixDSym H(3); TMatrixDSym DW1D(3);
    456                 covX.Zero();    // Reset vertex covariance
     491                covX.Zero();            // Reset vertex covariance
    457492                cterm.Zero();   // Reset constant term
    458493                H.Zero();               // Reset H matrix
    459494                DW1D.Zero();
    460495                //
    461                 //std::cout << "VertexFinder: start loop on tracks" << std::endl;
     496                // Reset work arrays
     497                //
     498                ResetWrkArrays();
     499                //
     500                // Start loop on tracks
     501                //
    462502                for (Int_t i = 0; i < fNtr; i++)
    463503                {
    … …  
    465505                        TVectorD par = *fPar[i];
    466506                        TMatrixDSym Cov = *fCov[i];
    467 
     507                        //
     508                        // For first iteration only
    468509                        Double_t fs;
    469510                        if (Ntry <= 0)  // Initialize all phases on first pass
    … …  
    473514                                Double_t arg = TMath::Max(1.0e-6, (R * R - D * D) / (1 + 2 * C * D));
    474515                                fs = 2 * TMath::ASin(C * TMath::Sqrt(arg));
    475                                 fi[i] = fs;
     516                                ffi.push_back(fs);
    476517                        }
    477518                        //
    478                         // Starting values
    479                         //
    480                         fs = fi[i];             // Get phase
    481                         //std::cout << "VertexFinder: phase fs set" << std::endl;
    482                         TVectorD xs = Fill_x(par, fs);
    483                         //std::cout << "VertexFinder: position xs set" << std::endl;
    484                         x0i[i] = new TVectorD(xs);                              // Start helix position
    485                         //std::cout << "VertexFinder: position x0i stored" << std::endl;
    486                         // W matrix = (A*C*A')^-1; W^-1 = A*C*A'
    487                         TMatrixD A = Fill_A(par, fs);                   // A = dx/da = derivatives wrt track parameters
    488                         //std::cout << "VertexFinder: derivatives A set" << std::endl;
    489                         TMatrixDSym Winv = Cov.Similarity(A);   // W^-1 = A*C*A'
    490                         Winvi[i] = new TMatrixDSym(Winv);               // Store W^-1 matrix
    491                         //std::cout << "VertexFinder: Winvi stored" << std::endl;
    492                         TMatrixDSym W = RegInv3(Winv);                  // W = (A*C*A')^-1
    493                         Wi[i] = new TMatrixDSym(W);                             // Store W matrix
    494                         //std::cout << "VertexFinder: Wi stored" << std::endl;
    495                         TVectorD a = Fill_a(par, fs);                   // a = dx/ds = derivatives wrt phase
    496                         //std::cout << "VertexFinder: derivatives a set" << std::endl;
    497                         ai[i] = new TVectorD(a);                                // Store a
    498                         //std::cout << "VertexFinder: derivatives a stored" << std::endl;
    499                         Double_t a2 = W.Similarity(a);
    500                         a2i[i] = a2;                                                    // Store a2
    501                         // Build D matrix
    502                         TMatrixDSym B(3);
    503 
    504                         B.Rank1Update(a, 1.0);
    505                         B *= -1. / a2;
    506                         B.Similarity(W);
    507                         TMatrixDSym Ds = W + B;                                 // D matrix
    508                         Di[i] = new TMatrixDSym(Ds);                    // Store D matrix
    509                         //std::cout << "VertexFinder: matrix Di stored" << std::endl;
     519                        // Update track related arrays
     520                        //
     521                        UpdateTrkArrays(i);
     522                        TMatrixDSym Ds = *fDi[i];
     523                        TMatrixDSym Winv = *fWinvi[i];
    510524                        TMatrixDSym DsW1Ds = Winv.Similarity(Ds);       // Service matrix to calculate covX
     525                        //
     526                        // Update global arrays
    511527                        DW1D += DsW1Ds;
    512528                        // Update hessian
    513529                        H += Ds;
    514530                        // update constant term
     531                        TVectorD xs = *fx0i[i];
    515532                        cterm += Ds * xs;
    516533                }                               // End loop on tracks
    517534                //
    518535                // update vertex position
    519                 TMatrixDSym H1 = RegInv3(H);
     536                TMatrixDSym H1 = RegInv(H);
    520537                x = H1 * cterm;
    521                 //std::cout << "VertexFinder: x vertex set" << std::endl;
     538                //
    522539                // Update vertex covariance
    523540                covX = DW1D.Similarity(H1);
    524                 //std::cout << "VertexFinder: cov vertex set" << std::endl;
     541                //
    525542                // Update phases and chi^2
    526543                Chi2 = 0.0;
    527544                for (Int_t i = 0; i < fNtr; i++)
    528545                {
    529                         TVectorD lambda = (*Di[i]) * (*x0i[i] - x);
    530                         TMatrixDSym Wm1 = *Winvi[i];
     546                        TVectorD lambda = (*fDi[i]) * (*fx0i[i] - x);
     547                        TMatrixDSym Wm1 = *fWinvi[i];
    531548                        fChi2List(i) = Wm1.Similarity(lambda);
    532549                        Chi2 += fChi2List(i);
    533                         TVectorD a = *ai[i];
    534                         TVectorD b = (*Wi[i]) * (x - *x0i[i]);
    535                         for (Int_t j = 0; j < 3; j++)fi[i] += a(j) * b(j) / a2i[i];
     550                        TVectorD a = *fai[i];
     551                        TVectorD b = (*fWi[i]) * (x - (*fx0i[i]));
     552                        for (Int_t j = 0; j < 3; j++)ffi[i] += a(j) * b(j) / fa2i[i];
    536553                }
    537 
    538554                //
    539555                TVectorD dx = x - x0;
    540556                x0 = x;
    541557                // update vertex stability
    542                 TMatrixDSym Hess = RegInv3(covX);
     558                TMatrixDSym Hess = RegInv(covX);
    543559                epsi = Hess.Similarity(dx);
    544560                Ntry++;
    … …  
    546562                // Store result
    547563                //
    548                 fXv = x;                        // Vertex position
     564                fXv = x;                                // Vertex position
    549565                fcovXv = covX;          // Vertex covariance
    550566                fChi2 = Chi2;           // Vertex fit Chi2
    551                 //
    552                 // Store intermediate data
    553                 //
    554 
    555                 //std::cout << "VertexFinder: before store intermediate data" << std::endl;
    556                 for (Int_t i = 0; i < fNtr; i++)
    557                 {
    558                         //std::cout << "VertexFinder: inside store intermediate data" << std::endl;
    559                         //std::cout << "i = " << i << ", fi[i] = " << fi[i] << std::endl;
    560                         //std::cout << "i = " << i << ", ffi[i] = " << ffi[i] << std::endl;
    561                         ffi[i] = fi[i];                         // Fit phases
    562                         //std::cout << "VertexFinder: fi stored" << std::endl;
    563                         fx0i[i] = x0i[i];                       // Track expansion points
    564                         //std::cout << "VertexFinder: x0i stored" << std::endl;
    565                         fai[i] = ai[i];                         // dx/dphi
    566                         //std::cout << "VertexFinder: ai stored" << std::endl;
    567                         fa2i[i] = a2i[i];                       // a'Wa
    568                         //std::cout << "VertexFinder: a2i stored" << std::endl;
    569                         fDi[i] = Di[i];                         // W-WBW
    570                         //std::cout << "VertexFinder: Di stored" << std::endl;
    571                         fWi[i] = Wi[i];                         // (ACA')^-1
    572                         //std::cout << "VertexFinder: Wi stored" << std::endl;
    573                         fWinvi[i] = Winvi[i];           // ACA'
    574                         //std::cout << "VertexFinder: Winvi stored" << std::endl;
    575                 }
    576                 //std::cout << "Iteration " << Ntry << " completed - Before cleanup" << std::endl;
    577                 //
    578                 // Cleanup
    579                 //
    580                 for (Int_t i = 0; i < fNtr; i++)
    581                 {
    582                         x0i[i]->Clear();
    583                         Winvi[i]->Clear();
    584                         Wi[i]->Clear();
    585                         ai[i]->Clear();
    586                         Di[i]->Clear();
    587 
    588                         delete x0i[i];
    589                         delete Winvi[i];
    590                         delete Wi[i];
    591                         delete ai[i];
    592                         delete Di[i];
    593                 }
    594 
    595                 //std::cout << "Iteration " << Ntry << " completed - After cleanup" << std::endl;
    596         }
    597         //
    598         fVtxDone = kTRUE;       // Set fitting completion flag
    599         //
    600         delete[] fi;            // Phases
    601         delete[] x0i;           // Track expansion point
    602         delete[] ai;            // dx/dphi
    603         delete[] a2i;           // a'Wa
    604         delete[] Di;            // W-WBW
    605         delete[] Wi;            // (ACA')^-1
    606         delete[] Winvi;         // ACA'
    607 }
    608 //
     567        }               // end of iteration loop
     568        //
     569        fVtxDone = kTRUE;               // Set fit completion flag
     570        fRold = TMath::Sqrt(fXv(0)*fXv(0) + fXv(1)*fXv(1));     // Store fit radius
     571        //
     572}
     573//
     574// Return fit vertex
    609575TVectorD VertexFit::GetVtx()
    610576{
    611         //std::cout << "GetVtx: flag set to " << fVtxDone << std::endl;
    612         if (!fVtxDone)VertexFinder();
     577        if (!fVtxDone) VertexFitter();
    613578        return fXv;
    614579}
    615 
     580//
     581// Return fit vertex covariance
    616582TMatrixDSym VertexFit::GetVtxCov()
    617583{
    618         if (!fVtxDone)VertexFinder();
     584        if (!fVtxDone) VertexFitter();
    619585        return fcovXv;
    620586}
    621 
     587//
     588// Return fit vertex chi2
    622589Double_t VertexFit::GetVtxChi2()
    623590{
    624         if (!fVtxDone)VertexFinder();
     591        if (!fVtxDone) VertexFitter();
    625592        return fChi2;
    626593}
    627 
     594//
     595// Return array of chi2 contributions from each track
    628596TVectorD VertexFit::GetVtxChi2List()
    629597{
    630         if (!fVtxDone)VertexFinder();
     598        if (!fVtxDone) VertexFitter();
    631599        return fChi2List;
    632600}
    … …  
    638606}
    639607//
    640 void VertexFit::AddTrk(TVectorD par, TMatrixDSym Cov)                   // Add track to input list
    641 {
    642         std::cout << "VertexFit::AddTrk: Not implemented yet" << std::endl;
    643 }
    644 void VertexFit::RemoveTrk(Int_t iTrk)                                                   // Remove iTrk track
    645 {
    646         std::cout << "VertexFit::RemoveTrk: Not implemented yet" << std::endl;
    647 }
     608// Adding tracks one by one
     609void VertexFit::AddTrk(TVectorD *par, TMatrixDSym *Cov)                 // Add track to input list
     610{
     611        fNtr++;
     612        fChi2List.ResizeTo(fNtr);       // Resize chi2 array
     613        fPar.push_back(par);                    // add new track
     614        fCov.push_back(Cov);
     615        //
     616        // Reset previous vertex temp arrays
     617        ResetWrkArrays();
     618        ffi.clear();
     619        fVtxDone = kFALSE;                      // Reset vertex done flag
     620}
     621//
     622// Removing tracks one by one
     623void VertexFit::RemoveTrk(Int_t iTrk)   // Remove iTrk track
     624{
     625        fNtr--;
     626        fChi2List.Clear();
     627        fChi2List.ResizeTo(fNtr);               // Resize chi2 array
     628        fPar.erase(fPar.begin() + iTrk);                // Remove track
     629        fCov.erase(fCov.begin() + iTrk);
     630        //
     631        // Reset previous vertex temp arrays
     632        ResetWrkArrays();
     633        ffi.clear();
     634        fVtxDone = kFALSE;                      // Reset vertex done flag
     635}

  • external/TrackCovariance/VertexFit.h

    r3cfe61d r64294db  
    77#include <TMatrixDSym.h>
    88#include "ObsTrk.h"
     9#include <vector>
    910#include <iostream>
    1011//
    … …  
    2122        // Inputs
    2223        Int_t fNtr;                                     // Number of tracks
    23         TVectorD** fPar;                        // Input parameter array
    24         TMatrixDSym** fCov;                     // Input parameter covariances
     24        std::vector<TVectorD*> fPar;            // Input parameter array
     25        std::vector<TMatrixDSym*> fCov;// Input parameter covariances
    2526        // Constraints
    2627        Bool_t fVtxCst;                         // Vertex constraint flag
    2728        TVectorD fxCst;                         // Constraint value
    28         TMatrixDSym fCovCst;            // Constraint covariance
     29        TMatrixDSym fCovCst;                    // Constraint covariance
    2930        //
    3031        // Results
    31         Bool_t fVtxDone;                        // Flag vertex fit completed
     32        Bool_t fVtxDone;                                // Flag vertex fit completed
     33        Double_t fRold;                         // Current value of vertex radius
    3234        TVectorD fXv;                           // Found vertex
    3335        TMatrixDSym fcovXv;                     // Vertex covariance
    … …  
    3537        TVectorD fChi2List;                     // List of Chi2 contributions
    3638        //
    37         // Transient arrays
    38         Double_t* ffi;                          // Fit phases
    39         TVectorD** fx0i;                        // Track expansion points
    40         TVectorD** fai;                         // dx/dphi
    41         Double_t* fa2i;                         // a'Wa
    42         TMatrixDSym** fDi;                      // W-WBW
    43         TMatrixDSym** fWi;                      // (ACA')^-1
    44         TMatrixDSym** fWinvi;           // ACA'
     39        // Work arrays
     40        std::vector<Double_t> ffi;                              // Fit phases
     41        std::vector<TVectorD*> fx0i;                            // Track expansion points
     42        std::vector<TVectorD*> fai;                             // dx/dphi
     43        std::vector<Double_t> fa2i;                             // a'Wa
     44        std::vector<TMatrixDSym*> fDi;                  // W-WBW
     45        std::vector<TMatrixDSym*> fWi;                  // (ACA')^-1
     46        std::vector<TMatrixDSym*> fWinvi;               // ACA'
    4547        //
    4648        // Service routines
    47         Double_t FastRv1(TVectorD p1, TVectorD p2);             // Fast vertex radius determination
     49        //void InitWrkArrays();                                                 // Initializations
     50        void ResetWrkArrays();                                                  // Clear work arrays
     51        Double_t StartRadius();                                                 // Starting vertex radius determination
    4852        Double_t FastRv(TVectorD p1, TVectorD p2);              // Fast vertex radius determination
    49         TMatrixDSym RegInv3(TMatrixDSym& Smat0);                // Regularized 3D matrix inversion
    50         TMatrixD Fill_A(TVectorD par, Double_t phi);    // Derivative of track position wrt track parameters
    51         TVectorD Fill_a(TVectorD par, Double_t phi);    // Derivative of track position wrt track phase
     53        TMatrixDSym RegInv(TMatrixDSym& Smat0);                 // Regularized 3D matrix inversion
     54        TMatrixD Fill_A(TVectorD par, Double_t phi);            // Derivative of track position wrt track parameters
     55        TVectorD Fill_a(TVectorD par, Double_t phi);            // Derivative of track position wrt track phase
    5256        TVectorD Fill_x0(TVectorD par);                                 // Track position at dma to z-axis
    53         TVectorD Fill_x(TVectorD par, Double_t phi);    // Track position at given phase
    54         void VertexFinder();                                                    // Vertex finder routine
     57        TVectorD Fill_x(TVectorD par, Double_t phi);            // Track position at given phase
     58        void UpdateTrkArrays(Int_t i);                                  // Fill track realted arrays
     59        void VertexFitter();                                                            // Vertex finder routine
    5560public:
    5661        //
    5762        // Constructors
    5863        VertexFit();                                                                            // Initialize waiting for tracks
    59         VertexFit(Int_t Ntr, ObsTrk** tracks);                          // Initialize with ObsTrk tracks
     64        VertexFit(Int_t Ntr, ObsTrk** tracks);                  // Initialize with ObsTrk tracks
    6065        VertexFit(Int_t Ntr, TVectorD** trkPar, TMatrixDSym** trkCov);  // Initialize with parameters and covariances
    6166        // Destructor
    … …  
    7176        // Handle tracks/constraints
    7277        void AddVtxConstraint(TVectorD xv, TMatrixDSym cov);    // Add gaussian vertex constraint
    73         void AddTrk(TVectorD par, TMatrixDSym Cov);                     // Add track to input list
    74         void RemoveTrk(Int_t iTrk);                                                     // Remove iTrk track
     78        void AddTrk(TVectorD *par, TMatrixDSym *Cov);                           // Add track to input list
     79        void RemoveTrk(Int_t iTrk);                                                             // Remove iTrk track
    7580        //
    7681};
Note: See TracChangeset for help on using the changeset viewer.