Fork me on GitHub

Changes in / [64294db:3cfe61d] in git


Ignore:
Files:
1 deleted
4 edited

Legend:

Unmodified
Added
Removed

  • external/TrackCovariance/TrkUtil.cc

    r64294db r3cfe61d  
    11#include "TrkUtil.h"
     2#include <TMath.h>
    23#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
    149}
    1510TrkUtil::TrkUtil()
    1611{
    1712        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
    2313}
    2414//
    … …  
    2717{
    2818        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
    3419}
    3520//
    … …  
    4429        Double_t pt = p.Pt();
    4530        Double_t C = a / (2 * pt);                      // Half curvature
    46         //std::cout << "ObsTrk::XPtoPar: fB = " << fB << ", a = " << a << ", pt = " << pt << ", C = " << C << std::endl;
     31        //cout << "ObsTrk::XPtoPar: fB = " << fB << ", a = " << a << ", pt = " << pt << ", C = " << C << endl;
    4732        Double_t r2 = x.Perp2();
    4833        Double_t cross = x(0) * p(1) - x(1) * p(0);
    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
     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
    5136        Double_t D;                                                     // Impact parameter D
    5237        if (pt < 10.0) D = (T - pt) / a;
    … …  
    5742        Par(2) = C;             // Store C
    5843        //Longitudinal parameters
    59         Double_t B = C * sqrt(TMath::Max(r2 - D * D, 0.0) / (1 + 2 * C * D));
    60         Double_t st = asin(B) / C;
     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;
    6146        Double_t ct = p(2) / pt;
    6247        Double_t z0 = x(2) - ct * st;
    … …  
    8570        //
    8671        TVector3 Xval;
    87         Xval(0) = -D * sin(phi0);
    88         Xval(1) = D * cos(phi0);
     72        Xval(0) = -D * TMath::Sin(phi0);
     73        Xval(1) = D * TMath::Cos(phi0);
    8974        Xval(2) = z0;
    9075        //
    … …  
    9479TVector3 TrkUtil::ParToP(TVectorD Par)
    9580{
    96         if (fBz == 0.0)std::cout << "TrkUtil::ParToP: Warning Bz not set" << std::endl;
     81        if (fBz == 0.0)
     82std::cout << "TrkUtil::ParToP: Warning Bz not set" << std::endl;
    9783        //
    9884        return ParToP(Par,fBz);
    … …  
    10793        TVector3 Pval;
    10894        Double_t pt = Bz * cSpeed() / TMath::Abs(2 * C);
    109         Pval(0) = pt * cos(phi0);
    110         Pval(1) = pt * sin(phi0);
     95        Pval(0) = pt * TMath::Cos(phi0);
     96        Pval(1) = pt * TMath::Sin(phi0);
    11197        Pval(2) = pt * ct;
    11298        //
    … …  
    127113        Double_t b = -cSpeed() * fBz / 2.;
    128114        pACTS(0) = 1000 * Par(0);               // D from m to mm
    129         pACTS(1) = 1000 * Par(3);       // z0 from m to mm
     115        pACTS(1) = 1000 * Par(3);               // z0 from m to mm
    130116        pACTS(2) = Par(1);                      // Phi0 is unchanged
    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
     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
    133119        pACTS(5) = 0.0;                         // Time: currently undefined
    134120        //
    … …  
    147133        A(0, 0) = 1000.;                // D-D  conversion to mm
    148134        A(1, 2) = 1.0;          // phi0-phi0
    149         A(2, 4) = 1.0 / (sqrt(1.0 + ct * ct) * b);      // q/p-C
     135        A(2, 4) = 1.0 / (TMath::Sqrt(1.0 + ct * ct) * b);       // q/p-C
    150136        A(3, 1) = 1000.;                // z0-z0 conversion to mm
    151137        A(4, 3) = -1.0 / (1.0 + ct * ct); // theta - cot(theta)
    152         A(4, 4) = -C * ct / (b * pow(1.0 + ct * ct, 3.0 / 2.0)); // q/p-cot(theta)
     138        A(4, 4) = -C * ct / (b * TMath::Power(1.0 + ct * ct, 3.0 / 2.0)); // q/p-cot(theta)
    153139        //
    154140        TMatrixDSym Cv = Cov;
    … …  
    232218        return Cmm;
    233219}
    234 //
    235 // Setup chamber volume
    236 void 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
    245 Double_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
    344 Bool_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 //
    392 Double_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 //
    400 Double_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 //
    506 Double_t TrkUtil::funcNcl(Double_t *xp, Double_t *par){
    507         Double_t bg = xp[0];
    508         return Nclusters(bg);
    509 }
    510 //
    511 void 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

    r64294db r3cfe61d  
    66#include <TVectorD.h>
    77#include <TMatrixDSym.h>
    8 #include <TRandom.h>
    98//
    109//
    … …  
    1615protected:
    1716        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
    2417        //
    2518        // Service routines
    … …  
    3831        TVectorD ParToILC(TVectorD Par);                // Parameter conversion
    3932        TMatrixDSym CovToILC(TMatrixDSym Cov);  // Covariance conversion
    40         //
    4133
    4234public:
    … …  
    6860        static TVectorD ParToMm(TVectorD Par);                  // Parameter conversion
    6961        static TMatrixDSym CovToMm(TMatrixDSym Cov);    // Covariance conversion
    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
     62
    8363};
    8464

  • external/TrackCovariance/VertexFit.cc

    r64294db r3cfe61d  
    88// Constructors
    99//
    10 //
    11 // Empty construction (to be used when adding tracks later with AddTrk() )
     10// Empty
    1211VertexFit::VertexFit()
    1312{
    1413        fNtr = 0;
    15         fRold = -1.0;
    1614        fVtxDone = kFALSE;
    1715        fVtxCst = kFALSE;
    … …  
    2119        fcovXv.ResizeTo(3, 3);
    2220}
    23 //
    24 // Build from list of parameters and covariances
     21// Parameters and covariances
    2522VertexFit::VertexFit(Int_t Ntr, TVectorD** trkPar, TMatrixDSym** trkCov)
    2623{
    2724        fNtr = Ntr;
    28         fRold = -1.0;
    2925        fVtxDone = kFALSE;
    3026        fVtxCst = kFALSE;
    … …  
    3430        fcovXv.ResizeTo(3, 3);
    3531        //
    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
     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
    4850VertexFit::VertexFit(Int_t Ntr, ObsTrk** track)
    4951{
    5052        fNtr = Ntr;
    51         fRold = -1.0;
    5253        fVtxDone = kFALSE;
    5354        fVtxCst = kFALSE;
    … …  
    5758        fcovXv.ResizeTo(3, 3);
    5859        //
    59         fChi2List.ResizeTo(fNtr);
     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);
     81}
     82//
     83// Destructor
     84VertexFit::~VertexFit()
     85{
     86        fxCst.Clear();
     87        fCovCst.Clear();
     88        fXv.Clear();
     89        fcovXv.Clear();
     90        fChi2List.Clear();
     91        //
    6092        for (Int_t i = 0; i < fNtr; i++)
    6193        {
    62                 fPar.push_back(new TVectorD(track[i]->GetObsPar()));
    63                 fCov.push_back(new TMatrixDSym(track[i]->GetCov()));
    64         }
    65 }
    66 //
    67 // Destructor
    68 //
    69 void 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 }
    87 VertexFit::~VertexFit()
    88 {
    89         fxCst.Clear();         
    90         fCovCst.Clear();               
    91         fXv.Clear();           
    92         fcovXv.Clear();         
    93         fChi2List.Clear();     
    94         //
    95         for (Int_t i = 0; i < fNtr; i++)
    96         {
    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();
     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        }
    105103        fNtr = 0;
    106 }
    107 //
    108 Double_t VertexFit::FastRv(TVectorD p1, TVectorD p2)
    109 {
    110         //
    111         // Find radius of minimum distance between two tracks
     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//
     115Double_t VertexFit::FastRv1(TVectorD p1, TVectorD p2)
     116{
     117        //
     118        // Find radius of intersection between two tracks in the transverse plane
    112119        //
    113120        // p = (D,phi, C, z0, ct)
    … …  
    115122        // Define arrays
    116123        //
    117         Double_t C1 = p1(2);
    118         Double_t C2 = p2(2);
    119         Double_t ph1 = p1(1);
    120         Double_t ph2 = p2(1);
     124        Double_t r1 = 1.0 / p1(2);
     125        Double_t r2 = 1.0 / p2(2);
    121126        TVectorD x0 = Fill_x0(p1);
    122127        TVectorD y0 = Fill_x0(p2);
    123128        TVectorD n = Fill_a(p1, 0.0);
    124         n *= (2*C1);
     129        n *= r1;
    125130        TVectorD k = Fill_a(p2, 0.0);
    126         k *= (2*C2);
     131        k *= r2;
    127132        //
    128133        // Setup and solve linear system
    … …  
    145150        TVectorD X = x0 + smin(0) * n;
    146151        TVectorD Y = y0 + smin(1) * k;
    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
    161 Double_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++)
    183                 {
    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                         }
     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}
     157Double_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)
     191                {
     192                        std::cout << "FastRv: maximum number of iteration reached" << std::endl;
     193                        break;
    192194                }
    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
     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        }
    203201        //
    204202        return R;
    205203}
    206 //
    207 // Regularized symmetric matrix inversion
    208 //
    209 TMatrixDSym 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)
    235                 {
    236                         std::cout << "VertexFit::RegInv: null determinant for N = 2" << std::endl;
    237                         Rinv.Zero();    // Return null matrix
     204
     205TMatrixDSym 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++)
     226                {
     227                        for (Int_t j = 0; j < 2; j++)Q(i, j) = RegMat(i, j);
    238228                }
    239                 else
    240                 {
    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;
     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++)
     246                {
     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);
    247250                }
    248         }
    249         //****************
    250         // case N > 2  ***
    251         //****************
     251                InvReg(2, 2) = -Det;
     252                //
     253                InvReg *= 1.0 / h;
     254                //
     255                //
     256                return InvReg.Similarity(D);
     257        }
    252258        else
    253259        {
    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;
     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        }
    305266}
    306267//
    … …  
    420381}
    421382//
    422 void 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 //
    455 void  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
     383void  VertexFit::VertexFinder()
     384{
     385        //
     386        // Vertex fit (units are meters)
    465387        //
    466388        // Initial variable definitions
    467389        TVectorD x(3);
    468390        TMatrixDSym covX(3);
     391        TVectorD x0(3); for (Int_t v = 0; v < 3; v++)x0(v) = 100.; // set to large value
    469392        Double_t Chi2 = 0;
    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
     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;
    477444        //
    478445        // Iteration properties
    … …  
    483450        Double_t epsi = 1000.;
    484451        //
    485         // Iteration loop
    486452        while (epsi > eps && Ntry < TryMax)             // Iterate until found vertex is stable
    487453        {
    488                 // Initialize arrays
    489454                x.Zero();
    490455                TVectorD cterm(3); TMatrixDSym H(3); TMatrixDSym DW1D(3);
    491                 covX.Zero();            // Reset vertex covariance
     456                covX.Zero();    // Reset vertex covariance
    492457                cterm.Zero();   // Reset constant term
    493458                H.Zero();               // Reset H matrix
    494459                DW1D.Zero();
    495460                //
    496                 // Reset work arrays
    497                 //
    498                 ResetWrkArrays();
    499                 //
    500                 // Start loop on tracks
    501                 //
     461                //std::cout << "VertexFinder: start loop on tracks" << std::endl;
    502462                for (Int_t i = 0; i < fNtr; i++)
    503463                {
    … …  
    505465                        TVectorD par = *fPar[i];
    506466                        TMatrixDSym Cov = *fCov[i];
    507                         //
    508                         // For first iteration only
     467
    509468                        Double_t fs;
    510469                        if (Ntry <= 0)  // Initialize all phases on first pass
    … …  
    514473                                Double_t arg = TMath::Max(1.0e-6, (R * R - D * D) / (1 + 2 * C * D));
    515474                                fs = 2 * TMath::ASin(C * TMath::Sqrt(arg));
    516                                 ffi.push_back(fs);
     475                                fi[i] = fs;
    517476                        }
    518477                        //
    519                         // Update track related arrays
     478                        // Starting values
    520479                        //
    521                         UpdateTrkArrays(i);
    522                         TMatrixDSym Ds = *fDi[i];
    523                         TMatrixDSym Winv = *fWinvi[i];
     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;
    524510                        TMatrixDSym DsW1Ds = Winv.Similarity(Ds);       // Service matrix to calculate covX
    525                         //
    526                         // Update global arrays
    527511                        DW1D += DsW1Ds;
    528512                        // Update hessian
    529513                        H += Ds;
    530514                        // update constant term
    531                         TVectorD xs = *fx0i[i];
    532515                        cterm += Ds * xs;
    533516                }                               // End loop on tracks
    534517                //
    535518                // update vertex position
    536                 TMatrixDSym H1 = RegInv(H);
     519                TMatrixDSym H1 = RegInv3(H);
    537520                x = H1 * cterm;
    538                 //
     521                //std::cout << "VertexFinder: x vertex set" << std::endl;
    539522                // Update vertex covariance
    540523                covX = DW1D.Similarity(H1);
    541                 //
     524                //std::cout << "VertexFinder: cov vertex set" << std::endl;
    542525                // Update phases and chi^2
    543526                Chi2 = 0.0;
    544527                for (Int_t i = 0; i < fNtr; i++)
    545528                {
    546                         TVectorD lambda = (*fDi[i]) * (*fx0i[i] - x);
    547                         TMatrixDSym Wm1 = *fWinvi[i];
     529                        TVectorD lambda = (*Di[i]) * (*x0i[i] - x);
     530                        TMatrixDSym Wm1 = *Winvi[i];
    548531                        fChi2List(i) = Wm1.Similarity(lambda);
    549532                        Chi2 += fChi2List(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];
     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];
    553536                }
     537
    554538                //
    555539                TVectorD dx = x - x0;
    556540                x0 = x;
    557541                // update vertex stability
    558                 TMatrixDSym Hess = RegInv(covX);
     542                TMatrixDSym Hess = RegInv3(covX);
    559543                epsi = Hess.Similarity(dx);
    560544                Ntry++;
    … …  
    562546                // Store result
    563547                //
    564                 fXv = x;                                // Vertex position
     548                fXv = x;                        // Vertex position
    565549                fcovXv = covX;          // Vertex covariance
    566550                fChi2 = Chi2;           // Vertex fit Chi2
    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
     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//
    575609TVectorD VertexFit::GetVtx()
    576610{
    577         if (!fVtxDone) VertexFitter();
     611        //std::cout << "GetVtx: flag set to " << fVtxDone << std::endl;
     612        if (!fVtxDone)VertexFinder();
    578613        return fXv;
    579614}
    580 //
    581 // Return fit vertex covariance
     615
    582616TMatrixDSym VertexFit::GetVtxCov()
    583617{
    584         if (!fVtxDone) VertexFitter();
     618        if (!fVtxDone)VertexFinder();
    585619        return fcovXv;
    586620}
    587 //
    588 // Return fit vertex chi2
     621
    589622Double_t VertexFit::GetVtxChi2()
    590623{
    591         if (!fVtxDone) VertexFitter();
     624        if (!fVtxDone)VertexFinder();
    592625        return fChi2;
    593626}
    594 //
    595 // Return array of chi2 contributions from each track
     627
    596628TVectorD VertexFit::GetVtxChi2List()
    597629{
    598         if (!fVtxDone) VertexFitter();
     630        if (!fVtxDone)VertexFinder();
    599631        return fChi2List;
    600632}
    … …  
    606638}
    607639//
    608 // Adding tracks one by one
    609 void 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
    623 void 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 }
     640void VertexFit::AddTrk(TVectorD par, TMatrixDSym Cov)                   // Add track to input list
     641{
     642        std::cout << "VertexFit::AddTrk: Not implemented yet" << std::endl;
     643}
     644void VertexFit::RemoveTrk(Int_t iTrk)                                                   // Remove iTrk track
     645{
     646        std::cout << "VertexFit::RemoveTrk: Not implemented yet" << std::endl;
     647}

  • external/TrackCovariance/VertexFit.h

    r64294db r3cfe61d  
    77#include <TMatrixDSym.h>
    88#include "ObsTrk.h"
    9 #include <vector>
    109#include <iostream>
    1110//
    … …  
    2221        // Inputs
    2322        Int_t fNtr;                                     // Number of tracks
    24         std::vector<TVectorD*> fPar;            // Input parameter array
    25         std::vector<TMatrixDSym*> fCov;// Input parameter covariances
     23        TVectorD** fPar;                        // Input parameter array
     24        TMatrixDSym** fCov;                     // Input parameter covariances
    2625        // Constraints
    2726        Bool_t fVtxCst;                         // Vertex constraint flag
    2827        TVectorD fxCst;                         // Constraint value
    29         TMatrixDSym fCovCst;                    // Constraint covariance
     28        TMatrixDSym fCovCst;            // Constraint covariance
    3029        //
    3130        // Results
    32         Bool_t fVtxDone;                                // Flag vertex fit completed
    33         Double_t fRold;                         // Current value of vertex radius
     31        Bool_t fVtxDone;                        // Flag vertex fit completed
    3432        TVectorD fXv;                           // Found vertex
    3533        TMatrixDSym fcovXv;                     // Vertex covariance
    … …  
    3735        TVectorD fChi2List;                     // List of Chi2 contributions
    3836        //
    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'
     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'
    4745        //
    4846        // Service routines
    49         //void InitWrkArrays();                                                 // Initializations
    50         void ResetWrkArrays();                                                  // Clear work arrays
    51         Double_t StartRadius();                                                 // Starting vertex radius determination
     47        Double_t FastRv1(TVectorD p1, TVectorD p2);             // Fast vertex radius determination
    5248        Double_t FastRv(TVectorD p1, TVectorD p2);              // Fast vertex radius determination
    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
     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
    5652        TVectorD Fill_x0(TVectorD par);                                 // Track position at dma to z-axis
    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
     53        TVectorD Fill_x(TVectorD par, Double_t phi);    // Track position at given phase
     54        void VertexFinder();                                                    // Vertex finder routine
    6055public:
    6156        //
    6257        // Constructors
    6358        VertexFit();                                                                            // Initialize waiting for tracks
    64         VertexFit(Int_t Ntr, ObsTrk** tracks);                  // Initialize with ObsTrk tracks
     59        VertexFit(Int_t Ntr, ObsTrk** tracks);                          // Initialize with ObsTrk tracks
    6560        VertexFit(Int_t Ntr, TVectorD** trkPar, TMatrixDSym** trkCov);  // Initialize with parameters and covariances
    6661        // Destructor
    … …  
    7671        // Handle tracks/constraints
    7772        void AddVtxConstraint(TVectorD xv, TMatrixDSym cov);    // Add gaussian vertex constraint
    78         void AddTrk(TVectorD *par, TMatrixDSym *Cov);                           // Add track to input list
    79         void RemoveTrk(Int_t iTrk);                                                             // Remove iTrk track
     73        void AddTrk(TVectorD par, TMatrixDSym Cov);                     // Add track to input list
     74        void RemoveTrk(Int_t iTrk);                                                     // Remove iTrk track
    8075        //
    8176};
Note: See TracChangeset for help on using the changeset viewer.