PointingKinematicConstraint.cc

Go to the documentation of this file.

#include "RecoVertex/KinematicFit/interface/PointingKinematicConstraint.h"
#include "RecoVertex/VertexPrimitives/interface/VertexException.h"

std::pair<AlgebraicVector, AlgebraicVector> PointingKinematicConstraint::value(const AlgebraicVector& exPoint) const
{
 if(exPoint.num_row() ==0 ) throw VertexException("PointingKinematicConstraint::value requested for zero Linearization point");

//security check for extended cartesian parametrization 
 int inSize = exPoint.num_row(); 
 if((inSize%7) !=0) throw VertexException("PointingKinematicConstraint::linearization point has a wrong dimension");
 int nStates = inSize/7;
 if(nStates != 1) throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
 
 AlgebraicVector lPar = exPoint;
 AlgebraicVector vl(2,0);
 
//vector of values 1x2  for given particle
 AlgebraicVector lValue = makeValue(lPar).first;
 vl(1) =lValue(1);
 vl(2) =lValue(2);
 return std::pair<AlgebraicVector,AlgebraicVector>(vl,lPar); 
}

std::pair<AlgebraicMatrix, AlgebraicVector> PointingKinematicConstraint::derivative(const AlgebraicVector& exPoint) const
{
 if(exPoint.num_row() ==0 ) throw VertexException("PointingKinematicConstraint::value requested for zero Linearization point");

//security check for extended cartesian parametrization 
 int inSize = exPoint.num_row(); 
 if((inSize%7) !=0) throw VertexException("PointingKinematicConstraint::linearization point has a wrong dimension");
 int nStates = inSize/7;
 if(nStates != 1) throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
 AlgebraicVector lPar = exPoint;

//2x7 derivative matrix for given particle
 AlgebraicMatrix lDeriv = makeDerivative(lPar).first;
 AlgebraicMatrix dr(2,7,0);
 dr.sub(1,1,lDeriv);
 return std::pair<AlgebraicMatrix,AlgebraicVector>(dr,lPar);
}

std::pair<AlgebraicMatrix, AlgebraicVector> PointingKinematicConstraint::derivative(const std::vector<RefCountedKinematicParticle> &par) const
{
 int nStates = par.size();
 if(nStates == 0) throw VertexException("PointingKinematicConstraint::Empty vector of particles passed");
 if(nStates != 1) throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
 
 AlgebraicMatrix dr(2,7,0);
 AlgebraicVector lPoint = asHepVector<7>(par.front()->currentState().kinematicParameters().vector());

//2x7 derivative matrix for given state  
 AlgebraicMatrix lDeriv = makeDerivative(lPoint).first;
 dr.sub(1,1,lDeriv);
 return std::pair<AlgebraicMatrix,AlgebraicVector>(dr,lPoint);
}

std::pair<AlgebraicVector, AlgebraicVector> PointingKinematicConstraint::value(const std::vector<RefCountedKinematicParticle> &par) const
{
 int nStates = par.size();
 if(nStates == 0) throw VertexException("PointingKinematicConstraint::Empty vector of particles passed");
 if(nStates != 1) throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
 AlgebraicVector vl(2,0);
 AlgebraicVector lPoint = asHepVector<7>(par.front()->currentState().kinematicParameters().vector());
 vl(1) = makeValue(lPoint).first(1);
 vl(2) = makeValue(lPoint).first(2);
 return std::pair<AlgebraicVector,AlgebraicVector>(vl,lPoint); 
}

std::pair<AlgebraicVector,AlgebraicVector> PointingKinematicConstraint::makeValue(const AlgebraicVector& exPoint)const
{
 AlgebraicVector vl(2,0);
 AlgebraicVector point = exPoint;
 double dx = point(1) - refPoint.x();
 double dy = point(2) - refPoint.y();
 double dz = point(3) - refPoint.z();
 double px = point(4);
 double py = point(5); 
 double pz = point(6);

// tangent solution
// vl(1) = dy/dx - py/px;
// vl(2) = dz/sqrt(dx*dx + dy*dy) - pz/sqrt(px*px + py*py); 


//half angle solution
 double sin_p = py/sqrt(px*px + py*py);
 double cos_p = px/sqrt(px*px + py*py);
 double sin_x = dy/sqrt(dx*dx + dy*dy);
 double cos_x = dx/sqrt(dx*dx + dy*dy);
 
 double sin_pt = pz/sqrt(px*px + py*py + pz*pz); 
 double cos_pt = sqrt(px*px + py*py)/sqrt(px*px + py*py + pz*pz);
 double sin_xt = dz/sqrt(dx*dx + dy*dy + dz*dz); 
 double cos_xt = sqrt(dx*dx + dy*dy)/sqrt(dx*dx + dy*dy + dz*dz);
 
 vl(1) = (1-cos_x)/sin_x - (1-cos_p)/sin_p;
 vl(2) = (1-cos_xt)/sin_xt - (1-cos_pt)/sin_pt;

//half angle corrected
// vl(1) = (sin_x/(1+cos_x)) - (sin_p/(1+cos_p));
// vl(2) = (sin_xt/(1+cos_xt)) - (sin_pt/(1+cos_pt));
 return std::pair<AlgebraicVector,AlgebraicVector>(vl,point);
}


