[537d24f] | 1 | #include <TMath.h>
|
---|
| 2 | #include <TVectorD.h>
|
---|
| 3 | #include <TVector3.h>
|
---|
| 4 | #include <TMatrixD.h>
|
---|
| 5 | #include <TMatrixDSym.h>
|
---|
| 6 | #include "VertexFit.h"
|
---|
| 7 | //
|
---|
| 8 | // Constructors
|
---|
| 9 | //
|
---|
[82db145] | 10 | //
|
---|
| 11 | // Empty construction (to be used when adding tracks later with AddTrk() )
|
---|
[537d24f] | 12 | VertexFit::VertexFit()
|
---|
| 13 | {
|
---|
| 14 | fNtr = 0;
|
---|
[82db145] | 15 | fRold = -1.0;
|
---|
[537d24f] | 16 | fVtxDone = kFALSE;
|
---|
| 17 | fVtxCst = kFALSE;
|
---|
| 18 | fxCst.ResizeTo(3);
|
---|
[91ef0b8] | 19 | fCovCst.ResizeTo(3, 3);
|
---|
[537d24f] | 20 | fXv.ResizeTo(3);
|
---|
[91ef0b8] | 21 | fcovXv.ResizeTo(3, 3);
|
---|
[537d24f] | 22 | }
|
---|
[82db145] | 23 | //
|
---|
| 24 | // Build from list of parameters and covariances
|
---|
[537d24f] | 25 | VertexFit::VertexFit(Int_t Ntr, TVectorD** trkPar, TMatrixDSym** trkCov)
|
---|
| 26 | {
|
---|
| 27 | fNtr = Ntr;
|
---|
[82db145] | 28 | fRold = -1.0;
|
---|
[537d24f] | 29 | fVtxDone = kFALSE;
|
---|
| 30 | fVtxCst = kFALSE;
|
---|
| 31 | fxCst.ResizeTo(3);
|
---|
[91ef0b8] | 32 | fCovCst.ResizeTo(3, 3);
|
---|
[537d24f] | 33 | fXv.ResizeTo(3);
|
---|
[91ef0b8] | 34 | fcovXv.ResizeTo(3, 3);
|
---|
[537d24f] | 35 | //
|
---|
[82db145] | 36 | for (Int_t i = 0; i < fNtr; i++)
|
---|
| 37 | {
|
---|
| 38 | TVectorD pr = *trkPar[i];
|
---|
| 39 | fPar.push_back(new TVectorD(pr));
|
---|
| 40 | TMatrixDSym cv = *trkCov[i];
|
---|
| 41 | fCov.push_back(new TMatrixDSym(cv));
|
---|
| 42 | }
|
---|
| 43 | fChi2List.ResizeTo(fNtr);
|
---|
| 44 | //
|
---|
[537d24f] | 45 | }
|
---|
[82db145] | 46 | //
|
---|
| 47 | // Build from ObsTrk list of tracks
|
---|
[537d24f] | 48 | VertexFit::VertexFit(Int_t Ntr, ObsTrk** track)
|
---|
| 49 | {
|
---|
| 50 | fNtr = Ntr;
|
---|
[82db145] | 51 | fRold = -1.0;
|
---|
[537d24f] | 52 | fVtxDone = kFALSE;
|
---|
| 53 | fVtxCst = kFALSE;
|
---|
| 54 | fxCst.ResizeTo(3);
|
---|
[91ef0b8] | 55 | fCovCst.ResizeTo(3, 3);
|
---|
[537d24f] | 56 | fXv.ResizeTo(3);
|
---|
[91ef0b8] | 57 | fcovXv.ResizeTo(3, 3);
|
---|
[537d24f] | 58 | //
|
---|
[82db145] | 59 | fChi2List.ResizeTo(fNtr);
|
---|
| 60 | for (Int_t i = 0; i < fNtr; i++)
|
---|
[537d24f] | 61 | {
|
---|
[82db145] | 62 | fPar.push_back(new TVectorD(track[i]->GetObsPar()));
|
---|
| 63 | fCov.push_back(new TMatrixDSym(track[i]->GetCov()));
|
---|
[537d24f] | 64 | }
|
---|
| 65 | }
|
---|
| 66 | //
|
---|
| 67 | // Destructor
|
---|
[82db145] | 68 | //
|
---|
| 69 | void VertexFit::ResetWrkArrays()
|
---|
| 70 | {
|
---|
| 71 | Int_t N = (Int_t)ffi.size();
|
---|
| 72 | for (Int_t i = 0; i < N; i++)
|
---|
| 73 | {
|
---|
| 74 | if (fx0i[i]) { fx0i[i]->Clear(); delete fx0i[i]; }
|
---|
| 75 | if (fai[i]) { fai[i]->Clear(); delete fai[i]; }
|
---|
| 76 | if (fDi[i]) { fDi[i]->Clear(); delete fDi[i]; }
|
---|
| 77 | if (fWi[i]) { fWi[i]->Clear(); delete fWi[i]; }
|
---|
| 78 | if (fWinvi[i]){ fWinvi[i]->Clear(); delete fWinvi[i]; }
|
---|
| 79 | }
|
---|
| 80 | fa2i.clear();
|
---|
| 81 | fx0i.clear();
|
---|
| 82 | fai.clear();
|
---|
| 83 | fDi.clear();
|
---|
| 84 | fWi.clear();
|
---|
| 85 | fWinvi.clear();
|
---|
| 86 | }
|
---|
[537d24f] | 87 | VertexFit::~VertexFit()
|
---|
| 88 | {
|
---|
[82db145] | 89 | fxCst.Clear();
|
---|
| 90 | fCovCst.Clear();
|
---|
| 91 | fXv.Clear();
|
---|
| 92 | fcovXv.Clear();
|
---|
| 93 | fChi2List.Clear();
|
---|
[537d24f] | 94 | //
|
---|
[91ef0b8] | 95 | for (Int_t i = 0; i < fNtr; i++)
|
---|
[537d24f] | 96 | {
|
---|
[82db145] | 97 | fPar[i]->Clear(); delete fPar[i];
|
---|
| 98 | fCov[i]->Clear(); delete fCov[i];
|
---|
[537d24f] | 99 | }
|
---|
[82db145] | 100 | fPar.clear();
|
---|
| 101 | fCov.clear();
|
---|
| 102 | //
|
---|
| 103 | ResetWrkArrays();
|
---|
| 104 | ffi.clear();
|
---|
[537d24f] | 105 | fNtr = 0;
|
---|
| 106 | }
|
---|
| 107 | //
|
---|
[82db145] | 108 | Double_t VertexFit::FastRv(TVectorD p1, TVectorD p2)
|
---|
[537d24f] | 109 | {
|
---|
| 110 | //
|
---|
[82db145] | 111 | // Find radius of minimum distance between two tracks
|
---|
[537d24f] | 112 | //
|
---|
| 113 | // p = (D,phi, C, z0, ct)
|
---|
| 114 | //
|
---|
| 115 | // Define arrays
|
---|
| 116 | //
|
---|
[82db145] | 117 | Double_t C1 = p1(2);
|
---|
| 118 | Double_t C2 = p2(2);
|
---|
| 119 | Double_t ph1 = p1(1);
|
---|
| 120 | Double_t ph2 = p2(1);
|
---|
[537d24f] | 121 | TVectorD x0 = Fill_x0(p1);
|
---|
| 122 | TVectorD y0 = Fill_x0(p2);
|
---|
| 123 | TVectorD n = Fill_a(p1, 0.0);
|
---|
[82db145] | 124 | n *= (2*C1);
|
---|
[537d24f] | 125 | TVectorD k = Fill_a(p2, 0.0);
|
---|
[82db145] | 126 | k *= (2*C2);
|
---|
[537d24f] | 127 | //
|
---|
| 128 | // Setup and solve linear system
|
---|
| 129 | //
|
---|
| 130 | Double_t nn = 0; for (Int_t i = 0; i < 3; i++)nn += n(i) * n(i);
|
---|
| 131 | Double_t nk = 0; for (Int_t i = 0; i < 3; i++)nk += n(i) * k(i);
|
---|
| 132 | Double_t kk = 0; for (Int_t i = 0; i < 3; i++)kk += k(i) * k(i);
|
---|
| 133 | Double_t discr = nn * kk - nk * nk;
|
---|
| 134 | TMatrixDSym H(2);
|
---|
| 135 | H(0, 0) = kk;
|
---|
| 136 | H(0, 1) = nk;
|
---|
| 137 | H(1, 0) = H(0, 1);
|
---|
| 138 | H(1, 1) = nn;
|
---|
| 139 | TVectorD c(2);
|
---|
[91ef0b8] | 140 | c(0) = 0; for (Int_t i = 0; i < 3; i++)c(0) += n(i) * (y0(i) - x0(i));
|
---|
[537d24f] | 141 | c(1) = 0; for (Int_t i = 0; i < 3; i++)c(1) += -k(i) * (y0(i) - x0(i));
|
---|
| 142 | TVectorD smin = (H * c);
|
---|
| 143 | smin *= 1.0 / discr;
|
---|
| 144 | //
|
---|
| 145 | TVectorD X = x0 + smin(0) * n;
|
---|
| 146 | TVectorD Y = y0 + smin(1) * k;
|
---|
| 147 | //
|
---|
[82db145] | 148 | // Higher order corrections
|
---|
| 149 | X(0) += -C1 * smin(0) * smin(0) * TMath::Sin(ph1);
|
---|
| 150 | X(1) += C1 * smin(0) * smin(0) * TMath::Cos(ph1);
|
---|
| 151 | Y(0) += -C2 * smin(1) * smin(1) * TMath::Sin(ph2);
|
---|
| 152 | Y(1) += C2 * smin(1) * smin(1) * TMath::Cos(ph2);
|
---|
| 153 | //
|
---|
| 154 | TVectorD Xavg = 0.5 * (X + Y);
|
---|
| 155 | //
|
---|
| 156 | //
|
---|
| 157 | return TMath::Sqrt(Xavg(0)*Xavg(0)+Xavg(1)*Xavg(1));
|
---|
[537d24f] | 158 | }
|
---|
[82db145] | 159 | //
|
---|
| 160 | // Starting radius determination
|
---|
| 161 | Double_t VertexFit::StartRadius()
|
---|
[537d24f] | 162 | {
|
---|
| 163 | //
|
---|
[82db145] | 164 | // Maximum impact parameter
|
---|
| 165 | Double_t Rd = 0;
|
---|
| 166 | for (Int_t i = 0; i < fNtr; i++)
|
---|
| 167 | {
|
---|
| 168 | TVectorD par = *fPar[i];
|
---|
| 169 | Double_t Dabs = TMath::Abs(par(0));
|
---|
| 170 | if (Dabs > Rd)Rd = Dabs;
|
---|
| 171 | }
|
---|
| 172 | //-----------------------------
|
---|
[537d24f] | 173 | //
|
---|
[82db145] | 174 | // Find track pair with phi difference closest to pi/2
|
---|
| 175 | Int_t isel = 0; Int_t jsel = 0; // selected track indices
|
---|
| 176 | Double_t dSinMax = 0.0; // Max phi difference
|
---|
| 177 | for (Int_t i = 0; i < fNtr - 1; i++)
|
---|
[537d24f] | 178 | {
|
---|
[82db145] | 179 | TVectorD pari = *fPar[i];
|
---|
| 180 | Double_t phi1 = pari(1);
|
---|
| 181 |
|
---|
| 182 | for (Int_t j = i + 1; j < fNtr; j++)
|
---|
[537d24f] | 183 | {
|
---|
[82db145] | 184 | TVectorD parj = *fPar[j];
|
---|
| 185 | Double_t phi2 = parj(1);
|
---|
| 186 | Double_t Sindphi = TMath::Abs(TMath::Sin(phi2 - phi1));
|
---|
| 187 | if (Sindphi > dSinMax)
|
---|
| 188 | {
|
---|
| 189 | isel = i; jsel = j;
|
---|
| 190 | dSinMax = Sindphi;
|
---|
| 191 | }
|
---|
[537d24f] | 192 | }
|
---|
| 193 | }
|
---|
| 194 | //
|
---|
[82db145] | 195 | //------------------------------------------
|
---|
| 196 | //
|
---|
| 197 | // Find radius of minimum distrance between tracks
|
---|
| 198 | TVectorD p1 = *fPar[isel];
|
---|
| 199 | TVectorD p2 = *fPar[jsel];
|
---|
| 200 | Double_t R = FastRv(p1, p2);
|
---|
| 201 | //
|
---|
| 202 | R = 0.9 * R + 0.1 * Rd; // Protect for overshoot
|
---|
| 203 | //
|
---|
[537d24f] | 204 | return R;
|
---|
| 205 | }
|
---|
[82db145] | 206 | //
|
---|
| 207 | // Regularized symmetric matrix inversion
|
---|
| 208 | //
|
---|
| 209 | TMatrixDSym VertexFit::RegInv(TMatrixDSym& Min)
|
---|
[537d24f] | 210 | {
|
---|
[82db145] | 211 | TMatrixDSym M = Min; // Decouple from input
|
---|
| 212 | Int_t N = M.GetNrows(); // Matrix size
|
---|
| 213 | TMatrixDSym D(N); D.Zero(); // Normaliztion matrix
|
---|
| 214 | TMatrixDSym R(N); // Normarized matrix
|
---|
| 215 | TMatrixDSym Rinv(N); // Inverse of R
|
---|
| 216 | TMatrixDSym Minv(N); // Inverse of M
|
---|
| 217 | //
|
---|
| 218 | // Check for 0's and normalize
|
---|
| 219 | for (Int_t i = 0; i < N; i++)
|
---|
[537d24f] | 220 | {
|
---|
[82db145] | 221 | if (M(i, i) != 0.0) D(i, i) = 1. / TMath::Sqrt(TMath::Abs(M(i, i)));
|
---|
| 222 | else D(i, i) = 1.0;
|
---|
[537d24f] | 223 | }
|
---|
[82db145] | 224 | R = M.Similarity(D);
|
---|
| 225 | //
|
---|
| 226 | // Recursive algorithms stops when N = 2
|
---|
| 227 | //
|
---|
| 228 | //****************
|
---|
| 229 | // case N = 2 ***
|
---|
| 230 | //****************
|
---|
| 231 | if (N == 2)
|
---|
[537d24f] | 232 | {
|
---|
[82db145] | 233 | Double_t det = R(0, 0) * R(1, 1) - R(0, 1) * R(1, 0);
|
---|
| 234 | if (det == 0)
|
---|
[537d24f] | 235 | {
|
---|
[82db145] | 236 | std::cout << "VertexFit::RegInv: null determinant for N = 2" << std::endl;
|
---|
| 237 | Rinv.Zero(); // Return null matrix
|
---|
[537d24f] | 238 | }
|
---|
[82db145] | 239 | else
|
---|
[537d24f] | 240 | {
|
---|
[82db145] | 241 | // invert matrix
|
---|
| 242 | Rinv(0, 0) = R(1, 1);
|
---|
| 243 | Rinv(0, 1) = -R(0, 1);
|
---|
| 244 | Rinv(1, 0) = Rinv(0, 1);
|
---|
| 245 | Rinv(1, 1) = R(0, 0);
|
---|
| 246 | Rinv *= 1. / det;
|
---|
[537d24f] | 247 | }
|
---|
| 248 | }
|
---|
[82db145] | 249 | //****************
|
---|
| 250 | // case N > 2 ***
|
---|
| 251 | //****************
|
---|
[537d24f] | 252 | else
|
---|
| 253 | {
|
---|
[82db145] | 254 | // Break up matrix
|
---|
| 255 | TMatrixDSym Q = R.GetSub(0, N - 2, 0, N - 2); // Upper left
|
---|
| 256 | TVectorD p(N - 1);
|
---|
| 257 | for (Int_t i = 0; i < N - 1; i++)p(i) = R(N - 1, i);
|
---|
| 258 | Double_t q = R(N - 1, N - 1);
|
---|
| 259 | //Invert pieces and re-assemble
|
---|
| 260 | TMatrixDSym Ainv(N - 1);
|
---|
| 261 | TMatrixDSym A(N - 1);
|
---|
| 262 | if (TMath::Abs(q) > 1.0e-15)
|
---|
| 263 | {
|
---|
| 264 | // Case |q| > 0
|
---|
| 265 | Ainv.Rank1Update(p, -1.0 / q);
|
---|
| 266 | Ainv += Q;
|
---|
| 267 | A = RegInv(Ainv); // Recursive call
|
---|
| 268 | TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
|
---|
| 269 | //
|
---|
| 270 | TVectorD b = (-1.0 / q) * (A * p);
|
---|
| 271 | for (Int_t i = 0; i < N - 1; i++)
|
---|
| 272 | {
|
---|
| 273 | Rinv(N - 1, i) = b(i);
|
---|
| 274 | Rinv(i, N - 1) = b(i);
|
---|
| 275 | }
|
---|
| 276 | //
|
---|
| 277 | Double_t pdotb = 0.;
|
---|
| 278 | for (Int_t i = 0; i < N - 1; i++)pdotb += p(i) * b(i);
|
---|
| 279 | Double_t c = (1.0 - pdotb) / q;
|
---|
| 280 | Rinv(N - 1, N - 1) = c;
|
---|
| 281 | }
|
---|
| 282 | else
|
---|
| 283 | {
|
---|
| 284 | // case q = 0
|
---|
| 285 | TMatrixDSym Qinv = RegInv(Q); // Recursive call
|
---|
| 286 | Double_t a = Qinv.Similarity(p);
|
---|
| 287 | Double_t c = -1.0 / a;
|
---|
| 288 | Rinv(N - 1, N - 1) = c;
|
---|
| 289 | //
|
---|
| 290 | TVectorD b = (1.0 / a) * (Qinv * p);
|
---|
| 291 | for (Int_t i = 0; i < N - 1; i++)
|
---|
| 292 | {
|
---|
| 293 | Rinv(N - 1, i) = b(i);
|
---|
| 294 | Rinv(i, N - 1) = b(i);
|
---|
| 295 | }
|
---|
| 296 | //
|
---|
| 297 | A.Rank1Update(p, -1 / a);
|
---|
| 298 | A += Q;
|
---|
| 299 | A.Similarity(Qinv);
|
---|
| 300 | TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
|
---|
| 301 | }
|
---|
[537d24f] | 302 | }
|
---|
[82db145] | 303 | Minv = Rinv.Similarity(D);
|
---|
| 304 | return Minv;
|
---|
[537d24f] | 305 | }
|
---|
| 306 | //
|
---|
| 307 | //
|
---|
| 308 | //
|
---|
| 309 | TMatrixD VertexFit::Fill_A(TVectorD par, Double_t phi)
|
---|
| 310 | {
|
---|
| 311 | //
|
---|
| 312 | // Derivative of track 3D position vector with respect to track parameters at constant phase
|
---|
| 313 | //
|
---|
| 314 | // par = vector of track parameters
|
---|
| 315 | // phi = phase
|
---|
| 316 | //
|
---|
| 317 | TMatrixD A(3, 5);
|
---|
| 318 | //
|
---|
| 319 | // Decode input arrays
|
---|
| 320 | //
|
---|
| 321 | Double_t D = par(0);
|
---|
| 322 | Double_t p0 = par(1);
|
---|
| 323 | Double_t C = par(2);
|
---|
| 324 | Double_t z0 = par(3);
|
---|
| 325 | Double_t ct = par(4);
|
---|
| 326 | //
|
---|
| 327 | // Fill derivative matrix dx/d alpha
|
---|
| 328 | // D
|
---|
| 329 | A(0, 0) = -TMath::Sin(p0);
|
---|
| 330 | A(1, 0) = TMath::Cos(p0);
|
---|
| 331 | A(2, 0) = 0.0;
|
---|
| 332 | // phi0
|
---|
[91ef0b8] | 333 | A(0, 1) = -D * TMath::Cos(p0) + (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
|
---|
| 334 | A(1, 1) = -D * TMath::Sin(p0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
|
---|
[537d24f] | 335 | A(2, 1) = 0.0;
|
---|
| 336 | // C
|
---|
[91ef0b8] | 337 | A(0, 2) = -(TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C * C);
|
---|
| 338 | A(1, 2) = (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C * C);
|
---|
| 339 | A(2, 2) = -ct * phi / (2 * C * C);
|
---|
[537d24f] | 340 | // z0
|
---|
| 341 | A(0, 3) = 0.0;
|
---|
| 342 | A(1, 3) = 0.0;
|
---|
| 343 | A(2, 3) = 1.0;
|
---|
| 344 | // ct = lambda
|
---|
| 345 | A(0, 4) = 0.0;
|
---|
| 346 | A(1, 4) = 0.0;
|
---|
| 347 | A(2, 4) = phi / (2 * C);
|
---|
| 348 | //
|
---|
| 349 | return A;
|
---|
| 350 | }
|
---|
| 351 | //
|
---|
| 352 | TVectorD VertexFit::Fill_a(TVectorD par, Double_t phi)
|
---|
| 353 | {
|
---|
| 354 | //
|
---|
| 355 | // Derivative of track 3D position vector with respect to phase at constant track parameters
|
---|
| 356 | //
|
---|
| 357 | // par = vector of track parameters
|
---|
| 358 | // phi = phase
|
---|
| 359 | //
|
---|
| 360 | TVectorD a(3);
|
---|
| 361 | //
|
---|
| 362 | // Decode input arrays
|
---|
| 363 | //
|
---|
| 364 | Double_t D = par(0);
|
---|
| 365 | Double_t p0 = par(1);
|
---|
| 366 | Double_t C = par(2);
|
---|
| 367 | Double_t z0 = par(3);
|
---|
| 368 | Double_t ct = par(4);
|
---|
| 369 | //
|
---|
| 370 | a(0) = TMath::Cos(phi + p0) / (2 * C);
|
---|
| 371 | a(1) = TMath::Sin(phi + p0) / (2 * C);
|
---|
| 372 | a(2) = ct / (2 * C);
|
---|
| 373 | //
|
---|
| 374 | return a;
|
---|
| 375 | }
|
---|
| 376 | //
|
---|
| 377 | TVectorD VertexFit::Fill_x0(TVectorD par)
|
---|
| 378 | {
|
---|
| 379 | //
|
---|
| 380 | // Calculate track 3D position at R = |D| (minimum approach to z-axis)
|
---|
| 381 | //
|
---|
| 382 | TVectorD x0(3);
|
---|
| 383 | //
|
---|
| 384 | // Decode input arrays
|
---|
| 385 | //
|
---|
| 386 | Double_t D = par(0);
|
---|
| 387 | Double_t p0 = par(1);
|
---|
| 388 | Double_t C = par(2);
|
---|
| 389 | Double_t z0 = par(3);
|
---|
| 390 | Double_t ct = par(4);
|
---|
| 391 | //
|
---|
[91ef0b8] | 392 | x0(0) = -D * TMath::Sin(p0);
|
---|
| 393 | x0(1) = D * TMath::Cos(p0);
|
---|
[537d24f] | 394 | x0(2) = z0;
|
---|
| 395 | //
|
---|
| 396 | return x0;
|
---|
| 397 | }
|
---|
| 398 | //
|
---|
| 399 | TVectorD VertexFit::Fill_x(TVectorD par, Double_t phi)
|
---|
| 400 | {
|
---|
| 401 | //
|
---|
| 402 | // Calculate track 3D position for a given phase, phi
|
---|
| 403 | //
|
---|
| 404 | TVectorD x(3);
|
---|
| 405 | //
|
---|
| 406 | // Decode input arrays
|
---|
| 407 | //
|
---|
| 408 | Double_t D = par(0);
|
---|
| 409 | Double_t p0 = par(1);
|
---|
| 410 | Double_t C = par(2);
|
---|
| 411 | Double_t z0 = par(3);
|
---|
| 412 | Double_t ct = par(4);
|
---|
| 413 | //
|
---|
| 414 | TVectorD x0 = Fill_x0(par);
|
---|
| 415 | x(0) = x0(0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
|
---|
| 416 | x(1) = x0(1) - (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
|
---|
[91ef0b8] | 417 | x(2) = x0(2) + ct * phi / (2 * C);
|
---|
[537d24f] | 418 | //
|
---|
| 419 | return x;
|
---|
| 420 | }
|
---|
| 421 | //
|
---|
[82db145] | 422 | void VertexFit::UpdateTrkArrays(Int_t i)
|
---|
[537d24f] | 423 | {
|
---|
| 424 | //
|
---|
[82db145] | 425 | // Get track parameters and their covariance
|
---|
| 426 | TVectorD par = *fPar[i];
|
---|
| 427 | TMatrixDSym Cov = *fCov[i];
|
---|
[537d24f] | 428 | //
|
---|
[82db145] | 429 | // Fill all track related work arrays arrays
|
---|
| 430 | Double_t fs = ffi[i]; // Get phase
|
---|
| 431 | TVectorD xs = Fill_x(par, fs);
|
---|
| 432 | fx0i.push_back(new TVectorD(xs)); // Start helix position
|
---|
[537d24f] | 433 | //
|
---|
[82db145] | 434 | TMatrixD A = Fill_A(par, fs); // A = dx/da = derivatives wrt track parameters
|
---|
| 435 | TMatrixDSym Winv = Cov.Similarity(A); // W^-1 = A*C*A'
|
---|
| 436 | fWinvi.push_back(new TMatrixDSym(Winv)); // Store W^-1 matrix
|
---|
[537d24f] | 437 | //
|
---|
[82db145] | 438 | TMatrixDSym W = RegInv(Winv); // W = (A*C*A')^-1
|
---|
| 439 | fWi.push_back(new TMatrixDSym(W)); // Store W matrix
|
---|
[537d24f] | 440 | //
|
---|
[82db145] | 441 | TVectorD a = Fill_a(par, fs); // a = dx/ds = derivatives wrt phase
|
---|
| 442 | fai.push_back(new TVectorD(a)); // Store a
|
---|
| 443 | //
|
---|
| 444 | Double_t a2 = W.Similarity(a);
|
---|
| 445 | fa2i.push_back(a2); // Store a2
|
---|
| 446 | //
|
---|
| 447 | // Build D matrix
|
---|
| 448 | TMatrixDSym B(3);
|
---|
| 449 | B.Rank1Update(a, -1. / a2);
|
---|
| 450 | B.Similarity(W);
|
---|
| 451 | TMatrixDSym Ds = W + B; // D matrix
|
---|
| 452 | fDi.push_back(new TMatrixDSym(Ds)); // Store D matrix
|
---|
| 453 | }
|
---|
| 454 | //
|
---|
| 455 | void VertexFit::VertexFitter()
|
---|
| 456 | {
|
---|
| 457 | //std::cout << "VertexFitter: just in" << std::endl;
|
---|
| 458 | if (fNtr < 2)
|
---|
[537d24f] | 459 | {
|
---|
[82db145] | 460 | std::cout << "VertexFit::VertexFitter - Method called with less than 2 tracks - Aborting " << std::endl;
|
---|
| 461 | std::exit(1);
|
---|
[537d24f] | 462 | }
|
---|
| 463 | //
|
---|
[82db145] | 464 | // Vertex fit
|
---|
| 465 | //
|
---|
| 466 | // Initial variable definitions
|
---|
| 467 | TVectorD x(3);
|
---|
| 468 | TMatrixDSym covX(3);
|
---|
| 469 | Double_t Chi2 = 0;
|
---|
| 470 | TVectorD x0 = fXv; // If previous fit done
|
---|
| 471 | if (fRold < 0.0)for (Int_t i = 0; i < 3; i++)x0(i) = 1000.; // Set to arbitrary large value if not
|
---|
| 472 | //
|
---|
| 473 | // Starting vertex radius approximation
|
---|
| 474 | //
|
---|
| 475 | Double_t R = fRold; // Use previous fit if available
|
---|
| 476 | if (R < 0.0) R = StartRadius(); // Rough vertex estimate
|
---|
[537d24f] | 477 | //
|
---|
| 478 | // Iteration properties
|
---|
| 479 | //
|
---|
| 480 | Int_t Ntry = 0;
|
---|
| 481 | Int_t TryMax = 100;
|
---|
| 482 | Double_t eps = 1.0e-9; // vertex stability
|
---|
| 483 | Double_t epsi = 1000.;
|
---|
| 484 | //
|
---|
[82db145] | 485 | // Iteration loop
|
---|
[537d24f] | 486 | while (epsi > eps && Ntry < TryMax) // Iterate until found vertex is stable
|
---|
| 487 | {
|
---|
[82db145] | 488 | // Initialize arrays
|
---|
[537d24f] | 489 | x.Zero();
|
---|
| 490 | TVectorD cterm(3); TMatrixDSym H(3); TMatrixDSym DW1D(3);
|
---|
[82db145] | 491 | covX.Zero(); // Reset vertex covariance
|
---|
[537d24f] | 492 | cterm.Zero(); // Reset constant term
|
---|
| 493 | H.Zero(); // Reset H matrix
|
---|
| 494 | DW1D.Zero();
|
---|
[127644a] | 495 | //
|
---|
[82db145] | 496 | // Reset work arrays
|
---|
| 497 | //
|
---|
| 498 | ResetWrkArrays();
|
---|
| 499 | //
|
---|
| 500 | // Start loop on tracks
|
---|
| 501 | //
|
---|
[537d24f] | 502 | for (Int_t i = 0; i < fNtr; i++)
|
---|
| 503 | {
|
---|
[127644a] | 504 | // Get track helix parameters and their covariance matrix
|
---|
[537d24f] | 505 | TVectorD par = *fPar[i];
|
---|
[127644a] | 506 | TMatrixDSym Cov = *fCov[i];
|
---|
[82db145] | 507 | //
|
---|
| 508 | // For first iteration only
|
---|
[537d24f] | 509 | Double_t fs;
|
---|
| 510 | if (Ntry <= 0) // Initialize all phases on first pass
|
---|
| 511 | {
|
---|
| 512 | Double_t D = par(0);
|
---|
| 513 | Double_t C = par(2);
|
---|
[91ef0b8] | 514 | Double_t arg = TMath::Max(1.0e-6, (R * R - D * D) / (1 + 2 * C * D));
|
---|
| 515 | fs = 2 * TMath::ASin(C * TMath::Sqrt(arg));
|
---|
[82db145] | 516 | ffi.push_back(fs);
|
---|
[537d24f] | 517 | }
|
---|
| 518 | //
|
---|
[82db145] | 519 | // Update track related arrays
|
---|
[537d24f] | 520 | //
|
---|
[82db145] | 521 | UpdateTrkArrays(i);
|
---|
| 522 | TMatrixDSym Ds = *fDi[i];
|
---|
| 523 | TMatrixDSym Winv = *fWinvi[i];
|
---|
[537d24f] | 524 | TMatrixDSym DsW1Ds = Winv.Similarity(Ds); // Service matrix to calculate covX
|
---|
[82db145] | 525 | //
|
---|
| 526 | // Update global arrays
|
---|
[127644a] | 527 | DW1D += DsW1Ds;
|
---|
[537d24f] | 528 | // Update hessian
|
---|
| 529 | H += Ds;
|
---|
| 530 | // update constant term
|
---|
[82db145] | 531 | TVectorD xs = *fx0i[i];
|
---|
[537d24f] | 532 | cterm += Ds * xs;
|
---|
| 533 | } // End loop on tracks
|
---|
| 534 | //
|
---|
| 535 | // update vertex position
|
---|
[82db145] | 536 | TMatrixDSym H1 = RegInv(H);
|
---|
[91ef0b8] | 537 | x = H1 * cterm;
|
---|
[82db145] | 538 | //
|
---|
[537d24f] | 539 | // Update vertex covariance
|
---|
| 540 | covX = DW1D.Similarity(H1);
|
---|
[82db145] | 541 | //
|
---|
[537d24f] | 542 | // Update phases and chi^2
|
---|
| 543 | Chi2 = 0.0;
|
---|
| 544 | for (Int_t i = 0; i < fNtr; i++)
|
---|
| 545 | {
|
---|
[82db145] | 546 | TVectorD lambda = (*fDi[i]) * (*fx0i[i] - x);
|
---|
| 547 | TMatrixDSym Wm1 = *fWinvi[i];
|
---|
[537d24f] | 548 | fChi2List(i) = Wm1.Similarity(lambda);
|
---|
| 549 | Chi2 += fChi2List(i);
|
---|
[82db145] | 550 | TVectorD a = *fai[i];
|
---|
| 551 | TVectorD b = (*fWi[i]) * (x - (*fx0i[i]));
|
---|
| 552 | for (Int_t j = 0; j < 3; j++)ffi[i] += a(j) * b(j) / fa2i[i];
|
---|
[537d24f] | 553 | }
|
---|
| 554 | //
|
---|
| 555 | TVectorD dx = x - x0;
|
---|
| 556 | x0 = x;
|
---|
| 557 | // update vertex stability
|
---|
[82db145] | 558 | TMatrixDSym Hess = RegInv(covX);
|
---|
[537d24f] | 559 | epsi = Hess.Similarity(dx);
|
---|
| 560 | Ntry++;
|
---|
| 561 | //
|
---|
| 562 | // Store result
|
---|
| 563 | //
|
---|
[82db145] | 564 | fXv = x; // Vertex position
|
---|
[537d24f] | 565 | fcovXv = covX; // Vertex covariance
|
---|
| 566 | fChi2 = Chi2; // Vertex fit Chi2
|
---|
[82db145] | 567 | } // end of iteration loop
|
---|
[537d24f] | 568 | //
|
---|
[82db145] | 569 | fVtxDone = kTRUE; // Set fit completion flag
|
---|
| 570 | fRold = TMath::Sqrt(fXv(0)*fXv(0) + fXv(1)*fXv(1)); // Store fit radius
|
---|
[537d24f] | 571 | //
|
---|
| 572 | }
|
---|
| 573 | //
|
---|
[82db145] | 574 | // Return fit vertex
|
---|
[537d24f] | 575 | TVectorD VertexFit::GetVtx()
|
---|
| 576 | {
|
---|
[82db145] | 577 | if (!fVtxDone) VertexFitter();
|
---|
[537d24f] | 578 | return fXv;
|
---|
| 579 | }
|
---|
[82db145] | 580 | //
|
---|
| 581 | // Return fit vertex covariance
|
---|
[537d24f] | 582 | TMatrixDSym VertexFit::GetVtxCov()
|
---|
| 583 | {
|
---|
[82db145] | 584 | if (!fVtxDone) VertexFitter();
|
---|
[537d24f] | 585 | return fcovXv;
|
---|
| 586 | }
|
---|
[82db145] | 587 | //
|
---|
| 588 | // Return fit vertex chi2
|
---|
[537d24f] | 589 | Double_t VertexFit::GetVtxChi2()
|
---|
| 590 | {
|
---|
[82db145] | 591 | if (!fVtxDone) VertexFitter();
|
---|
[537d24f] | 592 | return fChi2;
|
---|
| 593 | }
|
---|
[82db145] | 594 | //
|
---|
| 595 | // Return array of chi2 contributions from each track
|
---|
[537d24f] | 596 | TVectorD VertexFit::GetVtxChi2List()
|
---|
| 597 | {
|
---|
[82db145] | 598 | if (!fVtxDone) VertexFitter();
|
---|
[537d24f] | 599 | return fChi2List;
|
---|
| 600 | }
|
---|
| 601 | //
|
---|
| 602 | // Handle tracks/constraints
|
---|
| 603 | void VertexFit::AddVtxConstraint(TVectorD xv, TMatrixDSym cov) // Add gaussian vertex constraint
|
---|
| 604 | {
|
---|
[127644a] | 605 | std::cout << "VertexFit::AddVtxConstraint: Not implemented yet" << std::endl;
|
---|
[537d24f] | 606 | }
|
---|
| 607 | //
|
---|
[82db145] | 608 | // Adding tracks one by one
|
---|
| 609 | void VertexFit::AddTrk(TVectorD *par, TMatrixDSym *Cov) // Add track to input list
|
---|
[537d24f] | 610 | {
|
---|
[82db145] | 611 | fNtr++;
|
---|
| 612 | fChi2List.ResizeTo(fNtr); // Resize chi2 array
|
---|
| 613 | fPar.push_back(par); // add new track
|
---|
| 614 | fCov.push_back(Cov);
|
---|
| 615 | //
|
---|
| 616 | // Reset previous vertex temp arrays
|
---|
| 617 | ResetWrkArrays();
|
---|
| 618 | ffi.clear();
|
---|
| 619 | fVtxDone = kFALSE; // Reset vertex done flag
|
---|
[537d24f] | 620 | }
|
---|
[82db145] | 621 | //
|
---|
| 622 | // Removing tracks one by one
|
---|
| 623 | void VertexFit::RemoveTrk(Int_t iTrk) // Remove iTrk track
|
---|
[537d24f] | 624 | {
|
---|
[82db145] | 625 | fNtr--;
|
---|
| 626 | fChi2List.Clear();
|
---|
| 627 | fChi2List.ResizeTo(fNtr); // Resize chi2 array
|
---|
| 628 | fPar.erase(fPar.begin() + iTrk); // Remove track
|
---|
| 629 | fCov.erase(fCov.begin() + iTrk);
|
---|
| 630 | //
|
---|
| 631 | // Reset previous vertex temp arrays
|
---|
| 632 | ResetWrkArrays();
|
---|
| 633 | ffi.clear();
|
---|
| 634 | fVtxDone = kFALSE; // Reset vertex done flag
|
---|
[537d24f] | 635 | }
|
---|