- p.GetXYZ(xyz); pprime.GetXYZ(xyzp);
- Double_t weight = 1;
- weight *= weight;
- fSum[0] += weight;
- fSum[1] += xyz[0]*weight;
- fSum[2] += xyz[1]*weight;
- fSum[3] += xyz[2]*weight;
- fSum[4] += (xyz[0]*xyz[0]+xyz[1]*xyz[1])*weight;
- fSum[5] += (xyz[0]*xyz[0]+xyz[2]*xyz[2])*weight;
- fSum[6] += (xyz[1]*xyz[1]+xyz[2]*xyz[2])*weight;
- fSum[7] -= xyz[0]*xyz[1]*weight;
- fSum[8] -= xyz[0]*xyz[2]*weight;
- fSum[9] -= xyz[1]*xyz[2]*weight;
- fSum[10] += (xyzp[0]-xyz[0])*weight;
- fSum[11] += (xyzp[1]-xyz[1])*weight;
- fSum[12] += (xyzp[2]-xyz[2])*weight;
- fSum[13] += (xyzp[2]*xyz[1]-xyzp[1]*xyz[2])*weight;
- fSum[14] += (xyzp[0]*xyz[2]-xyzp[2]*xyz[0])*weight;
- fSum[15] += (xyzp[1]*xyz[0]-xyzp[0]*xyz[1])*weight;
-
- fSumR += ((xyzp[0]-xyz[0])*(xyzp[0]-xyz[0])+
- (xyzp[1]-xyz[1])*(xyzp[1]-xyz[1])+
- (xyzp[2]-xyz[2])*(xyzp[2]-xyz[2]))*weight;
+ Float_t cov[6],covp[6];
+ p.GetXYZ(xyz,cov); pprime.GetXYZ(xyzp,covp);
+ TMatrixDSym mcov(3);
+ mcov(0,0) = cov[0]; mcov(0,1) = cov[1]; mcov(0,2) = cov[2];
+ mcov(1,0) = cov[1]; mcov(1,1) = cov[3]; mcov(1,2) = cov[4];
+ mcov(2,0) = cov[2]; mcov(2,1) = cov[4]; mcov(2,2) = cov[5];
+ TMatrixDSym mcovp(3);
+ mcovp(0,0) = covp[0]; mcovp(0,1) = covp[1]; mcovp(0,2) = covp[2];
+ mcovp(1,0) = covp[1]; mcovp(1,1) = covp[3]; mcovp(1,2) = covp[4];
+ mcovp(2,0) = covp[2]; mcovp(2,1) = covp[4]; mcovp(2,2) = covp[5];
+ TMatrixDSym msum = mcov + mcovp;
+
+
+ msum.Invert();
+
+
+ if (!msum.IsValid()) return;
+
+ TMatrixD sums(3,1);
+ sums(0,0) = (xyzp[0]-xyz[0]);
+ sums(1,0) = (xyzp[1]-xyz[1]);
+ sums(2,0) = (xyzp[2]-xyz[2]);
+ TMatrixD sumst = sums.T(); sums.T();
+
+ TMatrixD mf(3,6);
+ mf(0,0) = 1; mf(1,0) = 0; mf(2,0) = 0;
+ mf(0,1) = 0; mf(1,1) = 1; mf(2,1) = 0;
+ mf(0,2) = 0; mf(1,2) = 0; mf(2,2) = 1;
+ mf(0,3) = 0; mf(1,3) = -xyz[2]; mf(2,3) = xyz[1];
+ mf(0,4) = xyz[2]; mf(1,4) = 0; mf(2,4) =-xyz[0];
+ mf(0,5) =-xyz[1]; mf(1,5) = xyz[0]; mf(2,5) = 0;
+
+ for(Int_t j=0;j<6;j++){
+ if(fBFixed[j]==kTRUE){
+ mf(0,j)=0.;mf(1,j)=0.;mf(2,j)=0.;
+ }
+ }
+
+ TMatrixD mft = mf.T(); mf.T();
+ TMatrixD sums2 = mft * msum * sums;
+
+ TMatrixD smatrix = mft * msum * mf;
+
+ fSum[0] += smatrix(0,0);
+ fSum[1] += smatrix(0,1);
+ fSum[2] += smatrix(0,2);
+ fSum[3] += smatrix(0,3);
+ fSum[4] += smatrix(0,4);
+ fSum[5] += smatrix(0,5);
+ fSum[6] += smatrix(1,1);
+ fSum[7] += smatrix(1,2);
+ fSum[8] += smatrix(1,3);
+ fSum[9] += smatrix(1,4);
+ fSum[10]+= smatrix(1,5);
+ fSum[11]+= smatrix(2,2);
+ fSum[12]+= smatrix(2,3);
+ fSum[13]+= smatrix(2,4);
+ fSum[14]+= smatrix(2,5);
+ fSum[15]+= smatrix(3,3);
+ fSum[16]+= smatrix(3,4);
+ fSum[17]+= smatrix(3,5);
+ fSum[18]+= smatrix(4,4);
+ fSum[19]+= smatrix(4,5);
+ fSum[20]+= smatrix(5,5);
+ fSum[21] += sums2(0,0);
+ fSum[22] += sums2(1,0);
+ fSum[23] += sums2(2,0);
+ fSum[24] += sums2(3,0);
+ fSum[25] += sums2(4,0);
+ fSum[26] += sums2(5,0);
+
+ TMatrixD tmp = sumst * msum * sums;
+ fSumR += tmp(0,0);