std::pair<AlgebraicMatrix, AlgebraicVector> PointingKinematicConstraint:: makeDerivative(const AlgebraicVector& exPoint) const
{
 AlgebraicMatrix dr(2,7,0);
 AlgebraicVector point = exPoint;
 double dx = point(1) - refPoint.x();
 double dy = point(2) - refPoint.y();
 double dz = point(3) - refPoint.z();
 double px = point(4);
 double py = point(5); 
 double pz = point(6);
                  
// double tr = px*px + py*py;
// double trd = dx*dx + dy*dy;
// double pr =1.5;
// double p_factor = pow(tr,pr);
// double x_factor = pow(trd,pr);
  
//tangent solution
/*
 dr(1,1) = -dy/(dx*dx);
 dr(1,2) = 1/dx;
 dr(1,3) = 0;
 dr(1,4) = py/(px*px);
 dr(1,5) = -1/px;
 dr(1,6) = 0;
 dr(1,7) = 0;
 
 dr(2,1) = -(dx*dz)/x_factor;
 dr(2,2) = -(dy*dz)/x_factor;
 dr(2,3) = 1/sqrt(dx*dx + dy*dy);
 dr(2,4) = (px*pz)/p_factor;
 dr(2,5) = (py*pz)/p_factor;
 dr(2,6) = -1/sqrt(px*px + py*py);
 dr(2,7) = 0.;
*/
//half angle solution corrected   
/*
 dr(1,1) = - dy/(dx*dx+dy*dy+dx*sqrt(dx*dx+dy*dy));
 dr(1,2) =   dx/(dx*dx+dy*dy+dx*sqrt(dx*dx+dy*dy));
 dr(1,3) = 0; 
 dr(1,4) = py/(px*px+py*py+px*sqrt(px*px+py*py));
 dr(1,5) = -px/(px*px+py*py+px*sqrt(px*px+py*py));
 dr(1,6) = 0;
 dr(1,7) = 0; 
*/

//half angle solution
 dr(1,1) = dx/(dy*sqrt(dx*dx + dy*dy)) - 1/dy;
 dr(1,2) = 1/sqrt(dx*dx+dy*dy) - sqrt(dx*dx+dy*dy)/(dy*dy)+ dx/(dy*dy);
 dr(1,3) = 0; 
 dr(1,4) = -(px/(py*sqrt(px*px + py*py)) - 1/py);
 dr(1,5) = -(1/sqrt(px*px+py*py) - sqrt(px*px+py*py)/(py*py)+ px/(py*py));
 dr(1,6) = 0;
 dr(1,7) = 0; 


//half angle solution
 dr(2,1) = (dx/dz)*(1/sqrt(dx*dx + dy*dy + dz*dz) - 1/sqrt(dx*dx + dy*dy));
 dr(2,2) = (dy/dz)*(1/sqrt(dx*dx + dy*dy + dz*dz) - 1/sqrt(dx*dx + dy*dy));
 dr(2,3) = (1/(dz*dz))*(sqrt(dx*dx + dy*dy) - sqrt(dx*dx+dy*dy+dz*dz)) + 1/sqrt(dx*dx+dy*dy+dz*dz);
 dr(2,4) = -(px/pz)*(1/sqrt(px*px + py*py + pz*pz) - 1/sqrt(px*px + py*py));
 dr(2,5) = -(py/pz)*(1/sqrt(px*px + py*py + pz*pz) - 1/sqrt(px*px + py*py));
 dr(2,6) = -((1/(pz*pz))*(sqrt(px*px + py*py) - sqrt(px*px+py*py+pz*pz)) + 1/sqrt(px*px+py*py+pz*pz));
 dr(2,7) = 0;
 
 return std::pair<AlgebraicMatrix,AlgebraicVector>(dr,point);
}

AlgebraicVector PointingKinematicConstraint::deviations(int nStates) const
{return AlgebraicVector(7*nStates,0);}

int PointingKinematicConstraint::numberOfEquations() const
{return 2;}