def myVertex(t1, t2, PosDir):
# closest distance between two tracks
# d = |pq . u x v|/|u x v|
a = ROOT.TVector3(PosDir[t1]['position'](0), PosDir[t1]['position'](1),
PosDir[t1]['position'](2))
u = ROOT.TVector3(PosDir[t1]['direction'](0), PosDir[t1]['direction'](1),
PosDir[t1]['direction'](2))
c = ROOT.TVector3(PosDir[t2]['position'](0), PosDir[t2]['position'](1),
PosDir[t2]['position'](2))
v = ROOT.TVector3(PosDir[t2]['direction'](0), PosDir[t2]['direction'](1),
PosDir[t2]['direction'](2))
pq = a - c
uCrossv = u.Cross(v)
dist = pq.Dot(uCrossv) / (uCrossv.Mag() + 1E-8)
# u.a - u.c + s*|u|**2 - u.v*t = 0
# v.a - v.c + s*v.u - t*|v|**2 = 0
E = u.Dot(a) - u.Dot(c)
F = v.Dot(a) - v.Dot(c)
A, B = u.Mag2(), -u.Dot(v)
C, D = u.Dot(v), -v.Mag2()
t = -(C * E - A * F) / (B * C - A * D)
X = c.x() + v.x() * t
Y = c.y() + v.y() * t
Z = c.z() + v.z() * t
# sT = ROOT.gROOT.FindAnything('cbmsim')
#print 'test2 ',X,Y,Z,dist
#print 'truth',sTree.MCTrack[2].GetStartX(),sTree.MCTrack[2].GetStartY(),sTree.MCTrack[2].GetStartZ()
return X, Y, Z, abs(dist)
def myVertex(t1, t2, PosDir):
# closest distance between two tracks
# d = |pq . u x v|/|u x v|
# print(' ')
# print('myvertex',PosDir)
# print(np.shape(PosDir))
# print(PosDir[0])
# print(PosDir[0][0])
a = ROOT.TVector3(PosDir[0][0](0), PosDir[0][0](1), PosDir[0][0](2))
u = ROOT.TVector3(PosDir[0][1](0), PosDir[0][1](1), PosDir[0][1](2))
c = ROOT.TVector3(PosDir[1][0](0), PosDir[1][0](1), PosDir[1][0](2))
v = ROOT.TVector3(PosDir[1][1](0), PosDir[1][1](1), PosDir[1][1](2))
pq = a - c
uCrossv = u.Cross(v)
dist = pq.Dot(uCrossv) / (uCrossv.Mag() + 1E-8)
# u.a - u.c + s*|u|**2 - u.v*t = 0
# v.a - v.c + s*v.u - t*|v|**2 = 0
E = u.Dot(a) - u.Dot(c)
F = v.Dot(a) - v.Dot(c)
A, B = u.Mag2(), -u.Dot(v)
C, D = u.Dot(v), -v.Mag2()
t = -(C * E - A * F) / (B * C - A * D)
X = c.x() + v.x() * t
Y = c.y() + v.y() * t
Z = c.z() + v.z() * t
return X, Y, Z, abs(dist)
def VertexError(self,t1,t2,PosDir,CovMat=None,scalFac=None):
# with improved Vx x,y resolution
a,u = PosDir[t1]['position'],PosDir[t1]['direction']
c,v = PosDir[t2]['position'],PosDir[t2]['direction']
Vsq = v.Dot(v)
Usq = u.Dot(u)
UV = u.Dot(v)
ca = c-a
denom = Usq*Vsq-UV**2
tmp2 = Vsq*u-UV*v
Va = ca.Dot(tmp2)/denom
tmp2 = UV*u-Usq*v
Vb = ca.Dot(tmp2)/denom
X = (a+c+Va*u+Vb*v) * 0.5
l1 = a - X + u*Va # l2 = c - X + v*Vb
dist = 2. * ROOT.TMath.Sqrt( l1.Dot(l1) )
if not CovMat: return X,dist
T = ROOT.TMatrixD(3,12)
for i in range(3):
for k in range(4):
for j in range(3):
KD = 0
if i==j: KD = 1
if k==0 or k==2:
# cova and covc
temp = ( u[j]*Vsq - v[j]*UV )*u[i] + (u[j]*UV-v[j]*Usq)*v[i]
sign = -1
if k==2 : sign = +1
T[i][3*k+j] = 0.5*( KD + sign*temp/denom )
elif k==1:
# covu
aNAZ = denom*( ca[j]*Vsq-v.Dot(ca)*v[j] )
aZAN = ( ca.Dot(u)*Vsq-ca.Dot(v)*UV )*2*( u[j]*Vsq-v[j]*UV )
bNAZ = denom*( ca[j]*UV+(u.Dot(ca)*v[j]) - 2*ca.Dot(v)*u[j] )
bZAN = ( ca.Dot(u)*UV-ca.Dot(v)*Usq )*2*( u[j]*Vsq-v[j]*UV )
T[i][3*k+j] = 0.5*( Va*KD + u[i]/denom**2*(aNAZ-aZAN) + v[i]/denom**2*(bNAZ-bZAN) )
elif k==3:
# covv
aNAZ = denom*( 2*ca.Dot(u)*v[j] - ca.Dot(v)*u[j] - ca[j]*UV )
aZAN = ( ca.Dot(u)*Vsq-ca.Dot(v)*UV )*2*( v[j]*Usq-u[j]*UV )
bNAZ = denom*( ca.Dot(u)*u[j]-ca[j]*Usq )
bZAN = ( ca.Dot(u)*UV-ca.Dot(v)*Usq )*2*( v[j]*Usq-u[j]*UV )
T[i][3*k+j] = 0.5*(Vb*KD + u[i]/denom**2*(aNAZ-aZAN) + v[i]/denom**2*(bNAZ-bZAN) )
transT = ROOT.TMatrixD(12,3)
transT.Transpose(T)
CovTracks = ROOT.TMatrixD(12,12)
tlist = [t1,t2]
for k in range(2):
for i in range(6):
for j in range(6):
xfac = 1.
if i>2: xfac = scalFac[tlist[k]]
if j>2: xfac = xfac * scalFac[tlist[k]]
CovTracks[i+k*6][j+k*6] = CovMat[tlist[k]][i][j] * xfac
# if i==5 or j==5 : CovMat[tlist[k]][i][j] = 0 # ignore error on z-direction
tmp = ROOT.TMatrixD(3,12)
tmp.Mult(T,CovTracks)
covX = ROOT.TMatrixD(3,3)
covX.Mult(tmp,transT)
return X,covX,dist
