KinematicFit/src/PointingKinematicConstraint.cc

0001 #include "RecoVertex/KinematicFit/interface/PointingKinematicConstraint.h"
0002 #include "RecoVertex/VertexPrimitives/interface/VertexException.h"
0003
0004 std::pair<AlgebraicVector, AlgebraicVector> PointingKinematicConstraint::value(const AlgebraicVector& exPoint) const {
0005   if (exPoint.num_row() == 0)
0006     throw VertexException("PointingKinematicConstraint::value requested for zero Linearization point");
0007
0008   //security check for extended cartesian parametrization
0009   int inSize = exPoint.num_row();
0010   if ((inSize % 7) != 0)
0011     throw VertexException("PointingKinematicConstraint::linearization point has a wrong dimension");
0012   int nStates = inSize / 7;
0013   if (nStates != 1)
0014     throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
0015
0016   AlgebraicVector lPar = exPoint;
0017   AlgebraicVector vl(2, 0);
0018
0019   //vector of values 1x2  for given particle
0020   AlgebraicVector lValue = makeValue(lPar).first;
0021   vl(1) = lValue(1);
0022   vl(2) = lValue(2);
0023   return std::pair<AlgebraicVector, AlgebraicVector>(vl, lPar);
0024 }
0025
0026 std::pair<AlgebraicMatrix, AlgebraicVector> PointingKinematicConstraint::derivative(
0027     const AlgebraicVector& exPoint) const {
0028   if (exPoint.num_row() == 0)
0029     throw VertexException("PointingKinematicConstraint::value requested for zero Linearization point");
0030
0031   //security check for extended cartesian parametrization
0032   int inSize = exPoint.num_row();
0033   if ((inSize % 7) != 0)
0034     throw VertexException("PointingKinematicConstraint::linearization point has a wrong dimension");
0035   int nStates = inSize / 7;
0036   if (nStates != 1)
0037     throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
0038   AlgebraicVector lPar = exPoint;
0039
0040   //2x7 derivative matrix for given particle
0041   AlgebraicMatrix lDeriv = makeDerivative(lPar).first;
0042   AlgebraicMatrix dr(2, 7, 0);
0043   dr.sub(1, 1, lDeriv);
0044   return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, lPar);
0045 }
0046
0047 std::pair<AlgebraicMatrix, AlgebraicVector> PointingKinematicConstraint::derivative(
0048     const std::vector<RefCountedKinematicParticle>& par) const {
0049   int nStates = par.size();
0050   if (nStates == 0)
0051     throw VertexException("PointingKinematicConstraint::Empty vector of particles passed");
0052   if (nStates != 1)
0053     throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
0054
0055   AlgebraicMatrix dr(2, 7, 0);
0056   AlgebraicVector lPoint = asHepVector<7>(par.front()->currentState().kinematicParameters().vector());
0057
0058   //2x7 derivative matrix for given state
0059   AlgebraicMatrix lDeriv = makeDerivative(lPoint).first;
0060   dr.sub(1, 1, lDeriv);
0061   return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, lPoint);
0062 }
0063
0064 std::pair<AlgebraicVector, AlgebraicVector> PointingKinematicConstraint::value(
0065     const std::vector<RefCountedKinematicParticle>& par) const {
0066   int nStates = par.size();
0067   if (nStates == 0)
0068     throw VertexException("PointingKinematicConstraint::Empty vector of particles passed");
0069   if (nStates != 1)
0070     throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
0071   AlgebraicVector vl(2, 0);
0072   AlgebraicVector lPoint = asHepVector<7>(par.front()->currentState().kinematicParameters().vector());
0073   vl(1) = makeValue(lPoint).first(1);
0074   vl(2) = makeValue(lPoint).first(2);
0075   return std::pair<AlgebraicVector, AlgebraicVector>(vl, lPoint);
0076 }
0077
0078 std::pair<AlgebraicVector, AlgebraicVector> PointingKinematicConstraint::makeValue(
0079     const AlgebraicVector& exPoint) const {
0080   AlgebraicVector vl(2, 0);
0081   AlgebraicVector point = exPoint;
0082   double dx = point(1) - refPoint.x();
0083   double dy = point(2) - refPoint.y();
0084   double dz = point(3) - refPoint.z();
0085   double px = point(4);
0086   double py = point(5);
0087   double pz = point(6);
0088
0089   // tangent solution
0090   // vl(1) = dy/dx - py/px;
0091   // vl(2) = dz/sqrt(dx*dx + dy*dy) - pz/sqrt(px*px + py*py);
0092
0093   //half angle solution
0094   double sin_p = py / sqrt(px * px + py * py);
0095   double cos_p = px / sqrt(px * px + py * py);
0096   double sin_x = dy / sqrt(dx * dx + dy * dy);
0097   double cos_x = dx / sqrt(dx * dx + dy * dy);
0098
0099   double sin_pt = pz / sqrt(px * px + py * py + pz * pz);
0100   double cos_pt = sqrt(px * px + py * py) / sqrt(px * px + py * py + pz * pz);
0101   double sin_xt = dz / sqrt(dx * dx + dy * dy + dz * dz);
0102   double cos_xt = sqrt(dx * dx + dy * dy) / sqrt(dx * dx + dy * dy + dz * dz);
0103
0104   vl(1) = (1 - cos_x) / sin_x - (1 - cos_p) / sin_p;
0105   vl(2) = (1 - cos_xt) / sin_xt - (1 - cos_pt) / sin_pt;
0106
0107   //half angle corrected
0108   // vl(1) = (sin_x/(1+cos_x)) - (sin_p/(1+cos_p));
0109   // vl(2) = (sin_xt/(1+cos_xt)) - (sin_pt/(1+cos_pt));
0110   return std::pair<AlgebraicVector, AlgebraicVector>(vl, point);
0111 }
0112
0113 std::pair<AlgebraicMatrix, AlgebraicVector> PointingKinematicConstraint::makeDerivative(
0114     const AlgebraicVector& exPoint) const {
0115   AlgebraicMatrix dr(2, 7, 0);
0116   AlgebraicVector point = exPoint;
0117   double dx = point(1) - refPoint.x();
0118   double dy = point(2) - refPoint.y();
0119   double dz = point(3) - refPoint.z();
0120   double px = point(4);
0121   double py = point(5);
0122   double pz = point(6);
0123
0124   // double tr = px*px + py*py;
0125   // double trd = dx*dx + dy*dy;
0126   // double pr =1.5;
0127   // double p_factor = pow(tr,pr);
0128   // double x_factor = pow(trd,pr);
0129
0130   //tangent solution
0131   /*
0132  dr(1,1) = -dy/(dx*dx);
0133  dr(1,2) = 1/dx;
0134  dr(1,3) = 0;
0135  dr(1,4) = py/(px*px);
0136  dr(1,5) = -1/px;
0137  dr(1,6) = 0;
0138  dr(1,7) = 0;
0139
0140  dr(2,1) = -(dx*dz)/x_factor;
0141  dr(2,2) = -(dy*dz)/x_factor;
0142  dr(2,3) = 1/sqrt(dx*dx + dy*dy);
0143  dr(2,4) = (px*pz)/p_factor;
0144  dr(2,5) = (py*pz)/p_factor;
0145  dr(2,6) = -1/sqrt(px*px + py*py);
0146  dr(2,7) = 0.;
0147 */
0148   //half angle solution corrected
0149   /*
0150  dr(1,1) = - dy/(dx*dx+dy*dy+dx*sqrt(dx*dx+dy*dy));
0151  dr(1,2) =   dx/(dx*dx+dy*dy+dx*sqrt(dx*dx+dy*dy));
0152  dr(1,3) = 0;
0153  dr(1,4) = py/(px*px+py*py+px*sqrt(px*px+py*py));
0154  dr(1,5) = -px/(px*px+py*py+px*sqrt(px*px+py*py));
0155  dr(1,6) = 0;
0156  dr(1,7) = 0;
0157 */
0158
0159   //half angle solution
0160   dr(1, 1) = dx / (dy * sqrt(dx * dx + dy * dy)) - 1 / dy;
0161   dr(1, 2) = 1 / sqrt(dx * dx + dy * dy) - sqrt(dx * dx + dy * dy) / (dy * dy) + dx / (dy * dy);
0162   dr(1, 3) = 0;
0163   dr(1, 4) = -(px / (py * sqrt(px * px + py * py)) - 1 / py);
0164   dr(1, 5) = -(1 / sqrt(px * px + py * py) - sqrt(px * px + py * py) / (py * py) + px / (py * py));
0165   dr(1, 6) = 0;
0166   dr(1, 7) = 0;
0167
0168   //half angle solution
0169   dr(2, 1) = (dx / dz) * (1 / sqrt(dx * dx + dy * dy + dz * dz) - 1 / sqrt(dx * dx + dy * dy));
0170   dr(2, 2) = (dy / dz) * (1 / sqrt(dx * dx + dy * dy + dz * dz) - 1 / sqrt(dx * dx + dy * dy));
0171   dr(2, 3) = (1 / (dz * dz)) * (sqrt(dx * dx + dy * dy) - sqrt(dx * dx + dy * dy + dz * dz)) +
0172              1 / sqrt(dx * dx + dy * dy + dz * dz);
0173   dr(2, 4) = -(px / pz) * (1 / sqrt(px * px + py * py + pz * pz) - 1 / sqrt(px * px + py * py));
0174   dr(2, 5) = -(py / pz) * (1 / sqrt(px * px + py * py + pz * pz) - 1 / sqrt(px * px + py * py));
0175   dr(2, 6) = -((1 / (pz * pz)) * (sqrt(px * px + py * py) - sqrt(px * px + py * py + pz * pz)) +
0176                1 / sqrt(px * px + py * py + pz * pz));
0177   dr(2, 7) = 0;
0178
0179   return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, point);
0180 }
0181
0182 AlgebraicVector PointingKinematicConstraint::deviations(int nStates) const { return AlgebraicVector(7 * nStates, 0); }
0183
0184 int PointingKinematicConstraint::numberOfEquations() const { return 2; }