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