| 1 | #include "TrkUtil.h"
|
|---|
| 2 | #include <TMath.h>
|
|---|
| 3 | #include <iostream>
|
|---|
| 4 |
|
|---|
| 5 | // Constructor
|
|---|
| 6 | TrkUtil::TrkUtil(Double_t Bz)
|
|---|
| 7 | {
|
|---|
| 8 | fBz = Bz;
|
|---|
| 9 | }
|
|---|
| 10 | TrkUtil::TrkUtil()
|
|---|
| 11 | {
|
|---|
| 12 | fBz = 0.0;
|
|---|
| 13 | }
|
|---|
| 14 | //
|
|---|
| 15 | // Destructor
|
|---|
| 16 | TrkUtil::~TrkUtil()
|
|---|
| 17 | {
|
|---|
| 18 | fBz = 0.0;
|
|---|
| 19 | }
|
|---|
| 20 | //
|
|---|
| 21 | // Helix parameters from position and momentum
|
|---|
| 22 | // static
|
|---|
| 23 | TVectorD TrkUtil::XPtoPar(TVector3 x, TVector3 p, Double_t Q, Double_t Bz)
|
|---|
| 24 | {
|
|---|
| 25 | //
|
|---|
| 26 | TVectorD Par(5);
|
|---|
| 27 | // Transverse parameters
|
|---|
| 28 | Double_t a = -Q * Bz * cSpeed(); // Units are Tesla, GeV and meters
|
|---|
| 29 | Double_t pt = p.Pt();
|
|---|
| 30 | Double_t C = a / (2 * pt); // Half curvature
|
|---|
| 31 | //cout << "ObsTrk::XPtoPar: fB = " << fB << ", a = " << a << ", pt = " << pt << ", C = " << C << endl;
|
|---|
| 32 | Double_t r2 = x.Perp2();
|
|---|
| 33 | Double_t cross = x(0) * p(1) - x(1) * p(0);
|
|---|
| 34 | Double_t T = TMath::Sqrt(pt * pt - 2 * a * cross + a * a * r2);
|
|---|
| 35 | Double_t phi0 = TMath::ATan2((p(1) - a * x(0)) / T, (p(0) + a * x(1)) / T); // Phi0
|
|---|
| 36 | Double_t D; // Impact parameter D
|
|---|
| 37 | if (pt < 10.0) D = (T - pt) / a;
|
|---|
| 38 | else D = (-2 * cross + a * r2) / (T + pt);
|
|---|
| 39 | //
|
|---|
| 40 | Par(0) = D; // Store D
|
|---|
| 41 | Par(1) = phi0; // Store phi0
|
|---|
| 42 | Par(2) = C; // Store C
|
|---|
| 43 | //Longitudinal parameters
|
|---|
| 44 | Double_t B = C * TMath::Sqrt(TMath::Max(r2 - D * D, 0.0) / (1 + 2 * C * D));
|
|---|
| 45 | Double_t st = TMath::ASin(B) / C;
|
|---|
| 46 | Double_t ct = p(2) / pt;
|
|---|
| 47 | Double_t z0 = x(2) - ct * st;
|
|---|
| 48 | //
|
|---|
| 49 | Par(3) = z0; // Store z0
|
|---|
| 50 | Par(4) = ct; // Store cot(theta)
|
|---|
| 51 | //
|
|---|
| 52 | return Par;
|
|---|
| 53 | }
|
|---|
| 54 | // non-static
|
|---|
| 55 | TVectorD TrkUtil::XPtoPar(TVector3 x, TVector3 p, Double_t Q)
|
|---|
| 56 | {
|
|---|
| 57 | //
|
|---|
| 58 | TVectorD Par(5);
|
|---|
| 59 | Double_t Bz = fBz;
|
|---|
| 60 | Par = XPtoPar(x, p, Q, Bz);
|
|---|
| 61 | //
|
|---|
| 62 | return Par;
|
|---|
| 63 | }
|
|---|
| 64 | //
|
|---|
| 65 | TVector3 TrkUtil::ParToX(TVectorD Par)
|
|---|
| 66 | {
|
|---|
| 67 | Double_t D = Par(0);
|
|---|
| 68 | Double_t phi0 = Par(1);
|
|---|
| 69 | Double_t z0 = Par(3);
|
|---|
| 70 | //
|
|---|
| 71 | TVector3 Xval;
|
|---|
| 72 | Xval(0) = -D * TMath::Sin(phi0);
|
|---|
| 73 | Xval(1) = D * TMath::Cos(phi0);
|
|---|
| 74 | Xval(2) = z0;
|
|---|
| 75 | //
|
|---|
| 76 | return Xval;
|
|---|
| 77 | }
|
|---|
| 78 | //
|
|---|
| 79 | TVector3 TrkUtil::ParToP(TVectorD Par)
|
|---|
| 80 | {
|
|---|
| 81 | if (fBz == 0.0)
|
|---|
| 82 | std::cout << "TrkUtil::ParToP: Warning Bz not set" << std::endl;
|
|---|
| 83 | //
|
|---|
| 84 | return ParToP(Par,fBz);
|
|---|
| 85 | }
|
|---|
| 86 | //
|
|---|
| 87 | TVector3 TrkUtil::ParToP(TVectorD Par, Double_t Bz)
|
|---|
| 88 | {
|
|---|
| 89 | Double_t C = Par(2);
|
|---|
| 90 | Double_t phi0 = Par(1);
|
|---|
| 91 | Double_t ct = Par(4);
|
|---|
| 92 | //
|
|---|
| 93 | TVector3 Pval;
|
|---|
| 94 | Double_t pt = Bz * cSpeed() / TMath::Abs(2 * C);
|
|---|
| 95 | Pval(0) = pt * TMath::Cos(phi0);
|
|---|
| 96 | Pval(1) = pt * TMath::Sin(phi0);
|
|---|
| 97 | Pval(2) = pt * ct;
|
|---|
| 98 | //
|
|---|
| 99 | return Pval;
|
|---|
| 100 | }
|
|---|
| 101 | //
|
|---|
| 102 | Double_t TrkUtil::ParToQ(TVectorD Par)
|
|---|
| 103 | {
|
|---|
| 104 | return TMath::Sign(1.0, -Par(2));
|
|---|
| 105 | }
|
|---|
| 106 |
|
|---|
| 107 | //
|
|---|
| 108 | // Parameter conversion to ACTS format
|
|---|
| 109 | TVectorD TrkUtil::ParToACTS(TVectorD Par)
|
|---|
| 110 | {
|
|---|
| 111 | TVectorD pACTS(6); // Return vector
|
|---|
| 112 | //
|
|---|
| 113 | Double_t b = -cSpeed() * fBz / 2.;
|
|---|
| 114 | pACTS(0) = 1000 * Par(0); // D from m to mm
|
|---|
| 115 | pACTS(1) = 1000 * Par(3); // z0 from m to mm
|
|---|
| 116 | pACTS(2) = Par(1); // Phi0 is unchanged
|
|---|
| 117 | pACTS(3) = TMath::ATan2(1.0, Par(4)); // Theta in [0, pi] range
|
|---|
| 118 | pACTS(4) = Par(2) / (b * TMath::Sqrt(1 + Par(4) * Par(4))); // q/p in GeV
|
|---|
| 119 | pACTS(5) = 0.0; // Time: currently undefined
|
|---|
| 120 | //
|
|---|
| 121 | return pACTS;
|
|---|
| 122 | }
|
|---|
| 123 | // Covariance conversion to ACTS format
|
|---|
| 124 | TMatrixDSym TrkUtil::CovToACTS(TVectorD Par, TMatrixDSym Cov)
|
|---|
| 125 | {
|
|---|
| 126 | TMatrixDSym cACTS(6); cACTS.Zero();
|
|---|
| 127 | Double_t b = -cSpeed() * fBz / 2.;
|
|---|
| 128 | //
|
|---|
| 129 | // Fill derivative matrix
|
|---|
| 130 | TMatrixD A(5, 5); A.Zero();
|
|---|
| 131 | Double_t ct = Par(4); // cot(theta)
|
|---|
| 132 | Double_t C = Par(2); // half curvature
|
|---|
| 133 | A(0, 0) = 1000.; // D-D conversion to mm
|
|---|
| 134 | A(1, 2) = 1.0; // phi0-phi0
|
|---|
| 135 | A(2, 4) = 1.0 / (TMath::Sqrt(1.0 + ct * ct) * b); // q/p-C
|
|---|
| 136 | A(3, 1) = 1000.; // z0-z0 conversion to mm
|
|---|
| 137 | A(4, 3) = -1.0 / (1.0 + ct * ct); // theta - cot(theta)
|
|---|
| 138 | A(4, 4) = -C * ct / (b * TMath::Power(1.0 + ct * ct, 3.0 / 2.0)); // q/p-cot(theta)
|
|---|
| 139 | //
|
|---|
| 140 | TMatrixDSym Cv = Cov;
|
|---|
| 141 | TMatrixD At(5, 5);
|
|---|
| 142 | At.Transpose(A);
|
|---|
| 143 | Cv.Similarity(At);
|
|---|
| 144 | TMatrixDSub(cACTS, 0, 4, 0, 4) = Cv;
|
|---|
| 145 | cACTS(5, 5) = 0.1; // Currently undefined: set to arbitrary value to avoid crashes
|
|---|
| 146 | //
|
|---|
| 147 | return cACTS;
|
|---|
| 148 | }
|
|---|
| 149 | //
|
|---|
| 150 | // Parameter conversion to ILC format
|
|---|
| 151 | TVectorD TrkUtil::ParToILC(TVectorD Par)
|
|---|
| 152 | {
|
|---|
| 153 | TVectorD pILC(5); // Return vector
|
|---|
| 154 | //
|
|---|
| 155 | pILC(0) = Par(0) * 1.0e3; // d0 in mm
|
|---|
| 156 | pILC(1) = Par(1); // phi0 is unchanged
|
|---|
| 157 | pILC(2) = -2 * Par(2) * 1.0e-3; // w in mm^-1
|
|---|
| 158 | pILC(3) = Par(3) * 1.0e3; // z0 in mm
|
|---|
| 159 | pILC(4) = Par(4); // tan(lambda) = cot(theta)
|
|---|
| 160 | //
|
|---|
| 161 | return pILC;
|
|---|
| 162 | }
|
|---|
| 163 | // Covariance conversion to ILC format
|
|---|
| 164 | TMatrixDSym TrkUtil::CovToILC(TMatrixDSym Cov)
|
|---|
| 165 | {
|
|---|
| 166 | TMatrixDSym cILC(5); cILC.Zero();
|
|---|
| 167 | //
|
|---|
| 168 | // Fill derivative matrix
|
|---|
| 169 | TMatrixD A(5, 5); A.Zero();
|
|---|
| 170 | //
|
|---|
| 171 | A(0, 0) = 1.0e3; // D-d0 in mm
|
|---|
| 172 | A(1, 1) = 1.0; // phi0-phi0
|
|---|
| 173 | A(2, 2) = -2.0e-3; // w-C
|
|---|
| 174 | A(3, 3) = 1.0e3; // z0-z0 conversion to mm
|
|---|
| 175 | A(4, 4) = 1.0; // tan(lambda) - cot(theta)
|
|---|
| 176 | //
|
|---|
| 177 | TMatrixDSym Cv = Cov;
|
|---|
| 178 | TMatrixD At(5, 5);
|
|---|
| 179 | At.Transpose(A);
|
|---|
| 180 | Cv.Similarity(At);
|
|---|
| 181 | cILC = Cv;
|
|---|
| 182 | //
|
|---|
| 183 | return cILC;
|
|---|
| 184 | }
|
|---|
| 185 | //
|
|---|
| 186 | // Conversion from meters to mm
|
|---|
| 187 | TVectorD TrkUtil::ParToMm(TVectorD Par) // Parameter conversion
|
|---|
| 188 | {
|
|---|
| 189 | TVectorD Pmm(5); // Return vector
|
|---|
| 190 | //
|
|---|
| 191 | Pmm(0) = Par(0) * 1.0e3; // d0 in mm
|
|---|
| 192 | Pmm(1) = Par(1); // phi0 is unchanged
|
|---|
| 193 | Pmm(2) = Par(2) * 1.0e-3; // C in mm^-1
|
|---|
| 194 | Pmm(3) = Par(3) * 1.0e3; // z0 in mm
|
|---|
| 195 | Pmm(4) = Par(4); // tan(lambda) = cot(theta) unchanged
|
|---|
| 196 | //
|
|---|
| 197 | return Pmm;
|
|---|
| 198 | }
|
|---|
| 199 | TMatrixDSym TrkUtil::CovToMm(TMatrixDSym Cov) // Covariance conversion
|
|---|
| 200 | {
|
|---|
| 201 | TMatrixDSym Cmm(5); Cmm.Zero();
|
|---|
| 202 | //
|
|---|
| 203 | // Fill derivative matrix
|
|---|
| 204 | TMatrixD A(5, 5); A.Zero();
|
|---|
| 205 | //
|
|---|
| 206 | A(0, 0) = 1.0e3; // D-d0 in mm
|
|---|
| 207 | A(1, 1) = 1.0; // phi0-phi0
|
|---|
| 208 | A(2, 2) = 1.0e-3; // C-C
|
|---|
| 209 | A(3, 3) = 1.0e3; // z0-z0 conversion to mm
|
|---|
| 210 | A(4, 4) = 1.0; // lambda - cot(theta)
|
|---|
| 211 | //
|
|---|
| 212 | TMatrixDSym Cv = Cov;
|
|---|
| 213 | TMatrixD At(5, 5);
|
|---|
| 214 | At.Transpose(A);
|
|---|
| 215 | Cv.Similarity(At);
|
|---|
| 216 | Cmm = Cv;
|
|---|
| 217 | //
|
|---|
| 218 | return Cmm;
|
|---|
| 219 | }
|
|---|
