Fork me on GitHub

Changeset 0b8551f in git


Ignore:
Timestamp:
Nov 29, 2021, 4:04:38 PM (3 years ago)
Author:
GitHub <noreply@…>
Branches:
master
Children:
0c0c9af
Parents:
9a7ea36 (diff), bd376e3 (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@…> (11/29/21 16:04:38)
git-committer:
GitHub <noreply@…> (11/29/21 16:04:38)
Message:

Merge pull request #102 from fbedesch/master

Major update to handle highly displaced tracks

Files:
15 edited

Legend:

Unmodified
Added
Removed

  • cards/delphes_card_IDEA.tcl

    r9a7ea36 r0b8551f  
    917917
    918918    add Branch EFlowTrackMerger/eflowTracks EFlowTrack Track
     919    add Branch TrackSmearing/tracks Track Track
    919920    add Branch Calorimeter/eflowPhotons EFlowPhoton Tower
    920921    add Branch TimeOfFlightNeutralHadron/eflowNeutralHadrons EFlowNeutralHadron Tower

  • examples/Pythia8/ee_zh.cmnd

    r9a7ea36 r0b8551f  
    11
    22! number of events to generate
    3 Main:numberOfEvents = 1000         ! number of events to generate
     3Main:numberOfEvents = 1000       ! number of events to generate
    44
    55Beams:idA = 11                   ! first beam, e- = -11

  • external/TrackCovariance/AcceptanceClx.cc

    r9a7ea36 r0b8551f  
    11#include <TVector3.h>
    22#include "AcceptanceClx.h"
    3 #include <iostream>
    43//
    54// Pt splitting routine
    … …  
    5958        // Initializations
    6059        //
    61         //std::cout << "Entered constructor of AccpeptanceClx" << std::endl;
     60        //cout << "Entered constructor of AccpeptanceClx" << endl;
    6261        // Setup grid
    6362        // Start grid parameters
    … …  
    7473        TVectorF Tha(NpThInp, ThInit);
    7574        Int_t NpTh = Tha.GetNrows();    // Nr. of starting theta points
    76         //std::cout << "AcceptanceClv:: Pta and Tha arrays defined" << std::endl;
     75        //cout << "AcceptanceClv:: Pta and Tha arrays defined" << endl;
    7776        //
    7877        // Grid splitting parameters
    … …  
    128127                                // Pt split
    129128                                if (dPt > dPtMin && dAp > dAmin && Nsplits < MaxSplits) {
    130                                         //std::cout << "Pt(" << ipt << ") = " << Pta(ipt) << ", dAp = " << dAp << std::endl;
    131129                                        NsplitCnt++;    // Total splits counter
    132130                                        Nsplits++;      // Increase splits/cycle
    133                                         NpPt++;         // Increase #pt points
    134                                         //std::cout << "New pt split: dAp = " << dAp << ", dPt = " << dPt <<
    135                                         //      ", Nsplits = " << Nsplits << ", NpPt = " << NpPt << std::endl;
     131                                        NpPt++;         // Increase #pt points
    136132                                        Float_t newPt = 0.5 * (Pta(ipt + 1) + Pta(ipt));
    137133                                        VecInsert(ipt, newPt, Pta);
    … …  
    156152                                // Theta split
    157153                                if (dTh > dThMin && dAt > dAmin && Nsplits < MaxSplits) {
    158                                         //std::cout << "Th(" << ith << ") = " << Tha(ith) << ", dAt = " << dAt << std::endl;
     154                                        //cout << "Th(" << ith << ") = " << Tha(ith) << ", dAt = " << dAt << endl;
    159155                                        NsplitCnt++;    // Total splits counter
    160156                                        Nsplits++;      // Increase splits
    161                                         NpTh++;         // Increase #pt points
    162                                         //std::cout << "New th split: dAt = " << dAt << ", dTh = " << dTh <<
    163                                         //      ", Nsplits = " << Nsplits << ", NpTh = " << NpTh << std::endl;
     157                                        NpTh++;         // Increase #pt points
    164158                                        Float_t newTh = 0.5 * (Tha(ith + 1) + Tha(ith));
    165159                                        VecInsert(ith, newTh, Tha);
    … …  
    202196        fThArray = Tha; // Array of Theta nodes
    203197        //
    204         std::cout << "AcceptanceClx:: Acceptance encoding with " << fNPtNodes
     198                std::cout << "AcceptanceClx:: Acceptance encoding with " << fNPtNodes
    205199                <<" pt nodes and "<< fNThNodes <<" theta nodes"<< std::endl;
    206200        Int_t Nrows = fAcc.GetNrows();
    207201        Int_t Ncols = fAcc.GetNcols();
    208         //std::cout<<"AcceptanceClx:: fAcc rows: "<<Nrows<<", cols: "<<Ncols<<std::endl;
    209         //std::cout<<"AcceptanceClx:: Pta size: "<<Pta.GetNrows()<<", Tha size: "<<Tha.GetNrows()<<std::endl;;
    210         //std::cout<<"AcceptanceClx:: pt nodes"<<std::endl; fPtArray.Print();
    211         //std::cout<<"AcceptanceClx:: th nodes"<<std::endl; fThArray.Print();
    212202}
    213203
    … …  
    290280        //
    291281        // Protect against values out of range
    292         //std::cout << "AcceptanceClx::HitNumber: just in pt= " << pt << ", theta = " << theta << std::endl;
    293         //std::cout << "AcceptanceClx::HitNumber: ptArr(0)= " << fPtArray(0) << ", thArr(0) = " << fThArray(0) << std::endl;
    294         //std::cout << "AcceptanceClx::HitNumber: ptArr(E)= " << fPtArray(fNPtNodes - 1)
    295         //      << ", thArr(E) = " << fThArray(fNThNodes - 1) << std::endl;
    296282        Float_t eps = 1.0e-4;
    297283        Float_t pt0 = (Float_t)pt;
    … …  
    311297        {
    312298                std::cout << "Search error: (ip, pt) = (" << ip << ", " << pt << "), pt0 = " << pt0 << std::endl;
    313                 std::cout << "Search error: pt nodes = " << fNPtNodes
     299                std::cout << "Search error: pt nodes = " << fNPtNodes 
    314300                        << " , last value = " << fPtArray(fNPtNodes - 1) << std::endl;
    315301        }
    … …  
    343329}
    344330//
     331

  • external/TrackCovariance/AcceptanceClx.h

    r9a7ea36 r0b8551f  
    2323private:
    2424        TMatrixF fAcc;          // Acceptance matrix
    25         Int_t fNPtNodes;        // Numer of Pt nodes
     25        Int_t fNPtNodes;        // Numer of Pt nodes 
    2626        TVectorF fPtArray;      // Array of Pt nodes
    27         Int_t fNThNodes;        // Numer of Theta nodes
     27        Int_t fNThNodes;        // Numer of Theta nodes 
    2828        TVectorF fThArray;      // Array of Theta nodes         (Theta in degrees)
    2929        //
    3030        // Service routines
    3131        void VecInsert(Int_t i, Float_t x, TVectorF& Vec);
    32         void SplitPt(Int_t i, TVectorF &AccPt);
     32        void SplitPt(Int_t i, TVectorF &AccPt); 
    3333        void SplitTh(Int_t i, TVectorF &AccTh);
    3434public:
    … …  
    4444        Int_t GetNrPt() { return fNPtNodes; }
    4545        Int_t GetNrTh() { return fNThNodes; }
    46 
     46       
    4747        TVectorF* GetPtArray() { return &fPtArray; }
    4848        TVectorF* GetThArray() { return &fThArray; }

  • external/TrackCovariance/ObsTrk.cc

    r9a7ea36 r0b8551f  
    66#include <TRandom.h>
    77#include <iostream>
     8#include "SolGeom.h"
    89#include "SolGridCov.h"
    910#include "ObsTrk.h"
    … …  
    1112// Constructors
    1213//
    13 // x(3) track origin, p(3) track momentum at origin, Q charge, B magnetic field in Tesla
    14 ObsTrk::ObsTrk(TVector3 x, TVector3 p, Double_t Q, Double_t B, SolGridCov *GC)
     14// x(3) track origin, p(3) track momentum at origin, Q charge, B ma gnetic field in Tesla
     15ObsTrk::ObsTrk(TVector3 x, TVector3 p, Double_t Q, SolGridCov *GC, SolGeom *G)
    1516{
    16         SetB(B);
     17        fB = G->B();
     18        SetB(fB);
     19        fG = G;
    1720        fGC = GC;
    1821        fGenX = x;
    1922        fGenP = p;
    2023        fGenQ = Q;
    21         fB = B;
    2224        fGenPar.ResizeTo(5);
    2325        fGenParMm.ResizeTo(5);
    … …  
    3638        fGenParACTS = ParToACTS(fGenPar);
    3739        fGenParILC = ParToILC(fGenPar);
    38         /*
    39         cout << "ObsTrk::ObsTrk: fGenPar";
    40         for (Int_t i = 0; i < 5; i++)cout << fGenPar(i) << ", ";
    41         cout << endl;
    42         */
    43         fObsPar = GenToObsPar(fGenPar, fGC);
     40        //
     41        fObsPar = GenToObsPar(fGenPar);
    4442        fObsParMm = ParToMm(fObsPar);
    4543        fObsParACTS = ParToACTS(fObsPar);
    … …  
    5452//
    5553// x[3] track origin, p[3] track momentum at origin, Q charge, B magnetic field in Tesla
    56 ObsTrk::ObsTrk(Double_t *x, Double_t *p, Double_t Q, Double_t B, SolGridCov* GC)
     54ObsTrk::ObsTrk(Double_t *x, Double_t *p, Double_t Q, SolGridCov* GC, SolGeom *G)
    5755{
    58         SetB(B);
     56        fB = G->B();
     57        SetB(fB);
     58        fG = G;
    5959        fGC = GC;
    6060        fGenX.SetXYZ(x[0],x[1],x[2]);
    6161        fGenP.SetXYZ(p[0],p[1],p[2]);
    6262        fGenQ = Q;
    63         fB = B;
    6463        fGenPar.ResizeTo(5);
    6564        fGenParMm.ResizeTo(5);
    … …  
    7776        fGenParACTS = ParToACTS(fGenPar);
    7877        fGenParILC = ParToILC(fGenPar);
    79         /*
    80         cout << "ObsTrk::ObsTrk: fGenPar";
    81         for (Int_t i = 0; i < 5; i++)cout << fGenPar(i) << ", ";
    82         cout << endl;
    83         */
    84         fObsPar = GenToObsPar(fGenPar, fGC);
     78        //
     79        fObsPar = GenToObsPar(fGenPar);
    8580        fObsParACTS = ParToACTS(fObsPar);
    8681        fObsParILC = ParToILC(fObsPar);
    … …  
    113108}
    114109//
    115 TVectorD ObsTrk::GenToObsPar(TVectorD gPar, SolGridCov *GC)
     110TVectorD ObsTrk::GenToObsPar(TVectorD gPar)
    116111{
    117         TVector3 p = ParToP(gPar);
    118         Double_t pt = p.Pt();
    119         Double_t tanTh = 1.0 / TMath::Abs(gPar(4));
    120         Double_t angd = TMath::ATan(tanTh)*180. / TMath::Pi();
    121112        //
    122113        // Check ranges
    123         Double_t minPt = GC->GetMinPt ();
    124         //if (pt < minPt) cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is below grid range of " << minPt << endl;
    125         Double_t maxPt = GC->GetMaxPt();
    126         //if (pt > maxPt) cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is above grid range of " << maxPt << endl;
    127         Double_t minAn = GC->GetMinAng();
    128         //if (angd < minAn) cout << "Warning ObsTrk::GenToObsPar: angle " << angd
    129         //      << " is below grid range of " << minAn << endl;
    130         Double_t maxAn = GC->GetMaxAng();
    131         //if (angd > maxAn) cout << "Warning ObsTrk::GenToObsPar: angle " << angd
    132         //      << " is above grid range of " << maxAn << endl;
     114        Double_t minPt = fGC->GetMinPt();
     115        //if (pt < minPt) std::cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is below grid range of " << minPt << std::endl;
     116        Double_t maxPt = fGC->GetMaxPt();
     117        //if (pt > maxPt) std::cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is above grid range of " << maxPt << std::endl;
     118        Double_t minAn = fGC->GetMinAng();
     119        //if (angd < minAn) std::cout << "Warning ObsTrk::GenToObsPar: angle " << angd
     120        //      << " is below grid range of " << minAn << std::endl;
     121        Double_t maxAn = fGC->GetMaxAng();
     122        //if (angd > maxAn) std::cout << "Warning ObsTrk::GenToObsPar: angle " << angd
     123        //      << " is above grid range of " << maxAn << std::endl;
    133124        //
    134         TMatrixDSym Cov = GC->GetCov(pt, angd);
     125        TMatrixDSym Cov(5);
     126        //
     127        // Check if track origin is inside beampipe and betwen the first disks
     128        //
     129        Double_t Rin = fG->GetRmin();
     130        Double_t ZinPos = fG->GetZminPos();
     131        Double_t ZinNeg = fG->GetZminNeg();
     132        Bool_t inside = TrkUtil::IsInside(fGenX, Rin, ZinNeg, ZinPos); // Check if in inner box
     133        if (inside)
     134        {
     135                //std::cout<<"ObsTrk:: inside: x= "<<fGenX(0)<<", y= "<<fGenX(1)
     136                //                        <<", z= "<<fGenX(2)<<std::endl;
     137                // Observed track parameters
     138                Double_t pt = fGenP.Pt();
     139                Double_t angd = fGenP.Theta() * 180. / TMath::Pi();
     140                Cov = fGC->GetCov(pt, angd);                            // Track covariance
     141        }
     142        else
     143        {
     144                //std::cout<<"ObsTrk:: outside: x= "<<fGenX(0)<<", y= "<<fGenX(1)
     145                //                         <<", z= "<<fGenX(2)<<std::endl;
     146                SolTrack* trk = new SolTrack(fGenX, fGenP, fG);
     147                Bool_t Res = kTRUE; Bool_t MS = kTRUE;
     148                trk->CovCalc(Res, MS);                                  // Calculate covariance matrix
     149                Cov = trk->Cov();                                       // Track covariance
     150                delete trk;
     151        }
     152        //
    135153        fCov = Cov;
    136154        //
    137155        // Now do Choleski decomposition and random number extraction, with appropriate stabilization
    138156        //
    139         TMatrixDSym CvN = Cov;
    140         TMatrixDSym DCv(5); DCv.Zero();
    141         TMatrixDSym DCvInv(5); DCvInv.Zero();
    142         for (Int_t id = 0; id < 5; id++)
    143         {
    144                 Double_t dVal = TMath::Sqrt(Cov(id, id));
    145                 DCv   (id, id) = dVal;
    146                 DCvInv(id, id) = 1.0 / dVal;
    147         }
    148         CvN.Similarity(DCvInv);                 // Normalize diagonal to 1
    149         TDecompChol Chl(CvN);
    150         Bool_t OK = Chl.Decompose();            // Choleski decomposition of normalized matrix
    151         TMatrixD U = Chl.GetU();                        // Get Upper triangular matrix
    152         TMatrixD Ut(TMatrixD::kTransposed, U); // Transposed of U (lower triangular)
    153         TVectorD r(5);
    154         for (Int_t i = 0; i < 5; i++)r(i) = gRandom->Gaus(0.0, 1.0);            // Array of normal random numbers
    155         TVectorD oPar = gPar + DCv*(Ut*r);      // Observed parameter vector
     157        TVectorD oPar = TrkUtil::CovSmear(gPar, Cov);
    156158        //
    157159        return oPar;

  • external/TrackCovariance/ObsTrk.h

    r9a7ea36 r0b8551f  
    66#include <TMatrixDSym.h>
    77#include <TDecompChol.h>
     8#include "SolGeom.h"
    89#include "TrkUtil.h"
    910#include "SolGridCov.h"
    … …  
    2324private:       
    2425        Double_t fB;                                    // Solenoid magnetic field
    25         SolGridCov *fGC;                                // Covariance matrix grid
     26        SolGridCov* fGC;                                // Covariance matrix grid
     27        SolGeom*    fG;                                 // Tracker geometry
    2628        Double_t fGenQ;                                 // Generated track charge
    2729        Double_t fObsQ;                                 // Observed  track charge
    … …  
    4749        // Service routines
    4850        //
    49         TVectorD GenToObsPar(TVectorD gPar, SolGridCov* GC);
     51        TVectorD GenToObsPar(TVectorD gPar);
    5052        //
    5153public:
    … …  
    5355        // Constructors
    5456        // x(3) track origin, p(3) track momentum at origin, Q charge, B magnetic field in Tesla
    55         ObsTrk(TVector3 x, TVector3 p, Double_t Q, Double_t B, SolGridCov *GC); // Initialize and generate smeared
    56         ObsTrk(Double_t *x, Double_t *p, Double_t Q, Double_t B, SolGridCov* GC);       // Initialize and generate smeared track
     57        ObsTrk(TVector3 x, TVector3 p, Double_t Q, SolGridCov *GC, SolGeom *G); // Initialize and generate smeared
     58        ObsTrk(Double_t *x, Double_t *p, Double_t Q, SolGridCov* GC, SolGeom *G);       // Initialize and generate smeared track
    5759        // Destructor
    5860        ~ObsTrk();

  • external/TrackCovariance/SolGeom.cc

    r9a7ea36 r0b8551f  
    8787    if (flLay == 1) fNm++;
    8888  }
     89//
     90// Define inner box for fast tracking
     91//
     92    SetMinBoundaries();
    8993}
    9094
    … …  
    104108  delete[] fflLay;
    105109}
     110
     111//
     112// Get inner boundaries of cylindrical box for fast simulation
     113//
     114void SolGeom::SetMinBoundaries()
     115{
     116        // Get radius of first barrel layer
     117        fRmin = 1000000.0;
     118        fZminPos = 1000000.0;
     119        fZminNeg = -1000000.0;
     120        for (Int_t i = 0; i < fNlay; i++){
     121                if (ftyLay[i] == 1) {                           // Cylinders
     122                        if (frPos[i] < fRmin) fRmin = frPos[i];
     123                }
     124                if (ftyLay[i] == 2) {                           // Disks
     125                        if (frPos[i] > 0.0 && frPos[i] < fZminPos) fZminPos = frPos[i]; // Positive direction
     126                        if (frPos[i] < 0.0 && frPos[i] > fZminNeg) fZminNeg = frPos[i]; // Negative direction
     127                }
     128        }
     129}

  • external/TrackCovariance/SolGeom.h

    r9a7ea36 r0b8551f  
    66
    77// Class to create geometry for solenoid geometry
     8// Simplified implementations with cylindrical and disk layers
     9//
     10// Author: F. Bedeschi, INFN - Pisa
    811
    912class SolGeom{
    … …  
    3740  Double_t *fsgLayL; // Resolution Lower side (meters) - 0 = no measurement
    3841  Bool_t   *fflLay;  // measurement flag = T, scattering only = F
     42//
     43//
     44  Double_t fRmin;       // Radius of first barrel layer
     45  Double_t fZminPos;    // Z of first disk in positive direction
     46  Double_t fZminNeg;    // Z of first disk in negative direction
     47  void SetMinBoundaries();      // define inner box for fast simulation
    3948
    4049public:
    … …  
    6170  Double_t lSgL(Int_t nlayer)      { return fsgLayL[nlayer]; }
    6271  Bool_t   isMeasure(Int_t nlayer) { return fflLay[nlayer]; }
     72  //
     73  // Define cylindrical box to use for fast simulation
     74  //
     75  Double_t GetRmin() { return fRmin; }
     76  Double_t GetZminPos() { return fZminPos; }
     77  Double_t GetZminNeg() { return fZminNeg; }
    6378};
    6479

  • external/TrackCovariance/SolGridCov.cc

    r9a7ea36 r0b8551f  
    103103        return Accept;
    104104}
    105 
    106 
     105//
     106// Detailed acceptance check
     107//
     108Bool_t SolGridCov::IsAccepted(TVector3 x, TVector3 p, SolGeom* G)
     109{
     110        Bool_t Accept = kFALSE;
     111        //
     112        // Check if track origin is inside beampipe and betwen the first disks
     113        //
     114        Double_t Rin = G->GetRmin();
     115        Double_t ZinPos = G->GetZminPos();
     116        Double_t ZinNeg = G->GetZminNeg();
     117        Bool_t inside = TrkUtil::IsInside(x, Rin, ZinNeg, ZinPos); // Check if in inner box
     118        if (inside) Accept = IsAccepted(p);
     119        else
     120        {
     121                SolTrack* trk = new SolTrack(x, p, G);
     122                if (trk->nmHit() >= fNminHits)Accept = kTRUE;
     123                delete trk;
     124        }
     125        //
     126        return Accept;
     127}
     128
     129//
    107130// Find bin in grid
    108131Int_t SolGridCov::GetMinIndex(Double_t xval, Int_t N, TVectorD x)
    … …  
    193216  if (!Chl.Decompose())
    194217  {
    195     cout << "SolGridCov::GetCov: Interpolated matrix not positive definite. Recovering ...." << endl;
     218    std::cout << "SolGridCov::GetCov: Interpolated matrix not positive definite. Recovering ...." << std::endl;
    196219    TMatrixDSym rCv = MakePosDef(CvN); CvN = rCv;
    197220    TMatrixDSym DCv(5); DCv.Zero();

  • external/TrackCovariance/SolGridCov.h

    r9a7ea36 r0b8551f  
    1818  TVectorD fAnga;    // Array of angle points in degrees
    1919  TMatrixDSym *fCov; // Pointers to grid of covariance matrices
    20         AcceptanceClx *fAcc;                            // Pointer to acceptance class
    21         Int_t fNminHits;                                        // Minimum number of hits to accept track
     20  AcceptanceClx *fAcc;          // Pointer to acceptance class
     21  Int_t fNminHits;              // Minimum number of hits to accept track
    2222  // Service routines
    2323  Int_t GetMinIndex(Double_t xval, Int_t N, TVectorD x); // Find bin
    … …  
    4040        void SetMinHits(Int_t MinHits) { fNminHits = MinHits; };        // Set minimum number of hits to accept (default = 6)
    4141        Int_t GetMinHits() { return fNminHits; };
    42         Bool_t IsAccepted(Double_t pt, Double_t Theta);         // From pt (GeV) and theta (degrees)
    43         Bool_t IsAccepted(Double_t *p);                                         // From momentum components (GeV)
    44         Bool_t IsAccepted(TVector3 p);                                          // As above in Vector3 format
     42        Bool_t IsAccepted(Double_t pt, Double_t Theta);                 // From pt (GeV) and theta (degrees)
     43        Bool_t IsAccepted(Double_t *p);                                 // From momentum components (GeV)
     44        Bool_t IsAccepted(TVector3 p);                                  // As above in Vector3 format
     45        Bool_t IsAccepted(TVector3 x, TVector3 p, SolGeom *G);          // As above checking track origin
    4546
    4647};

  • external/TrackCovariance/SolTrack.cc

    r9a7ea36 r0b8551f  
    1 #include <iostream>
    21
     2#include "SolGeom.h"
     3#include "SolTrack.h"
    34#include <TString.h>
    45#include <TMath.h>
    … …  
    78#include <TDecompChol.h>
    89#include <TMatrixDSymEigen.h>
    9 
    10 #include "SolGeom.h"
    11 #include "SolTrack.h"
    12 
    13 using namespace std;
    14 
     10#include <TGraph.h>
     11#include <iostream>
     12//
     13// Constructors
    1514SolTrack::SolTrack(Double_t *x, Double_t *p, SolGeom *G)
    1615{
    17   fG = G;
    18   // Store momentum
    19   fp[0] = p[0]; fp[1] = p[1]; fp[2] = p[2];
    20   Double_t px = p[0]; Double_t py = p[1]; Double_t pz = p[2];
    21   fx[0] = x[0]; fx[1] = x[1]; fx[2] = x[2];
    22   Double_t xx = x[0]; Double_t yy = x[1]; Double_t zz = x[2];
    23   // Store parameters
    24   Double_t pt = TMath::Sqrt(px*px + py*py);
    25   Double_t Charge = 1.0;            // Don't worry about charge for now
    26   Double_t a = -Charge*G->B()*0.2998;     // Normalized speed of light
    27   Double_t C = a / (2 * pt);          // pt in GeV, B in Tesla, C in meters
    28   Double_t r2 = xx*xx + yy*yy;
    29   Double_t cross = xx*py - yy*px;
    30   Double_t T = TMath::Sqrt(pt*pt - 2 * a*cross + a*a*r2);
    31   Double_t phi0 = TMath::ATan2((py - a*xx) / T, (px + a*yy) / T);
    32   Double_t D;
    33   if (pt < 10.0) D = (T - pt) / a;
    34   else D = (-2 * cross + a*r2) / (T + pt);
    35   Double_t B = C*TMath::Sqrt((r2 - D*D) / (1 + 2 * C*D));
    36   Double_t st = TMath::ASin(B) / C;
    37   Double_t ct = pz / pt;
    38   Double_t z0 = zz - ct*st;
    39   fpar[0] = D;
    40   fpar[1] = phi0;
    41   fpar[2] = C;
    42   fpar[3] = z0;
    43   fpar[4] = ct;
    44   // Init covariances
    45   fCov.ResizeTo(5, 5);
     16        // Set B field
     17        fG = G;                                 // Store geometry
     18        Double_t B = G->B();
     19        SetB(B);
     20        // Store momentum and position
     21        fp[0] = p[0]; fp[1] = p[1]; fp[2] = p[2];
     22        fx[0] = x[0]; fx[1] = x[1]; fx[2] = x[2];
     23        // Get generated parameters
     24        TVector3 xv(fx);
     25        TVector3 pv(fp);
     26        Double_t Charge = 1.0;                                          // Don't worry about charge for now
     27        TVectorD gPar = XPtoPar(xv, pv, Charge);
     28        // Store parameters
     29        fpar[0] = gPar(0);
     30        fpar[1] = gPar(1);
     31        fpar[2] = gPar(2);
     32        fpar[3] = gPar(3);
     33        fpar[4] = gPar(4);
     34        //cout << "SolTrack:: C = " << C << ", fpar[2] = " << fpar[2] << endl;
     35        //
     36        // Init covariances
     37        //
     38        fCov.ResizeTo(5, 5);
    4639}
    4740SolTrack::SolTrack(TVector3 x, TVector3 p, SolGeom* G)
    4841{
    49         fG = G;
     42        // set B field
     43        fG = G;                                 // Store geometry
     44        Double_t B = G->B();
     45        SetB(B);
    5046        // Store momentum
    5147        fp[0] = p(0); fp[1] = p(1); fp[2] = p(2);
    52         Double_t px = p(0); Double_t py = p(1); Double_t pz = p(2);
    5348        fx[0] = x(0); fx[1] = x(1); fx[2] = x(2);
    54         Double_t xx = x(0); Double_t yy = x(1); Double_t zz = x(2);
     49        // Get generated parameters
     50        Double_t Charge = 1.0;                                          // Don't worry about charge for now
     51        TVectorD gPar = XPtoPar(x, p, Charge);
    5552        // Store parameters
    56         Double_t pt = TMath::Sqrt(px * px + py * py);
    57         Double_t Charge = 1.0;                                          // Don't worry about charge for now
    58         Double_t a = -Charge * G->B() * 0.2998;                 // Normalized speed of light
    59         Double_t C = a / (2 * pt);                                      // pt in GeV, B in Tesla, C in meters
    60         Double_t r2 = xx * xx + yy * yy;
    61         Double_t cross = xx * py - yy * px;
    62         Double_t T = TMath::Sqrt(pt * pt - 2 * a * cross + a * a * r2);
    63         Double_t phi0 = TMath::ATan2((py - a * xx) / T, (px + a * yy) / T);
    64         Double_t D;
    65         if (pt < 10.0) D = (T - pt) / a;
    66         else D = (-2 * cross + a * r2) / (T + pt);
    67         Double_t B = C * TMath::Sqrt((r2 - D * D) / (1 + 2 * C * D));
    68         Double_t st = TMath::ASin(B) / C;
    69         Double_t ct = pz / pt;
    70         Double_t z0 = zz - ct * st;
     53        fpar[0] = gPar(0);
     54        fpar[1] = gPar(1);
     55        fpar[2] = gPar(2);
     56        fpar[3] = gPar(3);
     57        fpar[4] = gPar(4);
     58        //cout << "SolTrack:: C = " << C << ", fpar[2] = " << fpar[2] << endl;
     59        //
     60        // Init covariances
     61        //
     62        fCov.ResizeTo(5, 5);
     63}
     64//
     65SolTrack::SolTrack(Double_t D, Double_t phi0, Double_t C, Double_t z0, Double_t ct, SolGeom *G)
     66{
     67        fG = G;
     68        Double_t B = G->B();
     69        SetB(B);
     70        // Store parameters
    7171        fpar[0] = D;
    7272        fpar[1] = phi0;
    … …  
    7474        fpar[3] = z0;
    7575        fpar[4] = ct;
    76         //cout << "SolTrack:: C = " << C << ", fpar[2] = " << fpar[2] << endl;
     76        // Store momentum
     77        Double_t pt = B * TrkUtil::cSpeed() / TMath::Abs(2 * C);
     78        Double_t px = pt*TMath::Cos(phi0);
     79        Double_t py = pt*TMath::Sin(phi0);
     80        Double_t pz = pt*ct;
     81        //
     82        fp[0] = px; fp[1] = py; fp[2] = pz;
     83        fx[0] = -D*TMath::Sin(phi0); fx[1] = D*TMath::Cos(phi0);  fx[2] = z0;
    7784        //
    7885        // Init covariances
    7986        //
    8087        fCov.ResizeTo(5, 5);
    81 }
    82 
    83 SolTrack::SolTrack(Double_t D, Double_t phi0, Double_t C, Double_t z0, Double_t ct, SolGeom *G)
    84 {
    85   fG = G;
    86   // Store parameters
    87   fpar[0] = D;
    88   fpar[1] = phi0;
    89   fpar[2] = C;
    90   fpar[3] = z0;
    91   fpar[4] = ct;
    92   // Store momentum
    93   Double_t pt = G->B()*0.2998 / TMath::Abs(2 * C);
    94   Double_t px = pt*TMath::Cos(phi0);
    95   Double_t py = pt*TMath::Sin(phi0);
    96   Double_t pz = pt*ct;
    97 
    98   fp[0] = px; fp[1] = py; fp[2] = pz;
    99   fx[0] = -D*TMath::Sin(phi0); fx[1] = D*TMath::Cos(phi0);  fx[2] = z0;
    100   // Init covariances
    101   fCov.ResizeTo(5, 5);
    10288}
    10389// Destructor
    10490SolTrack::~SolTrack()
    10591{
    106         delete[] & fp;
    107         delete[] & fpar;
    108   fCov.Clear();
    109 }
     92        fCov.Clear();
     93}
     94//
    11095// Calculate intersection with given layer
    11196Bool_t SolTrack::HitLayer(Int_t il, Double_t &R, Double_t &phi, Double_t &zz)
    11297{
    113   Double_t Di = D();
    114   Double_t phi0i = phi0();
    115   Double_t Ci = C();
    116   Double_t z0i = z0();
    117   Double_t cti = ct();
    118 
    119   R = 0; phi = 0; zz = 0;
    120 
    121   Bool_t val = kFALSE;
    122   if (fG->lTyp(il) == 1)      // Cylinder: layer at constant R
    123   {
    124     R = fG->lPos(il);
    125     Double_t argph = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di);
    126     if (TMath::Abs(argph) < 1.0)
    127     {
    128       Double_t argz = Ci*TMath::Sqrt((R*R - Di*Di) / (1 + 2 * Ci*Di));
    129       if (TMath::Abs(argz) < 1.0)
    130       {
    131         zz = z0i + cti*TMath::ASin(argz) / Ci;
    132         if (zz > fG->lxMin(il) && zz < fG->lxMax(il))
    133         {
    134           phi = phi0i + TMath::ASin(argph);
    135           val = kTRUE;
    136         }
    137       }
    138     }
    139   }
    140   else if (fG->lTyp(il) == 2)   // disk: layer at constant z
    141   {
    142     zz = fG->lPos(il);
    143     Double_t arg = Ci*(zz - z0i) / cti;
    144     if (TMath::Abs(arg) < 1.0 && (zz - z0i) / cti > 0)
    145     {
    146       R = TMath::Sqrt(Di*Di + (1. + 2.*Ci*Di)*pow(TMath::Sin(arg), 2) / (Ci*Ci));
    147       if (R > fG->lxMin(il) && R < fG->lxMax(il))
    148       {
    149         Double_t arg1 = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di);
    150         if (TMath::Abs(arg1) < 1.0)
    151         {
    152           phi = phi0i + TMath::ASin(arg1);
    153           val = kTRUE;
    154         }
    155       }
    156     }
    157   }
    158   //
    159   return val;
    160 }
     98        Double_t Di = D();
     99        Double_t phi0i = phi0();
     100        Double_t Ci = C();
     101        Double_t z0i = z0();
     102        Double_t cti = ct();
     103        //
     104        R = 0; phi = 0; zz = 0;
     105        Bool_t val = kFALSE;
     106        Double_t Rmin = TMath::Sqrt(fx[0] * fx[0] + fx[1] * fx[1]); // Smallest track radius
     107        if (Rmin < TMath::Abs(Di)) return val;
     108        //
     109        Double_t ArgzMin = Ci * TMath::Sqrt((Rmin * Rmin - Di * Di) / (1 + 2 * Ci * Di));
     110        Double_t stMin = TMath::ASin(ArgzMin) / Ci;                                     // Arc length at track origin
     111        //
     112        if (fG->lTyp(il) == 1)                  // Cylinder: layer at constant R
     113        {
     114                R = fG->lPos(il);
     115                Double_t argph = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di);
     116                if (TMath::Abs(argph) < 1.0 && R > Rmin)
     117                {
     118                        Double_t argz = Ci*TMath::Sqrt((R*R - Di*Di) / (1 + 2 * Ci*Di));
     119                        if (TMath::Abs(argz) < 1.0)
     120                        {
     121                                zz = z0i + cti*TMath::ASin(argz) / Ci;
     122                                if (zz > fG->lxMin(il) && zz < fG->lxMax(il))
     123                                {
     124                                        phi = phi0i + TMath::ASin(argph);
     125                                        val = kTRUE;
     126                                }
     127                        }
     128                }
     129        }
     130        else if (fG->lTyp(il) == 2)             // disk: layer at constant z
     131        {
     132                zz = fG->lPos(il);
     133                Double_t st = (zz - z0i) / cti;
     134                if (TMath::Abs(Ci * st) < 1.0 && st > stMin)
     135                {
     136                        R = TMath::Sqrt(Di * Di + (1. + 2. * Ci * Di) * pow(TMath::Sin(Ci * st), 2) / (Ci * Ci));
     137                        if (R > fG->lxMin(il) && R < fG->lxMax(il))
     138                        {
     139                                Double_t arg1 = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di);
     140                                if (TMath::Abs(arg1) < 1.0)
     141                                {
     142                                        phi = phi0i + TMath::ASin(arg1);
     143                                        val = kTRUE;
     144                                }
     145                        }
     146                }
     147        }
     148        //
     149        return val;
     150}
     151//
    161152// # of layers hit
    162153Int_t SolTrack::nHit()
    163154{
    164   Int_t kh = 0;
    165   Double_t R; Double_t phi; Double_t zz;
    166   for (Int_t i = 0; i < fG->Nl(); i++)
    167   if (HitLayer(i, R, phi, zz))kh++;
    168 
    169   return kh;
    170 }
    171 
     155        Int_t kh = 0;
     156        Double_t R; Double_t phi; Double_t zz;
     157        for (Int_t i = 0; i < fG->Nl(); i++)
     158        if (HitLayer(i, R, phi, zz))kh++;
     159        //
     160        return kh;
     161}
     162//
    172163// # of measurement layers hit
    173164Int_t SolTrack::nmHit()
    … …  
    181172}
    182173//
    183 
    184 
    185174// List of layers hit with intersections
    186175Int_t SolTrack::HitList(Int_t *&ihh, Double_t *&rhh, Double_t *&zhh)
    187176{
    188   // Return lists of hits associated to a track including all scattering layers.
    189   // Return value is the total number of measurement hits
    190   // kmh = total number of measurement layers hit for given type
    191   // ihh = pointer to layer number
    192   // rhh = radius of hit
    193   // zhh = z of hit
    194 
    195   // ***** NB: double layers with stereo on lower layer not included
    196 
    197   Int_t kh = 0; // Number of layers hit
    198   Int_t kmh = 0; // Number of measurement layers of given type
    199   for (Int_t i = 0; i < fG->Nl(); i++)
    200   {
    201     Double_t R; Double_t phi; Double_t zz;
    202     if (HitLayer(i, R, phi, zz)) // Only barrel type layers
    203     {
    204       zhh[kh] = zz;
    205       rhh[kh] = R;
    206       ihh[kh] = i;
    207       if (fG->isMeasure(i))kmh++;
    208       kh++;
    209     }
    210   }
    211 
    212   return kmh;
    213 }
    214 
     177        //
     178        // Return lists of hits associated to a track including all scattering layers.
     179        // Return value is the total number of measurement hits
     180        // kmh = total number of measurement layers hit for given type
     181        // ihh = pointer to layer number
     182        // rhh = radius of hit
     183        // zhh = z of hit
     184        //
     185        // ***** NB: double layers with stereo on lower layer not included
     186        //
     187        Int_t kh = 0;   // Number of layers hit
     188        Int_t kmh = 0;  // Number of measurement layers of given type
     189        for (Int_t i = 0; i < fG->Nl(); i++)
     190        {
     191                Double_t R; Double_t phi; Double_t zz;
     192                if (HitLayer(i, R, phi, zz))
     193                {
     194                        zhh[kh] = zz;
     195                        rhh[kh] = R;
     196                        ihh[kh] = i;
     197                        if (fG->isMeasure(i))kmh++;
     198                        kh++;
     199                }
     200        }
     201        //
     202        return kmh;
     203}
     204//
    215205// List of XYZ measurements without any error
    216206Int_t SolTrack::HitListXYZ(Int_t *&ihh, Double_t *&Xh, Double_t *&Yh, Double_t *&Zh)
    217207{
    218208        //
    219         // Return lists of hits associated to a track for all measurement layers.
     209        // Return lists of hits associated to a track for all measurement layers. 
    220210        // Return value is the total number of measurement hits
    221211        // kmh = total number of measurement layers hit for given type
    … …  
    244234}
    245235//
     236// Track plot
     237//
     238TGraph *SolTrack::TrkPlot()
     239{
     240        //
     241        // Fill list of layers hit
     242        //
     243        Int_t Nhit = nHit();                                    // Total number of layers hit
     244        //cout << "Nhit = " << Nhit << endl;
     245        Double_t *zh = new Double_t[Nhit];              // z of hit
     246        Double_t *rh = new Double_t[Nhit];              // r of hit
     247        Int_t    *ih = new Int_t   [Nhit];              // true index of layer
     248        Int_t kmh;                                                              // Number of measurement layers hit
     249        //
     250        kmh = HitList(ih, rh, zh);                              // hit layer list
     251        //for (Int_t j = 0; j < Nhit; j++) cout << "r = " << rh[j] << ", z = " << zh[j] << endl;
     252        Double_t *dh = new Double_t[Nhit];              // Hit distance from origin
     253        for(Int_t i=0; i<Nhit; i++)dh[i] = TMath::ASin(C() * TMath::Sqrt((rh[i] * rh[i] - D() * D()) / (1. + 2 * C() * D()))) / C();    // Arc length traveled;
     254        //
     255        Int_t *hord = new Int_t[Nhit];
     256        TMath::Sort(Nhit, dh, hord, kFALSE);            // Order by increasing phase
     257        Double_t *z = new Double_t[Nhit];               // z of ordered hit
     258        Double_t *r = new Double_t[Nhit];               // r of ordered hit
     259        for (Int_t i = 0; i < Nhit; i++)
     260        {
     261                z[i] = zh[hord[i]];
     262                r[i] = rh[hord[i]];
     263        }
     264        //cout << "After ordering" << endl;
     265        //for (Int_t j = 0; j < Nhit; j++) cout << "r = " << rh[j] << ", z = " << zh[j] << endl;
     266        TGraph *gr = new TGraph(Nhit, z, r);    // graph intersection with layers
     267        gr->SetMarkerStyle(kCircle);
     268        gr->SetMarkerColor(kMagenta);
     269        gr->SetMarkerSize(1);
     270        gr->SetLineColor(kMagenta);
     271        //
     272        // clean up
     273        //
     274        delete[] zh;
     275        delete[] rh;
     276        delete[] ih;
     277        delete[] hord;
     278        return gr;
     279}
     280//
    246281// Covariance matrix estimation
    247282//
    … …  
    250285        //
    251286        //
    252         // Input flags:
     287        // Input flags: 
    253288        //                              Res = .TRUE. turn on resolution effects/Use standard resolutions
    254289        //                                        .FALSE. set all resolutions to 0
    … …  
    265300        Int_t ntry = 0;
    266301        Int_t ntrymax = 0;
    267         Int_t Nhit = nHit();                                    // Total number of layers hit
     302        Int_t Nhit = nHit();                            // Total number of layers hit
    268303        Double_t *zhh = new Double_t[Nhit];             // z of hit
    269304        Double_t *rhh = new Double_t[Nhit];             // r of hit
    270305        Double_t *dhh = new Double_t[Nhit];             // distance of hit from origin
    271306        Int_t    *ihh = new Int_t[Nhit];                // true index of layer
    272         Int_t kmh;                                                              // Number of measurement layers hit
     307        Int_t kmh;                                      // Number of measurement layers hit
    273308        //
    274309        kmh = HitList(ihh, rhh, zhh);                   // hit layer list
    275         Int_t mTot = 0;                                                 // Total number of measurements
     310        Int_t mTot = 0;                                 // Total number of measurements
    276311        for (Int_t i = 0; i < Nhit; i++)
    277312        {
    278                 dhh[i] = TMath::Sqrt(rhh[i] * rhh[i] + zhh[i] * zhh[i]);
     313                Double_t rr = rhh[i];
     314                dhh[i] = TMath::ASin(C() * TMath::Sqrt((rr * rr - D() * D()) / (1. + 2 * C() * D()))) / C();    // Arc length traveled
    279315                if (fG->isMeasure(ihh[i])) mTot += fG->lND(ihh[i]);     // Count number of measurements
    280316        }
    281317        //
    282         // Order hit list by increasing distance from origin
     318        // Order hit list by increasing arc length
    283319        //
    284320        Int_t    *hord = new Int_t[Nhit];               // hit order by increasing distance from origin
    285         TMath::Sort(Nhit, dhh, hord, kFALSE);   // Order by increasing distance from origin
     321        TMath::Sort(Nhit, dhh, hord, kFALSE);           // Order by increasing distance from origin
    286322        Double_t *zh = new Double_t[Nhit];              // d-ordered z of hit
    287323        Double_t *rh = new Double_t[Nhit];              // d-ordered r of hit
    … …  
    297333        // Store interdistances and multiple scattering angles
    298334        //
    299         Double_t sn2t = 1.0 / (1 + ct()*ct());                  //sin^2 theta of track
     335        Double_t sn2t = 1.0 / (1.0 + ct()*ct());                        //sin^2 theta of track
    300336        Double_t cs2t = 1.0 - sn2t;                                             //cos^2 theta
    301337        Double_t snt = TMath::Sqrt(sn2t);                               // sin theta
    … …  
    306342        //
    307343        TMatrixDSym dik(Nhit);  dik.Zero();             // Distances between layers
    308         Double_t *thms = new Double_t[Nhit];    // Scattering angles/plane
    309         Double_t *cs = new Double_t[Nhit];              // Cosine of angle with layer normal
     344        Double_t *thms = new Double_t[Nhit];            // Scattering angles/plane
     345        Double_t* cs = new Double_t[Nhit];              // Cosine of angle with normal in transverse plane
     346        //
    310347        for (Int_t ii = 0; ii < Nhit; ii++)             // Hit layer loop
    311348        {
    312349                Int_t i = ih[ii];                                       // Get true layer number
     350                Int_t il = hord[ii];                                    // Unordered layer
    313351                Double_t B = C()*TMath::Sqrt((rh[ii] * rh[ii] - D()*D()) / (1 + 2 * C()*D()));
     352                //
    314353                Double_t pxi = px0*(1-2*B*B)-2*py0*B*TMath::Sqrt(1-B*B);                // Momentum at scattering layer
    315354                Double_t pyi = py0*(1-2*B*B)+2*px0*B*TMath::Sqrt(1-B*B);
    316355                Double_t pzi = pz0;
    317356                Double_t ArgRp = (rh[ii]*C() + (1 + C() * D())*D() / rh[ii]) / (1 + 2 * C()*D());
     357                //
    318358                Double_t phi = phi0() + TMath::ASin(ArgRp);
    319359                Double_t nx = TMath::Cos(phi);          // Barrel layer normal
    320360                Double_t ny = TMath::Sin(phi);
    321361                Double_t nz = 0.0;
    322                 if (fG->lTyp(i) == 2)                                                           // this is Z layer
     362                cs[ii] = TMath::Abs((pxi * nx + pyi * ny) / pt());
     363                //
     364                if (fG->lTyp(i) == 2)                   // this is Z layer
    323365                {
    324366                        nx = 0.0;
    … …  
    326368                        nz = 1.0;
    327369                }
    328                 Double_t corr = (pxi*nx + pyi * ny + pzi * nz) / p();
    329                 cs[ii] = corr;
    330                 Double_t Rlf = fG->lTh(i) / (corr*fG->lX0(i));          // Rad. length fraction
     370                Double_t corr = TMath::Abs(pxi*nx + pyi * ny + pzi * nz) / p();
     371                Double_t Rlf = fG->lTh(i) / (corr*fG->lX0(i));                                  // Rad. length fraction
    331372                thms[ii] = 0.0136*TMath::Sqrt(Rlf)*(1.0 + 0.038*TMath::Log(Rlf)) / p();         // MS angle
    332373                if (!MS)thms[ii] = 0;
    … …  
    334375                for (Int_t kk = 0; kk < ii; kk++)       // Fill distances between layers
    335376                {
    336                         //dik(ii, kk) = TMath::Sqrt(pow(rh[ii] - rh[kk], 2) + pow(zh[ii] - zh[kk], 2));
    337                         Double_t Ci = C();
    338                         dik(ii, kk) = (TMath::ASin(Ci*rh[ii])-TMath::ASin(Ci*rh[kk]))/(Ci*snt);
     377                        Int_t kl = hord[kk];            // Unordered layer
     378                        dik(ii, kk) = TMath::Abs(dhh[il] - dhh[kl])/snt;
    339379                        dik(kk, ii) = dik(ii, kk);
    340380                }
    341                 //
    342                 // Store momentum components for resolution correction cosines
    343                 //
    344                 Double_t *pRi = new Double_t[Nhit];
    345                 pRi[ii] = TMath::Abs(pxi * TMath::Cos(phi) + pyi * TMath::Sin(phi)); // Radial component
    346                 Double_t *pPhi = new Double_t[Nhit];
    347                 pPhi[ii] = TMath::Abs(pxi * TMath::Sin(phi) - pyi * TMath::Cos(phi)); // Phi component
    348381        }
    349382        //
    350383        // Fill measurement covariance
    351384        //
    352         Int_t *mTl = new Int_t[mTot];           // Pointer from measurement number to true layer number
     385        TVectorD tPar(5,fpar);
     386        //
    353387        TMatrixDSym Sm(mTot); Sm.Zero();        // Measurement covariance
    354         TMatrixD Rm(mTot, 5);                           // Derivative matrix
    355         Int_t im = 0;
     388        TMatrixD Rm(mTot, 5);                   // Derivative matrix
     389        Int_t im = 0;                                           // Initialize number of measurement counter
    356390        //
    357391        // Fill derivatives and error matrix with MS
    358392        //
    359         Double_t AngMax = 90.; Double_t AngMx = AngMax * TMath::Pi() / 180.;
    360         Double_t csMin = TMath::Cos(AngMx);     // Set maximum angle wrt normal
    361         //
    362393        for (Int_t ii = 0; ii < Nhit; ii++)
    363394        {
    364                 Int_t i = ih[ii];                                       // True layer number
     395                Int_t i = ih[ii];                               // True layer number
    365396                Int_t ityp  = fG->lTyp(i);                      // Layer type Barrel or Z
    366397                Int_t nmeai = fG->lND(i);                       // # measurements in layer
    367                 if (fG->isMeasure(i) && nmeai >0 && cs[ii] > csMin)
    368                 {
    369                         Double_t Di    = D();                           // Get true track parameters
    370                         Double_t phi0i = phi0();
    371                         Double_t Ci    = C();
    372                         Double_t z0i   = z0();
    373                         Double_t cti   = ct();
    374                         //
    375                         Double_t Ri    = rh[ii];
    376                         Double_t ArgRp = (Ri*Ci + (1 + Ci * Di)*Di / Ri) / (1 + 2 * Ci*Di);
    377                         Double_t ArgRz = Ci * TMath::Sqrt((Ri*Ri - Di * Di) / (1 + 2 * Ci*Di));
    378                         TVectorD dRphi(5); dRphi.Zero();                // R-phi derivatives @ const. R
    379                         TVectorD dRz(5); dRz.Zero();                    // z     derivatives @ const. R
    380                         //
    381                         // Derivative overflow protection
    382                         Double_t dMin = 0.8;
    383                         dRphi(0) = (1 - 2 * Ci*Ci*Ri*Ri) /
    384                                 TMath::Max(dMin,TMath::Sqrt(1 - ArgRp * ArgRp));        // D derivative
    385                         dRphi(1) = Ri;                                                                                                          // phi0 derivative
    386                         dRphi(2) = Ri * Ri /
    387                                 TMath::Max(dMin,TMath::Sqrt(1 - ArgRp * ArgRp));                                // C derivative
    388                         dRphi(3) = 0.0;                                                                                         // z0 derivative
    389                         dRphi(4) = 0.0;                                                                                         // cot(theta) derivative
    390                         //
    391                         dRz(0) = -cti * Di /
    392                                 (Ri*TMath::Max(dMin,TMath::Sqrt(1 - Ci * Ci*Ri*Ri)));   // D
    393                         dRz(1) = 0.0;                                                                                           // Phi0
    394                         dRz(2) = cti * (Ri*Ci / TMath::Sqrt(1-Ri*Ri*Ci*Ci) -            // C
    395                                 TMath::ASin(Ri*Ci)) / (Ci*Ci);
    396                         dRz(3) = 1.0;                                                                                           // Z0
    397                         dRz(4) = TMath::ASin(ArgRz) / Ci;                                                       // Cot(theta)
     398               
     399                if (fG->isMeasure(i))
     400                {
     401                        Double_t Ri = rh[ii];
     402                        Double_t zi = zh[ii];
    398403                        //
    399404                        for (Int_t nmi = 0; nmi < nmeai; nmi++)
    400405                        {
    401                                 mTl[im] = i;
    402                                 Double_t stri = 0;
    403                                 Double_t sig = 0;
     406                                Double_t stri = 0;                                              // Stereo angle
     407                                Double_t sig = 0;                                               // Layer resolution
     408                                // Constant R derivatives
     409                                TVectorD dRphi(5); dRphi.Zero();                // R-phi derivatives @ const. R
     410                                TVectorD dRz(5); dRz.Zero();                    // z     derivatives @ const. R
     411                                //
    404412                                if (nmi + 1 == 1)               // Upper layer measurements
    405413                                {
    … …  
    407415                                        Double_t csa = TMath::Cos(stri);
    408416                                        Double_t ssa = TMath::Sin(stri);
     417                                        //
    409418                                        sig = fG->lSgU(i);      // Resolution
    410419                                        if (ityp == 1)          // Barrel type layer (Measure R-phi, stereo or z at const. R)
    411420                                        {
    412421                                                //
    413                                                 // Almost exact solution (CD<<1, D<<R)
     422                                                // Exact solution
     423                                                dRphi = derRphi_R(tPar, Ri);
     424                                                dRz   = derZ_R   (tPar, Ri);
     425                                                //
    414426                                                Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0);      // D derivative
    415427                                                Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1);      // phi0 derivative
    416428                                                Rm(im, 2) = csa * dRphi(2) - ssa * dRz(2);      // C derivative
    417429                                                Rm(im, 3) = csa * dRphi(3) - ssa * dRz(3);      // z0 derivative
    418                                                 Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4);      // cot(theta) derivative
     430                                                Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4);      // cot(theta) derivative       
    419431                                        }
    420                                         if (ityp == 2)          // Z type layer (Measure phi at const. Z)
     432                                        if (ityp == 2)          // Z type layer (Measure R-phi at const. Z)
    421433                                        {
    422                                                 Rm(im, 0) = 1.0;                                        // D derivative
    423                                                 Rm(im, 1) = rh[ii];                                     // phi0 derivative
    424                                                 Rm(im, 2) = rh[ii] * rh[ii];            // C derivative
    425                                                 Rm(im, 3) = 0;                                          // z0 derivative
    426                                                 Rm(im, 4) = 0;                                          // cot(theta) derivative
     434                                                TVectorD dRphz(5); dRphz.Zero();                // R-phi derivatives @ const. z
     435                                                dRphz = derRphi_Z(tPar, zi);
     436                                                //
     437                                                Rm(im, 0) = dRphz(0);                                   // D derivative
     438                                                Rm(im, 1) = dRphz(1);                                   // phi0 derivative
     439                                                Rm(im, 2) = dRphz(2);                                   // C derivative
     440                                                Rm(im, 3) = dRphz(3);                                   // z0 derivative
     441                                                Rm(im, 4) = dRphz(4);                                   // cot(theta) derivative
    427442                                        }
    428443                                }
    … …  
    436451                                        {
    437452                                                //
    438                                                 // Almost exact solution (CD<<1, D<<R)
     453                                                // Exact solution
     454                                                dRphi = derRphi_R(tPar, Ri);
     455                                                dRz   = derZ_R   (tPar, Ri);
     456                                                //
    439457                                                Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0);      // D derivative
    440458                                                Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1);      // phi0 derivative
    … …  
    445463                                        if (ityp == 2)                  // Z type layer (Measure R at const. z)
    446464                                        {
    447                                                 Rm(im, 0) = 0;                                                          // D derivative
    448                                                 Rm(im, 1) = 0;                                                          // phi0 derivative
    449                                                 Rm(im, 2) = 0;                                                          // C derivative
    450                                                 Rm(im, 3) = -1.0 / ct();                                        // z0 derivative
    451                                                 Rm(im, 4) = -rh[ii] / ct();                                     // cot(theta) derivative
     465                                                TVectorD dRRz(5); dRRz.Zero();                  // R     derivatives @ const. z
     466                                                dRRz = derR_Z(tPar, zi);
     467                                                //
     468                                                Rm(im, 0) = dRRz(0);                                    // D derivative
     469                                                Rm(im, 1) = dRRz(1);                                    // phi0 derivative
     470                                                Rm(im, 2) = dRRz(2);                                    // C derivative
     471                                                Rm(im, 3) = dRRz(3);                                    // z0 derivative
     472                                                Rm(im, 4) = dRRz(4);                                    // cot(theta) derivative
    452473                                        }
    453474                                }
    … …  
    457478                                //
    458479                                Int_t km = 0;
     480                                Double_t CosMin = TMath::Sin(TMath::Pi() / 9.); // Protect for derivative explosion
    459481                                for (Int_t kk = 0; kk <= ii; kk++)
    460482                                {
    461                                         Int_t k = ih[kk];                                       // True layer number
    462                                         Int_t ktyp = fG->lTyp(k);                       // Layer type Barrel or
     483                                        Int_t k = ih[kk];                               // True layer number
     484                                        Int_t ktyp = fG->lTyp(k);                       // Layer type Barrel or disk
    463485                                        Int_t nmeak = fG->lND(k);                       // # measurements in layer
    464                                         if (fG->isMeasure(k) && nmeak > 0 &&cs[kk] > csMin)
     486                                        if (fG->isMeasure(k))
    465487                                        {
    466488                                                for (Int_t nmk = 0; nmk < nmeak; nmk++)
    467489                                                {
    468490                                                        Double_t strk = 0;
    469                                                         if (nmk + 1 == 1) strk = fG->lStU(k);   // Stereo angle
    470                                                         if (nmk + 1 == 2) strk = fG->lStL(k);   // Stereo angle
    471                                                         if (im == km && Res) Sm(im, km) += sig*sig;     // Detector resolution on diagonal
     491                                                        if (nmk + 1 == 1) strk = fG->lStU(k);   // Stereo angle upper
     492                                                        if (nmk + 1 == 2) strk = fG->lStL(k);   // Stereo angle lower
     493                                                        //if (im == km && Res) Sm(im, km) += sig*sig;   // Detector resolution on diagonal
     494                                                        if (im == km && Res) {
     495                                                                Double_t sg = sig;
     496                                                                if(TMath::Abs(strk) < TMath::Pi()/6. && cs[kk] < CosMin)
     497                                                                TMath::Min(1000.*sig,sg = sig/pow(cs[kk],4));
     498                                                                Sm(im, km) += sg * sg;  // Detector resolution on diagonal
     499                                                        }
    472500                                                        //
    473501                                                        // Loop on all layers below for MS contributions
    474                                                         for (Int_t jj = 0; jj < kk; jj++)
     502                                                        for (Int_t jj = 0; jj < kk; jj++)               
    475503                                                        {
    476504                                                                Double_t di = dik(ii, jj);
    … …  
    482510                                                                if (ktyp == 1) msk = ms / snt;                  // Barrel
    483511                                                                else if (ktyp == 2) msk = ms / cst;             // Disk
    484                                                                 Double_t ci = TMath::Cos(stri); Double_t si = TMath::Sin(stri);
    485                                                                 Double_t ck = TMath::Cos(strk); Double_t sk = TMath::Sin(strk);
    486                                                                 Sm(im, km) += di*dk*(ci*ck*ms*ms + si*sk*msi*msk);                      // Ms contribution
     512                                                                Double_t ci = TMath::Abs(TMath::Cos(stri)); Double_t si = TMath::Abs(TMath::Sin(stri));
     513                                                                Double_t ck = TMath::Abs(TMath::Cos(strk)); Double_t sk = TMath::Abs(TMath::Sin(strk));
     514                                                                Sm(im, km) += di*dk*(ci*ck*ms*ms + si*sk*msi*msk);      // Ms contribution
    487515                                                        }
    488516                                                        //
    … …  
    497525        }
    498526        Sm.ResizeTo(mTot, mTot);
     527        TMatrixDSym SmTemp = Sm;
    499528        Rm.ResizeTo(mTot, 5);
    500529        //
    … …  
    506535        for (Int_t id = 0; id < mTot; id++) DSmInv(id, id) = 1.0 / TMath::Sqrt(Sm(id, id));
    507536        TMatrixDSym SmN = Sm.Similarity(DSmInv);        // Normalize diagonal to 1
    508         //SmN.Invert();
    509537        //
    510538        // Protected matrix inversions
    511539        //
    512         TDecompChol Chl(SmN);
     540        TDecompChol Chl(SmN,1.e-12);
    513541        TMatrixDSym SmNinv = SmN;
    514542        if (Chl.Decompose())
    … …  
    519547        else
    520548        {
    521                 cout << "SolTrack::CovCalc: Error matrix not positive definite. Recovering ...." << endl;
     549                std::cout << "SolTrack::CovCalc: Error matrix not positive definite. Recovering ...." << std::endl;
     550                //cout << "pt = " << pt() << endl;
    522551                if (ntry < ntrymax)
    523552                {
    … …  
    525554                        ntry++;
    526555                }
     556                //
    527557                TMatrixDSym rSmN = MakePosDef(SmN); SmN = rSmN;
    528558                TDecompChol rChl(SmN);
    … …  
    533563        Sm = SmNinv.Similarity(DSmInv);                 // Error matrix inverted
    534564        TMatrixDSym H = Sm.SimilarityT(Rm);             // Calculate half Hessian
    535         // Normalize before inversion
    536565        const Int_t Npar = 5;
    537566        TMatrixDSym DHinv(Npar); DHinv.Zero();
    … …  
    541570        Hnrm.Invert();
    542571        fCov = Hnrm.Similarity(DHinv);
    543 }
    544 
     572        //
     573        // debug
     574        //
     575        if(TMath::IsNaN(fCov(0,0)))
     576        {
     577                std::cout<<"SolTrack::CovCalc: NaN found in covariance matrix"<<std::endl;
     578        }
     579        //
     580        // Lots of cleanup to do
     581        delete[] zhh;
     582        delete[] rhh;
     583        delete[] dhh;
     584        delete[] ihh;
     585        delete[] hord;
     586        delete[] zh;
     587        delete[] rh;
     588        delete[] ih;
     589        delete[] cs;
     590        delete[] thms;
     591}
     592//
    545593// Force positive definitness in normalized matrix
    546594TMatrixDSym SolTrack::MakePosDef(TMatrixDSym NormMat)
    … …  
    558606        if (Check)
    559607        {
    560                 cout << "SolTrack::MakePosDef: input matrix doesn ot have 1 on diagonal. Abort." << endl;
     608                std::cout << "SolTrack::MakePosDef: input matrix doesn ot have 1 on diagonal. Abort." << std::endl;
    561609                return rMatN;
    562610        }
    … …  
    566614        TMatrixD U = Eign.GetEigenVectors();
    567615        TVectorD lambda = Eign.GetEigenValues();
     616        //cout << "Eigenvalues:"; lambda.Print();
     617        //cout << "Input matrix: "; NormMat.Print();
    568618        // Reset negative eigenvalues to small positive value
    569619        TMatrixDSym D(Size); D.Zero(); Double_t eps = 1.0e-13;
    … …  
    587637                }
    588638        }
     639        //cout << "Rebuilt matrix: "; rMatN.Print();
    589640        return rMatN;
    590641}

  • external/TrackCovariance/SolTrack.h

    r9a7ea36 r0b8551f  
     1//
    12#ifndef G__SOLTRK_H
    23#define G__SOLTRK_H
    3 
     4//
    45#include <TMath.h>
    56#include <TVector3.h>
    67#include <TMatrixDSym.h>
    7 
    8 class SolGeom;
    9 
     8#include "SolGeom.h"
     9#include "TrkUtil.h"
     10#include <TGraph.h>
     11//
     12//
    1013// Class to store track information
    1114// Assumes that the geometry has been initialized
    12 class SolTrack{
    13   // Track handling class
    14   // Assume tracks originate from (0,0) for the time being
     15//
     16class SolTrack: public TrkUtil
     17{
     18        //
     19        // Track handling class
     20        // Assume tracks originate from (0,0) for the time being
     21        //
    1522private:
    16   Int_t fNl;        // Actual number of layers
    17   SolGeom *fG;      // Geometry
    18   Double_t fp[3];   // px, py, pz momentum
    19   Double_t fx[3];   //  x,  y,  z track origin
    20   Double_t fpar[5]; // D, phi0, C, z0, cot(theta)
    21 
    22   TMatrixDSym fCov; // Full covariance matrix
    23 
     23        Int_t fNl;      // Actual number of layers
     24        SolGeom *fG;    // Geometry
     25        Double_t fp[3]; // px, py, pz momentum
     26        Double_t fx[3]; //  x,  y,  z track origin
     27        Double_t fpar[5];       // D, phi0, C, z0, cot(theta)
     28        //
     29        TMatrixDSym fCov;               // Full covariance matrix
     30        //
     31        //
    2432public:
    25   // Constructors
    26   SolTrack(Double_t *x, Double_t *p, SolGeom *G);
     33        //
     34        // Constructors
     35        SolTrack(Double_t *x, Double_t *p, SolGeom *G);
    2736        SolTrack(TVector3 x, TVector3 p, SolGeom* G);
    28   SolTrack(Double_t D, Double_t phi0, Double_t C, Double_t z0, Double_t ct, SolGeom *G);
    29   // Destructor
    30   ~SolTrack();
    31   // Accessors
    32   // Position (at minimum approach)
    33   Double_t x() { return fx[0]; }
    34   Double_t y() { return fx[1]; }
    35   Double_t z() { return fx[2]; }
    36   // Momentum  (at minimum approach)
    37   Double_t px() { return fp[0]; }
    38   Double_t py() { return fp[1]; }
    39   Double_t pz() { return fp[2]; }
    40   Double_t pt() { return TMath::Sqrt(fp[0] * fp[0] + fp[1] * fp[1]); }
    41   Double_t p()  { return TMath::Sqrt(fp[0] * fp[0] + fp[1] * fp[1] + fp[2] * fp[2]); }
    42   // Track parameters
    43   Double_t D()    { return fpar[0]; }
    44   Double_t phi0() { return fpar[1]; }
    45   Double_t C()    { return fpar[2]; }
    46   Double_t z0()   { return fpar[3]; }
    47   Double_t ct()   { return fpar[4]; }
    48   // Covariance
    49   TMatrixDSym Cov()   { return fCov; }
    50   // Track parameter covariance calculation
    51   void CovCalc(Bool_t Res, Bool_t MS);
    52   // Parameter errors
    53   Double_t s_D()    { return TMath::Sqrt(fCov(0, 0)); }
    54   Double_t s_phi0() { return TMath::Sqrt(fCov(1, 1)); }
    55   Double_t s_C()    { return TMath::Sqrt(fCov(2, 2)); }
    56   Double_t s_pt()   { return 2 * s_C()*pt() / (0.2998*fG->B()); } // Dpt/pt
    57   Double_t s_z0()   { return TMath::Sqrt(fCov(3, 3)); }
    58   Double_t s_ct()   { return TMath::Sqrt(fCov(4, 4)); }
    59   // Track hit management
    60   Int_t nHit();
    61   Int_t nmHit();
    62   Bool_t HitLayer(Int_t Layer, Double_t &R, Double_t &phi, Double_t &zz);
    63   Int_t HitList(Int_t *&ihh, Double_t *&rhh, Double_t *&zhh);
    64   Int_t HitListXYZ(Int_t *&ihh, Double_t *&Xh, Double_t *&Yh, Double_t *&Zh);
    65 
    66   // Make normalized matrix positive definite
    67   TMatrixDSym MakePosDef(TMatrixDSym NormMat);
     37        SolTrack(Double_t D, Double_t phi0, Double_t C, Double_t z0, Double_t ct, SolGeom *G);
     38        // Destructor
     39        ~SolTrack();
     40        // Accessors
     41        // Position (at minimum approach)
     42        Double_t x() { return fx[0]; }
     43        Double_t y() { return fx[1]; }
     44        Double_t z() { return fx[2]; }
     45        // Momentum  (at minimum approach)
     46        Double_t px() { return fp[0]; }
     47        Double_t py() { return fp[1]; }
     48        Double_t pz() { return fp[2]; }
     49        Double_t pt() { return TMath::Sqrt(fp[0] * fp[0] + fp[1] * fp[1]); }
     50        Double_t p()  { return TMath::Sqrt(fp[0] * fp[0] + fp[1] * fp[1] + fp[2] * fp[2]); }
     51        // Track parameters
     52        Double_t D()    { return fpar[0]; }
     53        Double_t phi0() { return fpar[1]; }
     54        Double_t C()    { return fpar[2]; }
     55        Double_t z0()   { return fpar[3]; }
     56        Double_t ct()   { return fpar[4]; }
     57        // Covariance
     58        TMatrixDSym Cov()               { return fCov; }
     59        // Track parameter covariance calculation
     60        void CovCalc(Bool_t Res, Bool_t MS);
     61        // Parameter errors
     62        Double_t s_D()    { return TMath::Sqrt(fCov(0, 0)); }
     63        Double_t s_phi0() { return TMath::Sqrt(fCov(1, 1)); }
     64        Double_t s_C()    { return TMath::Sqrt(fCov(2, 2)); }
     65        Double_t s_pt()   { return 2 * s_C()*pt() / (0.2998*fG->B()); } // Dpt/pt
     66        Double_t s_z0()   { return TMath::Sqrt(fCov(3, 3)); }
     67        Double_t s_ct()   { return TMath::Sqrt(fCov(4, 4)); }
     68        //
     69        // Track hit management
     70        Int_t nHit();
     71        Int_t nmHit();
     72        Bool_t HitLayer(Int_t Layer, Double_t &R, Double_t &phi, Double_t &zz);
     73        Int_t HitList(Int_t *&ihh, Double_t *&rhh, Double_t *&zhh);
     74        Int_t HitListXYZ(Int_t *&ihh, Double_t *&Xh, Double_t *&Yh, Double_t *&Zh);
     75        //
     76        // Track graph
     77        TGraph *TrkPlot();      // Graph with R-z plot of track trajectory
     78        //
     79        // Make normalized matrix positive definite
     80        TMatrixDSym MakePosDef(TMatrixDSym NormMat);
    6881};
     82//
    6983#endif

  • external/TrackCovariance/TrkUtil.cc

    r9a7ea36 r0b8551f  
    33#include <algorithm>
    44#include <TSpline.h>
     5#include <TDecompChol.h>
    56
    67// Constructor
    … …  
    3334        fZmin = 0.0;                            // Lower                DCH z
    3435        fZmax = 0.0;                            // Higher       DCH z
     36}
     37//
     38// Distance between two lines
     39//
     40void TrkUtil::LineDistance(TVector3 x0, TVector3 y0, TVector3 dirx, TVector3 diry, Double_t &sx, Double_t &sy, Double_t &distance)
     41{
     42        TMatrixDSym M(2);
     43        M(0,0) = dirx.Mag2();
     44        M(1,1) = diry.Mag2();
     45        M(0,1) = -dirx.Dot(diry);
     46        M(1,0) = M(0,1);
     47        M.Invert();
     48        TVectorD c(2);
     49        c(0) = dirx.Dot(y0-x0);
     50        c(1) = diry.Dot(x0-y0);
     51        TVectorD st = M*c;
     52        //
     53        // Fill output
     54        sx = st(0);
     55        sy = st(1);
     56        //
     57        TVector3 x = x0+sx*dirx;
     58        TVector3 y = y0+sy*diry;
     59        TVector3 d = x-y;
     60        distance = d.Mag();
     61}
     62//
     63// Covariance smearing
     64//
     65TVectorD TrkUtil::CovSmear(TVectorD x, TMatrixDSym C)
     66{
     67        //
     68        // Check arrays
     69        //
     70        // Consistency of dimensions
     71        Int_t Nvec = x.GetNrows();
     72        Int_t Nmat = C.GetNrows();
     73        if (Nvec != Nmat || Nvec == 0)
     74        {
     75                std::cout << "TrkUtil::CovSmear: vector/matrix mismatch. Aborting." << std::endl;
     76                exit(EXIT_FAILURE);
     77        }
     78        // Positive diagonal elements
     79        for (Int_t i = 0; i < Nvec; i++)
     80        {
     81                if (C(i, i) <= 0.0)
     82                {
     83                        std::cout << "TrkUtil::CovSmear: covariance matrix has negative diagonal elements. Aborting." << std::endl;
     84                        exit(EXIT_FAILURE);
     85                }
     86        }
     87        //
     88        // Do a Choleski decomposition and random number extraction, with appropriate stabilization
     89        //
     90        TMatrixDSym CvN = C;
     91        TMatrixDSym DCv(Nvec); DCv.Zero();
     92        TMatrixDSym DCvInv(Nvec); DCvInv.Zero();
     93        for (Int_t id = 0; id < Nvec; id++)
     94        {
     95                Double_t dVal = TMath::Sqrt(C(id, id));
     96                DCv(id, id) = dVal;
     97                DCvInv(id, id) = 1.0 / dVal;
     98        }
     99        CvN.Similarity(DCvInv);                 // Normalize diagonal to 1
     100        TDecompChol Chl(CvN);
     101        Bool_t OK = Chl.Decompose();            // Choleski decomposition of normalized matrix
     102        if (!OK)
     103        {
     104                std::cout << "TrkUtil::CovSmear: covariance matrix is not positive definite. Aborting." << std::endl;
     105                exit(EXIT_FAILURE);
     106        }
     107        TMatrixD U = Chl.GetU();                        // Get Upper triangular matrix
     108        TMatrixD Ut(TMatrixD::kTransposed, U); // Transposed of U (lower triangular)
     109        TVectorD r(Nvec);
     110        for (Int_t i = 0; i < Nvec; i++)r(i) = gRandom->Gaus(0.0, 1.0);         // Array of normal random numbers
     111        TVectorD xOut = x + DCv * (Ut * r);     // Observed parameter vector
     112        //
     113        return xOut;
    35114}
    36115//
    … …  
    48127        Double_t r2 = x(0) * x(0) + x(1) * x(1);
    49128        Double_t cross = x(0) * p(1) - x(1) * p(0);
    50         Double_t T = sqrt(pt * pt - 2 * a * cross + a * a * r2);
    51         Double_t phi0 = atan2((p(1) - a * x(0)) / T, (p(0) + a * x(1)) / T);    // Phi0
     129        Double_t T = TMath::Sqrt(pt * pt - 2 * a * cross + a * a * r2);
     130        Double_t phi0 = TMath::ATan2((p(1) - a * x(0)) / T, (p(0) + a * x(1)) / T);     // Phi0
    52131        Double_t D;                                                     // Impact parameter D
    53132        if (pt < 10.0) D = (T - pt) / a;
    … …  
    58137        Par(2) = C;             // Store C
    59138        //Longitudinal parameters
    60         Double_t B = C * sqrt(TMath::Max(r2 - D * D, 0.0) / (1 + 2 * C * D));
    61         Double_t st = asin(B) / C;
     139        Double_t B = C * TMath::Sqrt(TMath::Max(r2 - D * D, 0.0) / (1 + 2 * C * D));
     140        Double_t st = TMath::ASin(B) / C;
    62141        Double_t ct = p(2) / pt;
    63142        Double_t z0;
    … …  
    235314        //
    236315        return Cmm;
     316}//
     317// Regularized symmetric matrix inversion
     318//
     319TMatrixDSym TrkUtil::RegInv(TMatrixDSym& Min)
     320{
     321        TMatrixDSym M = Min;                            // Decouple from input
     322        Int_t N = M.GetNrows();                 // Matrix size
     323        TMatrixDSym D(N); D.Zero();             // Normaliztion matrix
     324        TMatrixDSym R(N);                               // Normarized matrix
     325        TMatrixDSym Rinv(N);                            // Inverse of R
     326        TMatrixDSym Minv(N);                            // Inverse of M
     327        //
     328        // Check for 0's and normalize
     329        for (Int_t i = 0; i < N; i++)
     330        {
     331                if (M(i, i) != 0.0) D(i, i) = 1. / TMath::Sqrt(TMath::Abs(M(i, i)));
     332                else D(i, i) = 1.0;
     333        }
     334        R = M.Similarity(D);
     335        //
     336        // Recursive algorithms stops when N = 2
     337        //
     338        //****************
     339        // case N = 2  ***
     340        //****************
     341        if (N == 2)
     342        {
     343                Double_t det = R(0, 0) * R(1, 1) - R(0, 1) * R(1, 0);
     344                if (det == 0)
     345                {
     346                        std::cout << "VertexFit::RegInv: null determinant for N = 2" << std::endl;
     347                        Rinv.Zero();    // Return null matrix
     348                }
     349                else
     350                {
     351                        // invert matrix
     352                        Rinv(0, 0) = R(1, 1);
     353                        Rinv(0, 1) = -R(0, 1);
     354                        Rinv(1, 0) = Rinv(0, 1);
     355                        Rinv(1, 1) = R(0, 0);
     356                        Rinv *= 1. / det;
     357                }
     358        }
     359        //****************
     360        // case N > 2  ***
     361        //****************
     362        else
     363        {
     364                // Break up matrix
     365                TMatrixDSym Q = R.GetSub(0, N - 2, 0, N - 2);   // Upper left
     366                TVectorD p(N - 1);
     367                for (Int_t i = 0; i < N - 1; i++)p(i) = R(N - 1, i);
     368                Double_t q = R(N - 1, N - 1);
     369                //Invert pieces and re-assemble
     370                TMatrixDSym Ainv(N - 1);
     371                TMatrixDSym A(N - 1);
     372                if (TMath::Abs(q) > 1.0e-15)
     373                {
     374                        // Case |q| > 0
     375                        Ainv.Rank1Update(p, -1.0 / q);
     376                        Ainv += Q;
     377                        A = RegInv(Ainv);               // Recursive call
     378                        TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
     379                        //
     380                        TVectorD b = (-1.0 / q) * (A * p);
     381                        for (Int_t i = 0; i < N - 1; i++)
     382                        {
     383                                Rinv(N - 1, i) = b(i);
     384                                Rinv(i, N - 1) = b(i);
     385                        }
     386                        //
     387                        Double_t pdotb = 0.;
     388                        for (Int_t i = 0; i < N - 1; i++)pdotb += p(i) * b(i);
     389                        Double_t c = (1.0 - pdotb) / q;
     390                        Rinv(N - 1, N - 1) = c;
     391                }
     392                else
     393                {
     394                        // case q = 0
     395                        TMatrixDSym Qinv = RegInv(Q);           // Recursive call
     396                        Double_t a = Qinv.Similarity(p);
     397                        Double_t c = -1.0 / a;
     398                        Rinv(N - 1, N - 1) = c;
     399                        //
     400                        TVectorD b = (1.0 / a) * (Qinv * p);
     401                        for (Int_t i = 0; i < N - 1; i++)
     402                        {
     403                                Rinv(N - 1, i) = b(i);
     404                                Rinv(i, N - 1) = b(i);
     405                        }
     406                        //
     407                        A.Rank1Update(p, -1 / a);
     408                        A += Q;
     409                        A.Similarity(Qinv);
     410                        TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
     411                }
     412        }
     413        Minv = Rinv.Similarity(D);
     414        return Minv;
     415}
     416//
     417// Track tracjectory
     418//
     419TVector3 TrkUtil::Xtrack(TVectorD par, Double_t s)
     420{
     421        //
     422        // unpack parameters
     423        Double_t D = par(0);
     424        Double_t p0 = par(1);
     425        Double_t C = par(2);
     426        Double_t z0 = par(3);
     427        Double_t ct = par(4);
     428        //
     429        Double_t x = -D * TMath::Sin(p0) + (TMath::Sin(s + p0) - TMath::Sin(p0)) / (2 * C);
     430        Double_t y =  D * TMath::Cos(p0) - (TMath::Cos(s + p0) - TMath::Cos(p0)) / (2 * C);     
     431        Double_t z = z0 + ct * s / (2 * C);
     432        //
     433        TVector3 Xt(x, y, z);
     434        return Xt;
     435}
     436//
     437// Track derivatives
     438//
     439// Constant radius
     440// R-Phi
     441TVectorD TrkUtil::derRphi_R(TVectorD par, Double_t R)
     442{
     443        TVectorD dRphi(5);      // return vector
     444        //
     445        // unpack parameters
     446        Double_t D = par(0);
     447        Double_t C = par(2);
     448        //
     449        Double_t s = 2 * TMath::ASin(C * TMath::Sqrt((R * R - D * D)/(1 + 2 * C * D)));
     450        TVector3 X = Xtrack(par, s);            // Intersection point
     451        TVector3 v(-X.y()/R, X.x()/R, 0.);      // measurement direction
     452        TMatrixD derX = derXdPar(par, s);       // dX/dp
     453        TVectorD derXs = derXds(par, s);        // dX/ds
     454        TVectorD ders = dsdPar_R(par, R);       // ds/dp       
     455        //
     456        for (Int_t i = 0; i < 5; i++)
     457        {
     458                dRphi(i) = 0.;
     459                for (Int_t j = 0; j < 3; j++)
     460                {
     461                        dRphi(i) += v(j) * (derX(j, i) + derXs(j) * ders(i));
     462                }
     463        }
     464        //
     465        return dRphi;
     466}
     467// z
     468TVectorD TrkUtil::derZ_R(TVectorD par, Double_t R)
     469{
     470
     471        TVectorD dZ(5); // return vector
     472        //
     473        // unpack parameters
     474        Double_t D = par(0);
     475        Double_t C = par(2);
     476        //
     477        Double_t s = 2 * TMath::ASin(C * TMath::Sqrt((R * R - D * D)/(1 + 2 * C * D))); // phase
     478        TVector3 v(0., 0., 1.);                         // measurement direction
     479        TMatrixD derX = derXdPar(par, s);       // dX/dp
     480        TVectorD derXs = derXds(par, s);        // dX/ds
     481        TVectorD ders = dsdPar_R(par, R);       // ds/dp       
     482        //
     483        for (Int_t i = 0; i < 5; i++)
     484        {
     485                dZ(i) = 0.;
     486                for (Int_t j = 0; j < 3; j++)
     487                {
     488                        dZ(i) += v(j) * (derX(j, i) + derXs(j) * ders(i));
     489                }
     490        }
     491        //
     492        return dZ;
     493}
     494//
     495// constant z
     496// R-Phi
     497TVectorD TrkUtil::derRphi_Z(TVectorD par, Double_t z)
     498{
     499        TVectorD dRphi(5);      // return vector
     500        //
     501        // unpack parameters
     502        Double_t C = par(2);
     503        Double_t z0 = par(3);
     504        Double_t ct = par(4);
     505        //
     506        Double_t s = 2 * C * (z - z0) / ct;
     507        TVector3 X = Xtrack(par, s);                    // Intersection point
     508        TVector3 v(-X.y() / X.Pt(), X.x() / X.Pt(), 0.);        // measurement direction
     509        TMatrixD derX = derXdPar(par, s);               // dX/dp
     510        TVectorD derXs = derXds(par, s);                // dX/ds
     511        TVectorD ders = dsdPar_z(par, z);               // ds/dp       
     512        //
     513        for (Int_t i = 0; i < 5; i++)
     514        {
     515                dRphi(i) = 0.;
     516                for (Int_t j = 0; j < 3; j++)
     517                {
     518                        dRphi(i) += v(j) * (derX(j, i) + derXs(j) * ders(i));
     519                }
     520        }
     521        //
     522        return dRphi;
     523
     524}
     525// R
     526TVectorD TrkUtil::derR_Z(TVectorD par, Double_t z)
     527{
     528        TVectorD dR(5); // return vector
     529        //
     530        // unpack parameters
     531        Double_t C = par(2);
     532        Double_t z0 = par(3);
     533        Double_t ct = par(4);
     534        //
     535        Double_t s = 2 * C * (z - z0) / ct;
     536        TVector3 X = Xtrack(par, s);                    // Intersection point
     537        TVector3 v(X.x() / X.Pt(), X.y() / X.Pt(), 0.); // measurement direction
     538        TMatrixD derX = derXdPar(par, s);               // dX/dp
     539        TVectorD derXs = derXds(par, s);                // dX/ds
     540        TVectorD ders = dsdPar_z(par, z);       // ds/dp       
     541        //
     542        for (Int_t i = 0; i < 5; i++)
     543        {
     544                dR(i) = 0.;
     545                for (Int_t j = 0; j < 3; j++)
     546                {
     547                        dR(i) += v(j) * (derX(j, i) + derXs(j) * ders(i));
     548                }
     549        }
     550        //
     551        return dR;
     552
     553}
     554//
     555// derivatives of track trajectory
     556//
     557// dX/dPar
     558TMatrixD TrkUtil::derXdPar(TVectorD par, Double_t s)
     559{
     560        TMatrixD dxdp(3, 5);    // return matrix
     561        //
     562        // unpack parameters
     563        Double_t D = par(0);
     564        Double_t p0 = par(1);
     565        Double_t C = par(2);
     566        Double_t z0 = par(3);
     567        Double_t ct = par(4);
     568        //
     569        // derivatives
     570        // dx/dD
     571        dxdp(0, 0) = -TMath::Sin(p0);
     572        dxdp(1, 0) =  TMath::Cos(p0);
     573        dxdp(2, 0) = 0.;
     574        // dx/dphi0
     575        dxdp(0, 1) = -D * TMath::Cos(p0) + (TMath::Cos(s + p0) - TMath::Cos(p0)) / (2 * C);
     576        dxdp(1, 1) = -D * TMath::Sin(p0) + (TMath::Sin(s + p0) - TMath::Sin(p0)) / (2 * C);
     577        dxdp(2, 1) = 0;
     578        // dx/dC
     579        dxdp(0, 2) = -(TMath::Sin(s + p0) - TMath::Sin(p0)) / (2 * C * C);
     580        dxdp(1, 2) =  (TMath::Cos(s + p0) - TMath::Cos(p0)) / (2 * C * C);
     581        dxdp(2, 2) = -ct * s / (2 * C * C);
     582        // dx/dz0
     583        dxdp(0, 3) = 0;
     584        dxdp(1, 3) = 0;
     585        dxdp(2, 3) = 1.;
     586        // dx/dCtg
     587        dxdp(0, 4) = 0;
     588        dxdp(1, 4) = 0;
     589        dxdp(2, 4) = s / (2 * C);
     590        //
     591        return dxdp;
     592}
     593//
     594// dX/ds
     595//
     596TVectorD TrkUtil::derXds(TVectorD par, Double_t s)
     597{
     598        TVectorD dxds(3);       // return vector
     599        //
     600        // unpack parameters
     601        Double_t p0 = par(1);
     602        Double_t C = par(2);
     603        Double_t ct = par(4);
     604        //
     605        // dX/ds
     606        dxds(0) = TMath::Cos(s + p0) / (2 * C);
     607        dxds(1) = TMath::Sin(s + p0) / (2 * C);
     608        dxds(2) = ct / (2 * C);
     609        //
     610        return dxds;
     611}
     612//
     613// derivative of trajectory phase s
     614//Constant R
     615TVectorD TrkUtil::dsdPar_R(TVectorD par, Double_t R)
     616{
     617        TVectorD dsdp(5);       // return vector
     618        //
     619        // unpack parameters
     620        Double_t D = par(0);
     621        Double_t p0 = par(1);
     622        Double_t C = par(2);
     623        //
     624        // derivatives
     625        Double_t opCD = 1. + 2 * C * D;
     626        Double_t A = C*TMath::Sqrt((R*R-D*D)/opCD);
     627        Double_t sqA0 = TMath::Sqrt(1. - A * A);
     628        Double_t dMin = 0.01;
     629        Double_t sqA = TMath::Max(dMin, sqA0);  // Protect against divergence
     630        //
     631        dsdp(0) = -2 * C * C * (D * (1. + C * D) + C * R * R) / (A * sqA * opCD * opCD);
     632        dsdp(1) = 0;
     633        dsdp(2) = 2 * A * (1 + C * D) / (C * sqA * opCD);
     634        dsdp(3) = 0;
     635        dsdp(4) = 0;
     636        //
     637        return dsdp;
     638}
     639// Constant z
     640TVectorD TrkUtil::dsdPar_z(TVectorD par, Double_t z)
     641{
     642        TVectorD dsdp(5);       // return vector
     643        //
     644        // unpack parameters
     645        Double_t C = par(2);
     646        Double_t z0 = par(3);
     647        Double_t ct = par(4);
     648        //
     649        // derivatives
     650        //
     651        dsdp(0) = 0;
     652        dsdp(1) = 0;
     653        dsdp(2) = 2*(z-z0)/ct;
     654        dsdp(3) = -2*C/ct;
     655        dsdp(4) = -2*C*(z-z0)/(ct*ct);
     656        //
     657        return dsdp;
    237658}
    238659//

  • external/TrackCovariance/TrkUtil.h

    r9a7ea36 r0b8551f  
    77#include <TMatrixDSym.h>
    88#include <TRandom.h>
     9#include <TMath.h>
    910//
    1011//
    … …  
    2829        TVectorD XPtoPar(TVector3 x, TVector3 p, Double_t Q);
    2930        TVector3 ParToP(TVectorD Par);
     31        TMatrixDSym RegInv(TMatrixDSym& Min);
     32        //
     33        // Track trajectory derivatives
     34        TMatrixD derXdPar(TVectorD par, Double_t s);    // derivatives of position wrt parameters
     35        TVectorD derXds(TVectorD par, Double_t s);              // derivatives of position wrt phase
     36        TVectorD dsdPar_R(TVectorD par, Double_t R);    // derivatives of phase at constant R
     37        TVectorD dsdPar_z(TVectorD par, Double_t z);    // derivatives of phase at constant z
    3038        //
    3139        // Conversion to ACTS parametrization
    … …  
    5462                Double_t c = 2.99792458e8;      // speed of light m/sec
    5563                //return TMath::C()*1.0e-9;     // Incompatible with root5
    56                 return c*1.0e-9;                // Reduced speed of light       
     64                return c*1.0e-9;                        // Reduced speed of light       
    5765        }
    5866        //
    … …  
    6371        static TVector3 ParToP(TVectorD Par, Double_t Bz);      // Get Momentum from track parameters
    6472        static Double_t ParToQ(TVectorD Par);                           // Get track charge
     73        static void LineDistance(TVector3 x0, TVector3 y0, TVector3 dirx, TVector3 diry, Double_t &sx, Double_t &sy, Double_t &distance);
     74        //
     75        // Track trajectory
     76        //
     77        static TVector3 Xtrack(TVectorD par, Double_t s);               // Parametric track trajectory
     78        TVectorD derRphi_R(TVectorD par, Double_t R);   // Derivatives of R-phi at constant R
     79        TVectorD derZ_R(TVectorD par, Double_t R);              // Derivatives of z at constant R
     80        TVectorD derRphi_Z(TVectorD par, Double_t z);   // Derivatives of R-phi at constant z
     81        TVectorD derR_Z(TVectorD par, Double_t z);              // Derivatives of R at constant z
     82        //
     83        // Smear with given covariance matrix
     84        //
     85        static TVectorD CovSmear(TVectorD x, TMatrixDSym C);
    6586        //
    6687        // Conversion from meters to mm
    … …  
    6889        static TVectorD ParToMm(TVectorD Par);                  // Parameter conversion
    6990        static TMatrixDSym CovToMm(TMatrixDSym Cov);    // Covariance conversion
     91        //
     92        // Inside cylindrical volume
     93        //
     94        static Bool_t IsInside(TVector3 x, Double_t Rout, Double_t Zmin, Double_t Zmax)
     95        {
     96                Bool_t Is = kFALSE;
     97                if (x.Pt() <= Rout && x.z() >= Zmin && x.z() <= Zmax)Is = kTRUE;
     98                return Is;
     99        }
    70100        //
    71101        // Cluster counting in gas

  • modules/TrackCovariance.cc

    r9a7ea36 r0b8551f  
    103103  Double_t mass, p, pt, q, ct;
    104104  Double_t dd0, ddz, dphi, dct, dp, dpt, dC;
     105  //
     106  // Get cylindrical box for fast track simulation
     107  //
     108  Double_t Rin = fGeometry->GetRmin();
     109  Double_t ZinPos = fGeometry->GetZminPos();
     110  Double_t ZinNeg = fGeometry->GetZminNeg();
    105111
    106112  fItInputArray->Reset();
    … …  
    115121    const TLorentzVector &candidateMomentum = particle->Momentum;
    116122
    117     if ( !fCovariance->IsAccepted(candidateMomentum.Vect()) ) continue;
     123    Bool_t inside = TrkUtil::IsInside(candidatePosition.Vect(), Rin, ZinNeg, ZinPos); // Check if in inner box
     124    Bool_t Accept = kTRUE;
     125    if(inside) Accept = fCovariance->IsAccepted(candidateMomentum.Vect());
     126    else       Accept = fCovariance->IsAccepted(candidatePosition.Vect(),candidateMomentum.Vect(), fGeometry);
     127    if(!Accept) continue;
    118128
    119129    mass = candidateMomentum.M();
    120130
    121     ObsTrk track(candidatePosition.Vect(), candidateMomentum.Vect(), candidate->Charge, fBz, fCovariance);
     131    ObsTrk track(candidatePosition.Vect(), candidateMomentum.Vect(), candidate->Charge, fCovariance, fGeometry);
    122132
    123133    mother    = candidate;
Note: See TracChangeset for help on using the changeset viewer.