| [ff9fb2d9] | 1 | #include <TMath.h>
|
|---|
| 2 | #include <TVector3.h>
|
|---|
| 3 | #include <TMatrixD.h>
|
|---|
| 4 | #include <TMatrixDSym.h>
|
|---|
| 5 | #include <TDecompChol.h>
|
|---|
| 6 | #include <TRandom.h>
|
|---|
| [942a705] | 7 | #include <iostream>
|
|---|
| [ff9fb2d9] | 8 | #include "SolGridCov.h"
|
|---|
| 9 | #include "ObsTrk.h"
|
|---|
| [942a705] | 10 | //
|
|---|
| 11 | // Constructors
|
|---|
| [ff9fb2d9] | 12 | // x(3) track origin, p(3) track momentum at origin, Q charge, B magnetic field in Tesla
|
|---|
| 13 | ObsTrk::ObsTrk(TVector3 x, TVector3 p, Double_t Q, Double_t B, SolGridCov *GC)
|
|---|
| 14 | {
|
|---|
| [942a705] | 15 | fGC = GC;
|
|---|
| 16 | fGenX = x;
|
|---|
| 17 | fGenP = p;
|
|---|
| 18 | fGenQ = Q;
|
|---|
| 19 | fB = B;
|
|---|
| 20 | fGenPar.ResizeTo(5);
|
|---|
| 21 | fGenParACTS.ResizeTo(6);
|
|---|
| [a0f5d71] | 22 | fGenParILC.ResizeTo(5);
|
|---|
| [942a705] | 23 | fObsPar.ResizeTo(5);
|
|---|
| 24 | fObsParACTS.ResizeTo(6);
|
|---|
| [a0f5d71] | 25 | fObsParILC.ResizeTo(5);
|
|---|
| [942a705] | 26 | fCov.ResizeTo(5, 5);
|
|---|
| 27 | fCovACTS.ResizeTo(6, 6);
|
|---|
| [a0f5d71] | 28 | fCovILC.ResizeTo(5, 5);
|
|---|
| [942a705] | 29 | fGenPar = XPtoPar(x,p,Q);
|
|---|
| 30 | fGenParACTS = ParToACTS(fGenPar);
|
|---|
| [a0f5d71] | 31 | fGenParILC = ParToILC(fGenPar);
|
|---|
| [942a705] | 32 | /*
|
|---|
| 33 | std::cout << "ObsTrk::ObsTrk: fGenPar";
|
|---|
| 34 | for (Int_t i = 0; i < 5; i++)std::cout << fGenPar(i) << ", ";
|
|---|
| 35 | std::cout << std::endl;
|
|---|
| 36 | */
|
|---|
| 37 | fObsPar = GenToObsPar(fGenPar, fGC);
|
|---|
| 38 | fObsParACTS = ParToACTS(fObsPar);
|
|---|
| [a0f5d71] | 39 | fObsParILC = ParToILC(fObsPar);
|
|---|
| [942a705] | 40 | fObsX = ParToX(fObsPar);
|
|---|
| 41 | fObsP = ParToP(fObsPar);
|
|---|
| 42 | fObsQ = ParToQ(fObsPar);
|
|---|
| 43 | fCovACTS = CovToACTS(fCov);
|
|---|
| [a0f5d71] | 44 | fCovILC = CovToILC(fCov);
|
|---|
| [ff9fb2d9] | 45 | }
|
|---|
| [170a11d] | 46 |
|
|---|
| 47 | // x[3] track origin, p[3] track momentum at origin, Q charge, B magnetic field in Tesla
|
|---|
| 48 | ObsTrk::ObsTrk(Double_t *x, Double_t *p, Double_t Q, Double_t B, SolGridCov* GC)
|
|---|
| 49 | {
|
|---|
| 50 | fGC = GC;
|
|---|
| 51 | fGenX.SetXYZ(x[0],x[1],x[2]);
|
|---|
| 52 | fGenP.SetXYZ(p[0],p[1],p[2]);
|
|---|
| 53 | fGenQ = Q;
|
|---|
| 54 | fB = B;
|
|---|
| 55 | fGenPar.ResizeTo(5);
|
|---|
| 56 | fGenParACTS.ResizeTo(6);
|
|---|
| 57 | fGenParILC.ResizeTo(5);
|
|---|
| 58 | fObsPar.ResizeTo(5);
|
|---|
| 59 | fObsParACTS.ResizeTo(6);
|
|---|
| 60 | fObsParILC.ResizeTo(5);
|
|---|
| 61 | fCov.ResizeTo(5, 5);
|
|---|
| 62 | fCovACTS.ResizeTo(6, 6);
|
|---|
| 63 | fCovILC.ResizeTo(5, 5);
|
|---|
| 64 | fGenPar = XPtoPar(fGenX, fGenP, Q);
|
|---|
| 65 | fGenParACTS = ParToACTS(fGenPar);
|
|---|
| 66 | fGenParILC = ParToILC(fGenPar);
|
|---|
| 67 | /*
|
|---|
| 68 | cout << "ObsTrk::ObsTrk: fGenPar";
|
|---|
| 69 | for (Int_t i = 0; i < 5; i++)cout << fGenPar(i) << ", ";
|
|---|
| 70 | cout << endl;
|
|---|
| 71 | */
|
|---|
| 72 | fObsPar = GenToObsPar(fGenPar, fGC);
|
|---|
| 73 | fObsParACTS = ParToACTS(fObsPar);
|
|---|
| 74 | fObsParILC = ParToILC(fObsPar);
|
|---|
| 75 | fObsX = ParToX(fObsPar);
|
|---|
| 76 | fObsP = ParToP(fObsPar);
|
|---|
| 77 | fObsQ = ParToQ(fObsPar);
|
|---|
| 78 | fCovACTS = CovToACTS(fCov);
|
|---|
| 79 | fCovILC = CovToILC(fCov);
|
|---|
| 80 | }
|
|---|
| 81 |
|
|---|
| 82 |
|
|---|
| [942a705] | 83 | //
|
|---|
| 84 | // Destructor
|
|---|
| [ff9fb2d9] | 85 | ObsTrk::~ObsTrk()
|
|---|
| 86 | {
|
|---|
| [942a705] | 87 | fGenX.Clear();
|
|---|
| 88 | fGenP.Clear();
|
|---|
| 89 | fGenPar.Clear();
|
|---|
| 90 | fGenParACTS.Clear();
|
|---|
| 91 | fObsX.Clear();
|
|---|
| 92 | fObsP.Clear();
|
|---|
| 93 | fObsPar.Clear();
|
|---|
| 94 | fObsParACTS.Clear();
|
|---|
| 95 | fCov.Clear();
|
|---|
| 96 | fCovACTS.Clear();
|
|---|
| [ff9fb2d9] | 97 | }
|
|---|
| 98 | TVectorD ObsTrk::XPtoPar(TVector3 x, TVector3 p, Double_t Q)
|
|---|
| 99 | {
|
|---|
| [942a705] | 100 | //
|
|---|
| 101 | TVectorD Par(5);
|
|---|
| 102 | // Transverse parameters
|
|---|
| 103 | Double_t a = -Q*fB*0.2998; // Units are Tesla, GeV and meters
|
|---|
| 104 | Double_t pt = p.Pt();
|
|---|
| 105 | Double_t C = a / (2 * pt); // Half curvature
|
|---|
| 106 | //std::cout << "ObsTrk::XPtoPar: fB = " << fB << ", a = " << a << ", pt = " << pt << ", C = " << C << std::endl;
|
|---|
| 107 | Double_t r2 = x.Perp2();
|
|---|
| 108 | Double_t cross = x(0)*p(1) - x(1)*p(0);
|
|---|
| 109 | Double_t T = TMath::Sqrt(pt*pt - 2 * a*cross + a*a*r2);
|
|---|
| 110 | Double_t phi0 = TMath::ATan2((p(1) - a*x(0)) / T, (p(0) + a*x(1)) / T); // Phi0
|
|---|
| 111 | Double_t D; // Impact parameter D
|
|---|
| 112 | if (pt < 10.0) D = (T - pt) / a;
|
|---|
| 113 | else D = (-2 * cross + a*r2) / (T + pt);
|
|---|
| 114 | //
|
|---|
| 115 | Par(0) = D; // Store D
|
|---|
| 116 | Par(1) = phi0; // Store phi0
|
|---|
| 117 | Par(2) = C; // Store C
|
|---|
| 118 | //Longitudinal parameters
|
|---|
| 119 | Double_t B = C*TMath::Sqrt(TMath::Max(r2 - D*D,0.0) / (1 + 2 * C*D));
|
|---|
| 120 | Double_t st = TMath::ASin(B) / C;
|
|---|
| 121 | Double_t ct = p(2) / pt;
|
|---|
| 122 | Double_t z0 = x(2) - ct*st;
|
|---|
| 123 | //
|
|---|
| 124 | Par(3) = z0; // Store z0
|
|---|
| 125 | Par(4) = ct; // Store cot(theta)
|
|---|
| 126 | //
|
|---|
| 127 | return Par;
|
|---|
| [ff9fb2d9] | 128 | }
|
|---|
| [942a705] | 129 | //
|
|---|
| [ff9fb2d9] | 130 | TVector3 ObsTrk::ParToX(TVectorD Par)
|
|---|
| 131 | {
|
|---|
| [942a705] | 132 | Double_t D = Par(0);
|
|---|
| 133 | Double_t phi0 = Par(1);
|
|---|
| 134 | Double_t z0 = Par(3);
|
|---|
| 135 | //
|
|---|
| 136 | TVector3 Xval;
|
|---|
| [170a11d] | 137 | Xval(0) = -D*TMath::Sin(phi0);
|
|---|
| 138 | Xval(1) = D*TMath::Cos(phi0);
|
|---|
| [942a705] | 139 | Xval(2) = z0;
|
|---|
| 140 | //
|
|---|
| 141 | return Xval;
|
|---|
| [ff9fb2d9] | 142 | }
|
|---|
| [942a705] | 143 | //
|
|---|
| [ff9fb2d9] | 144 | TVector3 ObsTrk::ParToP(TVectorD Par)
|
|---|
| 145 | {
|
|---|
| [942a705] | 146 | Double_t C = Par(2);
|
|---|
| 147 | Double_t phi0 = Par(1);
|
|---|
| 148 | Double_t ct = Par(4);
|
|---|
| 149 | //
|
|---|
| 150 | TVector3 Pval;
|
|---|
| 151 | Double_t pt = fB*0.2998 / TMath::Abs(2 * C);
|
|---|
| 152 | Pval(0) = pt*TMath::Cos(phi0);
|
|---|
| 153 | Pval(1) = pt*TMath::Sin(phi0);
|
|---|
| 154 | Pval(2) = pt*ct;
|
|---|
| 155 | //
|
|---|
| 156 | return Pval;
|
|---|
| [ff9fb2d9] | 157 | }
|
|---|
| [942a705] | 158 | //
|
|---|
| [ff9fb2d9] | 159 |
|
|---|
| 160 | Double_t ObsTrk::ParToQ(TVectorD Par)
|
|---|
| 161 | {
|
|---|
| [942a705] | 162 | return TMath::Sign(1.0, -Par(2));
|
|---|
| [ff9fb2d9] | 163 | }
|
|---|
| [942a705] | 164 | //
|
|---|
| [ff9fb2d9] | 165 | TVectorD ObsTrk::GenToObsPar(TVectorD gPar, SolGridCov *GC)
|
|---|
| 166 | {
|
|---|
| [942a705] | 167 | TVector3 p = ParToP(gPar);
|
|---|
| 168 | Double_t pt = p.Pt();
|
|---|
| 169 | Double_t tanTh = 1.0 / TMath::Abs(gPar(4));
|
|---|
| 170 | Double_t angd = TMath::ATan(tanTh)*180. / TMath::Pi();
|
|---|
| 171 | //
|
|---|
| 172 | // Check ranges
|
|---|
| 173 | Double_t minPt = GC->GetMinPt ();
|
|---|
| [170a11d] | 174 | //if (pt < minPt) std::cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is below grid range of " << minPt << std::endl;
|
|---|
| [942a705] | 175 | Double_t maxPt = GC->GetMaxPt();
|
|---|
| [170a11d] | 176 | //if (pt > maxPt) std::cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is above grid range of " << maxPt << std::endl;
|
|---|
| [942a705] | 177 | Double_t minAn = GC->GetMinAng();
|
|---|
| [170a11d] | 178 | //if (angd < minAn) std::cout << "Warning ObsTrk::GenToObsPar: angle " << angd
|
|---|
| 179 | // << " is below grid range of " << minAn << std::endl;
|
|---|
| [942a705] | 180 | Double_t maxAn = GC->GetMaxAng();
|
|---|
| [170a11d] | 181 | //if (angd > maxAn) std::cout << "Warning ObsTrk::GenToObsPar: angle " << angd
|
|---|
| 182 | // << " is above grid range of " << maxAn << std::endl;
|
|---|
| [942a705] | 183 | //
|
|---|
| 184 | TMatrixDSym Cov = GC->GetCov(pt, angd);
|
|---|
| 185 | fCov = Cov;
|
|---|
| 186 | //
|
|---|
| 187 | // Now do Choleski decomposition and random number extraction, with appropriate stabilization
|
|---|
| 188 | //
|
|---|
| 189 | TMatrixDSym CvN = Cov;
|
|---|
| 190 | TMatrixDSym DCv(5); DCv.Zero();
|
|---|
| 191 | TMatrixDSym DCvInv(5); DCvInv.Zero();
|
|---|
| 192 | for (Int_t id = 0; id < 5; id++)
|
|---|
| 193 | {
|
|---|
| 194 | Double_t dVal = TMath::Sqrt(Cov(id, id));
|
|---|
| 195 | DCv (id, id) = dVal;
|
|---|
| 196 | DCvInv(id, id) = 1.0 / dVal;
|
|---|
| 197 | }
|
|---|
| 198 | CvN.Similarity(DCvInv); // Normalize diagonal to 1
|
|---|
| 199 | TDecompChol Chl(CvN);
|
|---|
| 200 | Bool_t OK = Chl.Decompose(); // Choleski decomposition of normalized matrix
|
|---|
| 201 | TMatrixD U = Chl.GetU(); // Get Upper triangular matrix
|
|---|
| 202 | TMatrixD Ut(TMatrixD::kTransposed, U); // Transposed of U (lower triangular)
|
|---|
| 203 | TVectorD r(5);
|
|---|
| 204 | for (Int_t i = 0; i < 5; i++)r(i) = gRandom->Gaus(0.0, 1.0); // Array of normal random numbers
|
|---|
| 205 | TVectorD oPar = gPar + DCv*(Ut*r); // Observed parameter vector
|
|---|
| 206 | //
|
|---|
| 207 | return oPar;
|
|---|
| 208 | }
|
|---|
| 209 | // Parameter conversion to ACTS format
|
|---|
| 210 | TVectorD ObsTrk::ParToACTS(TVectorD Par)
|
|---|
| 211 | {
|
|---|
| 212 | TVectorD pACTS(6); // Return vector
|
|---|
| 213 | //
|
|---|
| 214 | Double_t b = -0.29988*fB / 2.;
|
|---|
| 215 | pACTS(0) = 1000*Par(0); // D from m to mm
|
|---|
| 216 | pACTS(1) = 1000 * Par(3); // z0 from m to mm
|
|---|
| 217 | pACTS(2) = Par(1); // Phi0 is unchanged
|
|---|
| [170a11d] | 218 | pACTS(3) = TMath::ATan2(1.0,Par(4)); // Theta in [0, pi] range
|
|---|
| [942a705] | 219 | pACTS(4) = Par(2) / (b*TMath::Sqrt(1 + Par(4)*Par(4))); // q/p in GeV
|
|---|
| 220 | pACTS(5) = 0.0; // Time: currently undefined
|
|---|
| 221 | //
|
|---|
| 222 | return pACTS;
|
|---|
| 223 | }
|
|---|
| 224 | // Covariance conversion to ACTS format
|
|---|
| 225 | TMatrixDSym ObsTrk::CovToACTS(TMatrixDSym Cov)
|
|---|
| 226 | {
|
|---|
| 227 | TMatrixDSym cACTS(6); cACTS.Zero();
|
|---|
| 228 | Double_t b = -0.29988*fB / 2.;
|
|---|
| 229 | //
|
|---|
| 230 | // Fill derivative matrix
|
|---|
| 231 | TMatrixD A(5, 5); A.Zero();
|
|---|
| 232 | Double_t ct = fGenPar(4); // cot(theta)
|
|---|
| 233 | Double_t C = fGenPar(2); // half curvature
|
|---|
| 234 | A(0, 0) = 1000.; // D-D conversion to mm
|
|---|
| 235 | A(1, 2) = 1.0; // phi0-phi0
|
|---|
| 236 | A(2, 4) = 1.0/(TMath::Sqrt(1.0 + ct*ct) * b); // q/p-C
|
|---|
| 237 | A(3, 1) = 1000.; // z0-z0 conversion to mm
|
|---|
| 238 | A(4, 3) = -1.0 / (1.0 + ct*ct); // theta - cot(theta)
|
|---|
| 239 | A(4, 4) = -C*ct / (b*pow(1.0 + ct*ct,3.0/2.0)); // q/p-cot(theta)
|
|---|
| 240 | //
|
|---|
| 241 | TMatrixDSym Cv = Cov;
|
|---|
| 242 | TMatrixD At(5, 5);
|
|---|
| 243 | At.Transpose(A);
|
|---|
| 244 | Cv.Similarity(At);
|
|---|
| 245 | TMatrixDSub(cACTS, 0, 4, 0, 4) = Cv;
|
|---|
| 246 | cACTS(5, 5) = 0.1; // Currently undefined: set to arbitrary value to avoid crashes
|
|---|
| 247 | //
|
|---|
| 248 | return cACTS;
|
|---|
| [ff9fb2d9] | 249 | }
|
|---|
| [942a705] | 250 |
|
|---|
| [a0f5d71] | 251 | // Parameter conversion to ILC format
|
|---|
| 252 | TVectorD ObsTrk::ParToILC(TVectorD Par)
|
|---|
| 253 | {
|
|---|
| 254 | TVectorD pILC(5); // Return vector
|
|---|
| 255 | //
|
|---|
| 256 | pILC(0) = Par(0)*1.0e3; // d0 in mm
|
|---|
| 257 | pILC(1) = Par(1); // phi0 is unchanged
|
|---|
| 258 | pILC(2) = -2 * Par(2)*1.0e-3; // w in mm^-1
|
|---|
| 259 | pILC(3) = Par(3)*1.0e3; // z0 in mm
|
|---|
| 260 | pILC(4) = Par(4); // tan(lambda) = cot(theta)
|
|---|
| 261 | //
|
|---|
| 262 | return pILC;
|
|---|
| 263 | }
|
|---|
| 264 | // Covariance conversion to ILC format
|
|---|
| 265 | TMatrixDSym ObsTrk::CovToILC(TMatrixDSym Cov)
|
|---|
| 266 | {
|
|---|
| 267 | TMatrixDSym cILC(5); cILC.Zero();
|
|---|
| 268 | //
|
|---|
| 269 | // Fill derivative matrix
|
|---|
| 270 | TMatrixD A(5, 5); A.Zero();
|
|---|
| 271 | //
|
|---|
| 272 | A(0, 0) = 1.0e3; // D-d0 in mm
|
|---|
| 273 | A(1, 1) = 1.0; // phi0-phi0
|
|---|
| 274 | A(2, 2) = -2.0e-3; // w-C
|
|---|
| 275 | A(3, 3) = 1.0e3; // z0-z0 conversion to mm
|
|---|
| 276 | A(4, 4) = 1.0; // tan(lambda) - cot(theta)
|
|---|
| 277 | //
|
|---|
| 278 | TMatrixDSym Cv = Cov;
|
|---|
| 279 | TMatrixD At(5, 5);
|
|---|
| 280 | At.Transpose(A);
|
|---|
| 281 | Cv.Similarity(At);
|
|---|
| 282 | cILC = Cv;
|
|---|
| 283 | //
|
|---|
| 284 | return cILC;
|
|---|
| 285 | }
|
|---|
