ROOT logo
//*-- Author : M. Sanchez
//*-- Modified : 07.03.2001 (M. Sanchez)

#include  "hgeomvertexfit2.h"
#include <iostream>

//_HADES_CLASS_DESCRIPTION 
////////////////////////////////////////////////
//HGeomVertexFit
//
//  Calculates the point of maximun approach to any given
//set of n tracks with user specified weights.
//
//  To start the operation the function reset() is called, then
//addLine() should be used on all tracks on the sample and 
//finally getVertex() will provide the desired point.
//
//  Note that getVertex() is not destructive, you can continue
//adding lines after getVertex() was called
////////////////////////////////////////////////

HGeomVertexFit2::HGeomVertexFit2(void)  {
  // The default constructor
}

HGeomVertexFit2::~HGeomVertexFit2(void) {
  // Everything that is constructed has to be destructed
}

void HGeomVertexFit2::addLine(const HGeomVector &r, const HGeomVector &alpha,
			  HSymMat4 &cov,Float_t zv,const Double_t w) {
  // Function to add lines to the fit.
  // Input
  //   r --> A point in the straight line
  //   alpha --> Direction vector
  //   w --> weight of this line in the fit
  //   cov --> Covariance matrix with elements
  //   		sigma(x, y, x', y')
  //   zv --> Stimation of z coord. of vertex
  Float_t sx = alpha.getX() / alpha.getZ();
  Float_t sy = alpha.getY() / alpha.getZ();
  Float_t x0 = r.getX() - r.getZ()*sx;
  Float_t y0 = r.getY() - r.getZ()*sy;

  //Extrapolate covariance matrix
  Float_t cov_x_x = cov(0,0) - alpha.getZ()*(2*cov(0,2) - alpha.getZ()*cov(2,2));
  Float_t cov_y_y = cov(1,1) - alpha.getZ()*(2*cov(1,3) - alpha.getZ()*cov(3,3));
  Float_t cov_sx_sx = cov(2,2);
  Float_t cov_sy_sy = cov(3,3);
  Float_t cov_x_y = cov(0,1) - alpha.getZ()*(cov(1,2) + cov(0,3) - alpha.getZ()*cov(2,3));
  Float_t cov_sx_sy = cov(2,3);
  Float_t cov_x_sx = cov(0,2) - alpha.getZ()*cov(2,2);
  Float_t cov_x_sy = cov(0,3) - alpha.getZ()*cov(2,3);
  Float_t cov_y_sx = cov(1,2) - alpha.getZ()*cov(2,3);
  Float_t cov_y_sy = cov(1,3) - alpha.getZ()*cov(3,3);

  //Covariance matrix for vertex: V
  HSymMat2 V;
  V(0,0) = cov_x_x + zv*(2*cov_x_sx + zv*cov_sx_sx);
  V(0,1) = cov_x_y + zv*(cov_y_sx + cov_x_sy + zv*cov_sx_sy);
  V(1,1) = cov_y_y + zv*(2*cov_y_sy + zv*cov_sy_sy); 

  Float_t det = V(0,0)*V(1,1) - V(0,1)*V(0,1);
  Float_t Sxx = V(0,0) / det;
  Float_t Syy = V(1,1) / det;
  Float_t Sxy = -V(0,1) / det;
  //Sxx = 1; Syy=1; Sxy=0;

  fM(0,0)= Sxx;
  fM(0,1)= Sxy;
  fM(0,2)= -(Sxx*sx + Sxy*sy);
  fM(1,1)= Syy;
  fM(1,2)= -(Sxy*sx + Syy*sy);
  fM(2,2)= -(Sxx*sx*sx + Syy*sy*sy + 2*Sxy*sx*sy);

  fSys(0,0)+=fM(0,0);
  fSys(0,1)+=fM(0,1);
  fSys(0,2)+=fM(0,2);
  fSys(1,1)+=fM(1,1);
  fSys(1,2)+=fM(1,2);
  fSys(2,2)+=fM(2,2);

  fB.X()+=Sxx*x0 + Sxy*y0;
  fB.Y()+=Sxy*x0 + Syy*y0;
  fB.Z()+=Sxx*x0*sx + Syy*y0*sy + Sxy*(x0*sx + sx*y0);
}

