KinematicFit/src/ColinearityKinematicConstraint.cc

0001 #include "RecoVertex/KinematicFit/interface/ColinearityKinematicConstraint.h"
0002 #include "RecoVertex/VertexPrimitives/interface/VertexException.h"
0003
0004 ColinearityKinematicConstraint::ColinearityKinematicConstraint(ConstraintDim dim) {
0005   dimension = dim;
0006   if (dimension == Phi)
0007     size = 1;
0008   else
0009     size = 2;
0010 }
0011
0012 AlgebraicVector ColinearityKinematicConstraint::value(const std::vector<KinematicState>& states,
0013                                                       const GlobalPoint& point) const {
0014   if (states.size() < 2)
0015     throw VertexException("ColinearityKinematicConstraint::<2 states passed");
0016   AlgebraicVector res(size, 0);
0017
0018   double a_1 = -states[0].particleCharge() * states[0].magneticField()->inInverseGeV(states[0].globalPosition()).z();
0019   double a_2 = -states[1].particleCharge() * states[1].magneticField()->inInverseGeV(states[1].globalPosition()).z();
0020
0021   AlgebraicVector7 p1 = states[0].kinematicParameters().vector();
0022   AlgebraicVector7 p2 = states[1].kinematicParameters().vector();
0023
0024   double p1vx = p1(3) - a_1 * (point.y() - p1(1));
0025   double p1vy = p1(4) + a_1 * (point.x() - p1(0));
0026   double pt1 = sqrt(p1(3) * p1(3) + p1(4) * p1(4));
0027
0028   double p2vx = p2(3) - a_2 * (point.y() - p2(1));
0029   double p2vy = p2(4) + a_2 * (point.x() - p2(0));
0030   double pt2 = sqrt(p2(3) * p2(3) + p2(4) * p2(4));
0031
0032   // H_phi:
0033   res(1) = atan2(p1vy, p1vx) - atan2(p2vy, p2vx);
0034   if (res(1) > M_PI)
0035     res(1) -= 2.0 * M_PI;
0036   if (res(1) <= -M_PI)
0037     res(1) += 2.0 * M_PI;
0038   // H_theta:
0039   if (dimension == PhiTheta) {
0040     res(2) = atan2(pt1, p1(5)) - atan2(pt2, p2(5));
0041     if (res(2) > M_PI)
0042       res(2) -= 2.0 * M_PI;
0043     if (res(2) <= -M_PI)
0044       res(2) += 2.0 * M_PI;
0045   }
0046
0047   // cout << res(1) << " "<<res(2) << "\n ";// res(2)
0048
0049   return res;
0050 }
0051
0052 AlgebraicMatrix ColinearityKinematicConstraint::parametersDerivative(const std::vector<KinematicState>& states,
0053                                                                      const GlobalPoint& point) const {
0054   int n_st = states.size();
0055   if (n_st < 2)
0056     throw VertexException("ColinearityKinematicConstraint::<2 states passed");
0057   AlgebraicMatrix res(size, n_st * 7, 0);
0058
0059   double a_1 = -states[0].particleCharge() * states[0].magneticField()->inInverseGeV(states[0].globalPosition()).z();
0060   double a_2 = -states[1].particleCharge() * states[1].magneticField()->inInverseGeV(states[1].globalPosition()).z();
0061
0062   AlgebraicVector7 p1 = states[0].kinematicParameters().vector();
0063   AlgebraicVector7 p2 = states[1].kinematicParameters().vector();
0064
0065   double p1vx = p1(3) - a_1 * (point.y() - p1(1));
0066   double p1vy = p1(4) + a_1 * (point.x() - p1(0));
0067   double k1 = 1.0 / (p1vx * p1vx + p1vy * p1vy);
0068   double pt1 = sqrt(p1(3) * p1(3) + p1(4) * p1(4));
0069   double pTot1 = sqrt(p1(3) * p1(3) + p1(4) * p1(4) + p1(5) * p1(5));
0070
0071   double p2vx = p2(3) - a_2 * (point.y() - p2(1));
0072   double p2vy = p2(4) + a_2 * (point.x() - p2(0));
0073   double k2 = 1.0 / (p2vx * p2vx + p2vy * p2vy);
0074   double pt2 = sqrt(p2(3) * p2(3) + p2(4) * p2(4));
0075   double pTot2 = sqrt(p2(3) * p2(3) + p2(4) * p2(4) + p2(5) * p2(5));
0076
0077   // H_phi:
0078
0079   //x1 and x2 derivatives: 1st and 8th elements
0080   res(1, 1) = -k1 * a_1 * p1vx;
0081   res(1, 8) = k2 * a_2 * p2vx;
0082
0083   //y1 and y2 derivatives: 2nd and 9th elements:
0084   res(1, 2) = -k1 * a_1 * p1vy;
0085   res(1, 9) = k2 * a_2 * p2vy;
0086
0087   //z1 and z2 components: 3d and 10th elmnets stay 0:
0088   res(1, 3) = 0.;
0089   res(1, 10) = 0.;
0090
0091   //px1 and px2 components: 4th and 11th elements:
0092   res(1, 4) = -k1 * p1vy;
0093   res(1, 11) = k2 * p2vy;
0094
0095   //py1 and py2 components: 5th and 12 elements:
0096   res(1, 5) = k1 * p1vx;
0097   res(1, 12) = -k2 * p2vx;
0098
0099   //pz1 and pz2 components: 6th and 13 elements:
0100   res(1, 6) = 0.;
0101   res(1, 13) = 0.;
0102   //mass components: 7th and 14th elements:
0103   res(1, 7) = 0.;
0104   res(1, 14) = 0.;
0105
0106   if (dimension == PhiTheta) {
0107     // H_theta:
0108     //x1 and x2 derivatives: 1st and 8th elements
0109     res(2, 1) = 0.;
0110     res(2, 8) = 0.;
0111
0112     //y1 and y2 derivatives: 2nd and 9th elements:
0113     res(2, 2) = 0.;
0114     res(2, 9) = 0.;
0115
0116     //z1 and z2 components: 3d and 10th elmnets stay 0:
0117     res(2, 3) = 0.;
0118     res(2, 10) = 0.;
0119
0120     res(2, 4) = p1(5) * p1(3) / (pTot1 * pTot1 * pt1);
0121     res(2, 11) = -p2(5) * p2(3) / (pTot2 * pTot2 * pt2);
0122
0123     //py1 and py2 components: 5th and 12 elements:
0124     res(2, 5) = p1(5) * p1(4) / (pTot1 * pTot1 * pt1);
0125     res(2, 12) = -p2(5) * p2(4) / (pTot2 * pTot2 * pt2);
0126
0127     //pz1 and pz2 components: 6th and 13 elements:
0128     res(2, 6) = -pt1 / (pTot1 * pTot1);
0129     res(2, 13) = pt2 / (pTot2 * pTot2);
0130     //mass components: 7th and 14th elements:
0131     res(2, 7) = 0.;
0132     res(2, 14) = 0.;
0133   }
0134
0135   return res;
0136 }
0137
0138 AlgebraicMatrix ColinearityKinematicConstraint::positionDerivative(const std::vector<KinematicState>& states,
0139                                                                    const GlobalPoint& point) const {
0140   AlgebraicMatrix res(size, 3, 0);
0141   if (states.size() < 2)
0142     throw VertexException("ColinearityKinematicConstraint::<2 states passed");
0143
0144   double a_1 = -states[0].particleCharge() * states[0].magneticField()->inInverseGeV(states[0].globalPosition()).z();
0145   double a_2 = -states[1].particleCharge() * states[1].magneticField()->inInverseGeV(states[1].globalPosition()).z();
0146
0147   AlgebraicVector7 p1 = states[0].kinematicParameters().vector();
0148   AlgebraicVector7 p2 = states[1].kinematicParameters().vector();
0149
0150   double p1vx = p1(3) - a_1 * (point.y() - p1(1));
0151   double p1vy = p1(4) + a_1 * (point.x() - p1(0));
0152   double k1 = 1.0 / (p1vx * p1vx + p1vy * p1vy);
0153   //double pt1 = sqrt(p1(3)*p1(3)+p1(4)*p1(4));
0154
0155   double p2vx = p2(3) - a_2 * (point.y() - p2(1));
0156   double p2vy = p2(4) + a_2 * (point.x() - p2(0));
0157   double k2 = 1.0 / (p2vx * p2vx + p2vy * p2vy);
0158   //double pt2  = sqrt(p2(3)*p2(3)+p2(4)*p2(4));
0159
0160   // H_phi:
0161
0162   // xv component
0163   res(1, 1) = k1 * a_1 * p1vx - k2 * a_2 * p2vx;
0164
0165   //yv component
0166   res(1, 2) = k1 * a_1 * p1vy - k2 * a_2 * p2vy;
0167
0168   //zv component
0169   res(1, 3) = 0.;
0170
0171   // H_theta: no correlation with vertex position
0172   if (dimension == PhiTheta) {
0173     res(2, 1) = 0.;
0174     res(2, 2) = 0.;
0175     res(2, 3) = 0.;
0176   }
0177
0178   return res;
0179 }