KinematicFit/src/SmartPointingConstraint.cc

0001 #include "RecoVertex/KinematicFit/interface/SmartPointingConstraint.h"
0002 #include "RecoVertex/VertexPrimitives/interface/VertexException.h"
0003
0004 std::pair<AlgebraicVector, AlgebraicVector> SmartPointingConstraint::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> SmartPointingConstraint::derivative(const AlgebraicVector& exPoint) const {
0027   if (exPoint.num_row() == 0)
0028     throw VertexException("PointingKinematicConstraint::value requested for zero Linearization point");
0029
0030   //security check for extended cartesian parametrization
0031   int inSize = exPoint.num_row();
0032   if ((inSize % 7) != 0)
0033     throw VertexException("PointingKinematicConstraint::linearization point has a wrong dimension");
0034   int nStates = inSize / 7;
0035   if (nStates != 1)
0036     throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
0037   AlgebraicVector lPar = exPoint;
0038
0039   //2x7 derivative matrix for given particle
0040   AlgebraicMatrix lDeriv = makeDerivative(lPar).first;
0041   AlgebraicMatrix dr(2, 7, 0);
0042   dr.sub(1, 1, lDeriv);
0043   return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, lPar);
0044 }
0045
0046 std::pair<AlgebraicMatrix, AlgebraicVector> SmartPointingConstraint::derivative(
0047     const std::vector<RefCountedKinematicParticle>& par) const {
0048   int nStates = par.size();
0049   if (nStates == 0)
0050     throw VertexException("PointingKinematicConstraint::Empty vector of particles passed");
0051   if (nStates != 1)
0052     throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
0053
0054   AlgebraicMatrix dr(2, 7, 0);
0055   AlgebraicVector lPoint = asHepVector<7>(par.front()->currentState().kinematicParameters().vector());
0056
0057   //2x7 derivative matrix for given state
0058   AlgebraicMatrix lDeriv = makeDerivative(lPoint).first;
0059   dr.sub(1, 1, lDeriv);
0060   // cout<<"Derivative returned: "<<dr<<endl;
0061   // cout<<"For the value: "<<lPoint<<endl;
0062   return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, lPoint);
0063 }
0064
0065 std::pair<AlgebraicVector, AlgebraicVector> SmartPointingConstraint::value(
0066     const std::vector<RefCountedKinematicParticle>& par) const {
0067   int nStates = par.size();
0068   if (nStates == 0)
0069     throw VertexException("PointingKinematicConstraint::Empty vector of particles passed");
0070   if (nStates != 1)
0071     throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
0072   AlgebraicVector vl(2, 0);
0073   AlgebraicVector lPoint = asHepVector<7>(par.front()->currentState().kinematicParameters().vector());
0074   vl(1) = makeValue(lPoint).first(1);
0075   vl(2) = makeValue(lPoint).first(2);
0076   // cout<<"Value returned: "<<vl<<endl;
0077   // cout<<"For the point: "<<lPoint<<endl;
0078
0079   return std::pair<AlgebraicVector, AlgebraicVector>(vl, lPoint);
0080 }
0081
0082 AlgebraicVector SmartPointingConstraint::deviations(int nStates) const { return AlgebraicVector(7 * nStates, 0); }
0083
0084 int SmartPointingConstraint::numberOfEquations() const { return 2; }
0085
0086 std::pair<AlgebraicVector, AlgebraicVector> SmartPointingConstraint::makeValue(const AlgebraicVector& exPoint) const {
0087   // cout<<"Make value called"<<endl;
0088   AlgebraicVector vl(2, 0);
0089   AlgebraicVector point = exPoint;
0090   double dx = point(1) - refPoint.x();
0091   double dy = point(2) - refPoint.y();
0092   double dz = point(3) - refPoint.z();
0093   double px = point(4);
0094   double py = point(5);
0095   double pz = point(6);
0096
0097   //full angle solution: sin(alpha - betha) = 0
0098   //sign swap allowed
0099   double cos_phi_p = px / sqrt(px * px + py * py);
0100   double sin_phi_p = py / sqrt(px * px + py * py);
0101   double cos_phi_x = dx / sqrt(dx * dx + dy * dy);
0102   double sin_phi_x = dy / sqrt(dx * dx + dy * dy);
0103
0104   double sin_theta_p = pz / sqrt(px * px + py * py + pz * pz);
0105   double sin_theta_x = dz / sqrt(dx * dx + dy * dy + dz * dz);
0106
0107   double cos_theta_p = sqrt(px * px + py * py) / sqrt(px * px + py * py + pz * pz);
0108   double cos_theta_x = sqrt(dx * dx + dy * dy) / sqrt(dx * dx + dy * dy + dz * dz);
0109
0110   float feq = sin_phi_p * cos_phi_x - cos_phi_p * sin_phi_x;
0111   float seq = sin_theta_p * cos_theta_x - cos_theta_p * sin_theta_x;
0112
0113   vl(1) = feq;
0114   vl(2) = seq;
0115
0116   return std::pair<AlgebraicVector, AlgebraicVector>(vl, point);
0117 }
0118
0119 std::pair<AlgebraicMatrix, AlgebraicVector> SmartPointingConstraint::makeDerivative(
0120     const AlgebraicVector& exPoint) const {
0121   AlgebraicMatrix dr(2, 7, 0);
0122   AlgebraicVector point = exPoint;
0123   double dx = point(1) - refPoint.x();
0124   double dy = point(2) - refPoint.y();
0125   double dz = point(3) - refPoint.z();
0126   double px = point(4);
0127   double py = point(5);
0128   double pz = point(6);
0129
0130   //angular functuions:
0131
0132   //half angle solution
0133   //d/dx_i
0134   dr(1, 1) = (dy * (dx * px + dy * py)) / (pow(pow(dx, 2) + pow(dy, 2), 1.5) * sqrt(pow(px, 2) + pow(py, 2)));
0135
0136   dr(1, 2) = -((dx * (dx * px + dy * py)) / (pow(pow(dx, 2) + pow(dy, 2), 1.5) * sqrt(pow(px, 2) + pow(py, 2))));
0137
0138   dr(1, 3) = 0;
0139
0140   //d/dp_i
0141   //debug: x->p index xhange in denominator
0142   dr(1, 4) = -((py * (dx * px + dy * py)) / (sqrt(pow(dx, 2) + pow(dy, 2)) * pow(pow(px, 2) + pow(py, 2), 1.5)));
0143
0144   dr(1, 5) = (px * (dx * px + dy * py)) / (sqrt(pow(dx, 2) + pow(dy, 2)) * pow(pow(px, 2) + pow(py, 2), 1.5));
0145
0146   dr(1, 6) = 0;
0147   dr(1, 7) = 0;
0148
0149   //2nd equation
0150   //d/dx_i
0151
0152   dr(2, 1) = (dx * dz * (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(px, 2) + pow(py, 2)) + dz * pz)) /
0153              (sqrt(pow(dx, 2) + pow(dy, 2)) * pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) *
0154               sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)));
0155
0156   dr(2, 2) = (dy * dz * (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(px, 2) + pow(py, 2)) + dz * pz)) /
0157              (sqrt(pow(dx, 2) + pow(dy, 2)) * pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) *
0158               sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)));
0159
0160   dr(2, 3) = (-((pow(dx, 2) + pow(dy, 2)) * sqrt(pow(px, 2) + pow(py, 2))) - sqrt(pow(dx, 2) + pow(dy, 2)) * dz * pz) /
0161              (pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) * sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)));
0162
0163   //d/dp_i
0164   //debug: x->p index xhange in denominator
0165
0166   dr(2, 4) = -((px * pz * (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(px, 2) + pow(py, 2)) + dz * pz)) /
0167                (sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)) * sqrt(pow(px, 2) + pow(py, 2)) *
0168                 pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5)));
0169
0170   dr(2, 5) = -((py * pz * (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(px, 2) + pow(py, 2)) + dz * pz)) /
0171                (sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)) * sqrt(pow(px, 2) + pow(py, 2)) *
0172                 pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5)));
0173
0174   dr(2, 6) = (sqrt(pow(dx, 2) + pow(dy, 2)) * (pow(px, 2) + pow(py, 2)) + dz * sqrt(pow(px, 2) + pow(py, 2)) * pz) /
0175              (sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)) * pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5));
0176
0177   dr(2, 7) = 0;
0178
0179   // cout<<"derivative matrix "<<dr<<endl;
0180   return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, point);
0181 }