void HGeomVertexFit2::getVertex(HGeomVector &out) {
  // This method fills the vector "out" with the coordinates
  //of the point of maximun approach to the lines added with
  //addLine()
  Double_t det=0;

  det=fSys(0,0)*fSys(1,1)*fSys(2,2) + fSys(0,1)*fSys(1,2)*fSys(0,2) 
    + fSys(0,1)*fSys(1,2)*fSys(0,2) - fSys(0,2)*fSys(1,1)*fSys(0,2) 
    - fSys(0,1)*fSys(0,1)*fSys(2,2) - fSys(1,2)*fSys(1,2)*fSys(0,0);

  fM(0,0)=fSys(1,1) * fSys(2,2) - fSys(1,2) * fSys(1,2);
  fM(0,1)=fSys(1,2) * fSys(0,2) - fSys(0,1) * fSys(2,2);
  fM(0,2)=fSys(0,1) * fSys(1,2) - fSys(1,1) * fSys(0,2);
  fM(1,1)=fSys(0,0) * fSys(2,2) - fSys(0,2) * fSys(0,2);
  fM(1,2)=fSys(0,1) * fSys(0,2) - fSys(0,0) * fSys(1,2);
  fM(2,2)=fSys(0,0) * fSys(1,1) - fSys(0,1) * fSys(0,1);
  fM(1,0)=fM(0,1);
  fM(2,0)=fM(0,2);
  fM(2,1)=fM(1,2);
  if(det==0){
      out.setXYZ(-1000.,-1000.,-1000.);
      return;
  }
  fM/=(det);

  #if DEBUG_LEVEL>1
  cout << "Det= " << det << endl;
  cout << "Equation system: \n";
  for (Int_t i=0;i<3;i++) {
    for (Int_t j=0;j<3;j++) cout << fSys(i,j) << "\t";
    cout << fB(i) << endl;
  }
  cout << "Inverse system matrix: \n";
  for (Int_t i=0;i<3;i++) {
    for (Int_t j=0;j<i;j++) cout << "\t";
    for (Int_t j=i;j<3;j++) cout << fM(i,j) << "\t";
    cout << endl;
  }
  #endif

  out=fM*fB;
}

void HGeomVertexFit2::reset(void) {
  // Resets the fitting procedure. That is, the class goes to the
  //state where no line was added
  for (Int_t i=0;i<3;i++) {
    for (Int_t j=0;j<3;j++) fM(i,j)=fSys(i,j)=0.0;
  }
  fB.setXYZ(0.0,0.0,0.0);
}

ClassImp(HGeomVertexFit2)
 hgeomvertexfit2.cc:1
 hgeomvertexfit2.cc:2
 hgeomvertexfit2.cc:3
 hgeomvertexfit2.cc:4
 hgeomvertexfit2.cc:5
 hgeomvertexfit2.cc:6
 hgeomvertexfit2.cc:7
 hgeomvertexfit2.cc:8
 hgeomvertexfit2.cc:9
 hgeomvertexfit2.cc:10
 hgeomvertexfit2.cc:11
 hgeomvertexfit2.cc:12
 hgeomvertexfit2.cc:13
 hgeomvertexfit2.cc:14
 hgeomvertexfit2.cc:15
 hgeomvertexfit2.cc:16
 hgeomvertexfit2.cc:17
 hgeomvertexfit2.cc:18
 hgeomvertexfit2.cc:19
 hgeomvertexfit2.cc:20
 hgeomvertexfit2.cc:21
 hgeomvertexfit2.cc:22
 hgeomvertexfit2.cc:23
 hgeomvertexfit2.cc:24
 hgeomvertexfit2.cc:25
 hgeomvertexfit2.cc:26
 hgeomvertexfit2.cc:27
 hgeomvertexfit2.cc:28
 hgeomvertexfit2.cc:29
 hgeomvertexfit2.cc:30
 hgeomvertexfit2.cc:31
 hgeomvertexfit2.cc:32
 hgeomvertexfit2.cc:33
 hgeomvertexfit2.cc:34
 hgeomvertexfit2.cc:35
 hgeomvertexfit2.cc:36
 hgeomvertexfit2.cc:37
 hgeomvertexfit2.cc:38
 hgeomvertexfit2.cc:39
 hgeomvertexfit2.cc:40
 hgeomvertexfit2.cc:41
 hgeomvertexfit2.cc:42
 hgeomvertexfit2.cc:43
 hgeomvertexfit2.cc:44
 hgeomvertexfit2.cc:45
 hgeomvertexfit2.cc:46
 hgeomvertexfit2.cc:47
 hgeomvertexfit2.cc:48
 hgeomvertexfit2.cc:49
 hgeomvertexfit2.cc:50
 hgeomvertexfit2.cc:51
 hgeomvertexfit2.cc:52
 hgeomvertexfit2.cc:53
 hgeomvertexfit2.cc:54
 hgeomvertexfit2.cc:55
 hgeomvertexfit2.cc:56
 hgeomvertexfit2.cc:57
 hgeomvertexfit2.cc:58
 hgeomvertexfit2.cc:59
 hgeomvertexfit2.cc:60
 hgeomvertexfit2.cc:61
 hgeomvertexfit2.cc:62
 hgeomvertexfit2.cc:63
 hgeomvertexfit2.cc:64
 hgeomvertexfit2.cc:65
 hgeomvertexfit2.cc:66
 hgeomvertexfit2.cc:67
 hgeomvertexfit2.cc:68
 hgeomvertexfit2.cc:69
 hgeomvertexfit2.cc:70
 hgeomvertexfit2.cc:71
 hgeomvertexfit2.cc:72
 hgeomvertexfit2.cc:73
 hgeomvertexfit2.cc:74
 hgeomvertexfit2.cc:75
 hgeomvertexfit2.cc:76
 hgeomvertexfit2.cc:77
 hgeomvertexfit2.cc:78
 hgeomvertexfit2.cc:79
 hgeomvertexfit2.cc:80
 hgeomvertexfit2.cc:81
 hgeomvertexfit2.cc:82
 hgeomvertexfit2.cc:83
 hgeomvertexfit2.cc:84
 hgeomvertexfit2.cc:85
 hgeomvertexfit2.cc:86
 hgeomvertexfit2.cc:87
 hgeomvertexfit2.cc:88
 hgeomvertexfit2.cc:89
 hgeomvertexfit2.cc:90
 hgeomvertexfit2.cc:91
 hgeomvertexfit2.cc:92
 hgeomvertexfit2.cc:93
 hgeomvertexfit2.cc:94
 hgeomvertexfit2.cc:95
 hgeomvertexfit2.cc:96
 hgeomvertexfit2.cc:97
 hgeomvertexfit2.cc:98
 hgeomvertexfit2.cc:99
 hgeomvertexfit2.cc:100
 hgeomvertexfit2.cc:101
 hgeomvertexfit2.cc:102
 hgeomvertexfit2.cc:103
 hgeomvertexfit2.cc:104
 hgeomvertexfit2.cc:105
 hgeomvertexfit2.cc:106
 hgeomvertexfit2.cc:107
 hgeomvertexfit2.cc:108
 hgeomvertexfit2.cc:109
 hgeomvertexfit2.cc:110
 hgeomvertexfit2.cc:111
 hgeomvertexfit2.cc:112
 hgeomvertexfit2.cc:113
 hgeomvertexfit2.cc:114
 hgeomvertexfit2.cc:115
 hgeomvertexfit2.cc:116
 hgeomvertexfit2.cc:117
 hgeomvertexfit2.cc:118
 hgeomvertexfit2.cc:119
 hgeomvertexfit2.cc:120
 hgeomvertexfit2.cc:121
 hgeomvertexfit2.cc:122
 hgeomvertexfit2.cc:123
 hgeomvertexfit2.cc:124
 hgeomvertexfit2.cc:125
 hgeomvertexfit2.cc:126
 hgeomvertexfit2.cc:127
 hgeomvertexfit2.cc:128
 hgeomvertexfit2.cc:129
 hgeomvertexfit2.cc:130
 hgeomvertexfit2.cc:131
 hgeomvertexfit2.cc:132
 hgeomvertexfit2.cc:133
 hgeomvertexfit2.cc:134
 hgeomvertexfit2.cc:135
 hgeomvertexfit2.cc:136
 hgeomvertexfit2.cc:137
 hgeomvertexfit2.cc:138
 hgeomvertexfit2.cc:139
 hgeomvertexfit2.cc:140