KinematicFit/interface/ColinearityKinematicConstraintT.h

0001 #ifndef ColinearityKinematicConstraintT_H
0002 #define ColinearityKinematicConstraintT_H
0003
0004 #include "RecoVertex/KinematicFitPrimitives/interface/MultiTrackKinematicConstraintT.h"
0005 #include "RecoVertex/KinematicFitPrimitives/interface/KinematicState.h"
0006 #include "DataFormats/CLHEP/interface/AlgebraicObjects.h"
0007
0008 #include "RecoVertex/VertexPrimitives/interface/VertexException.h"
0009
0010 /**
0011  * Constraint to force the two tracks to be colinear (parallel), in 2D (phi) or 3D (phi-theta).
0012  *
0013  * Warning: Since this constraint makes only sense with two tracks, two and only
0014  * two tracks should be used in the fit.
0015  *
0016  */
0017
0018 namespace colinearityKinematic {
0019   enum ConstraintDim { Phi = 1, PhiTheta = 2 };
0020 }
0021
0022 template <enum colinearityKinematic::ConstraintDim Dim>
0023 class ColinearityKinematicConstraintT : public MultiTrackKinematicConstraintT<2, int(Dim)> {
0024 private:
0025   double a_1;
0026   double a_2;
0027
0028   AlgebraicVector7 p1;
0029   AlgebraicVector7 p2;
0030
0031   GlobalPoint point;
0032
0033 public:
0034   typedef MultiTrackKinematicConstraintT<2, int(Dim)> super;
0035
0036   ColinearityKinematicConstraintT() {}
0037
0038   // initialize the constraint so it can precompute common qualtities to the three next call
0039   void init(const std::vector<KinematicState>& states,
0040             const GlobalPoint& ipoint,
0041             const GlobalVector& fieldValue) override {
0042     if (states.size() != 2)
0043       throw VertexException("ColinearityKinematicConstraint::<2 states passed");
0044
0045     point = ipoint;
0046
0047     a_1 = -states[0].particleCharge() * fieldValue.z();
0048     a_2 = -states[1].particleCharge() * fieldValue.z();
0049
0050     p1 = states[0].kinematicParameters().vector();
0051     p2 = states[1].kinematicParameters().vector();
0052   }
0053
0054   /**
0055    * Number of equations per track used for the fit
0056    */
0057   int numberOfEquations() const override { return Dim == colinearityKinematic::Phi ? 1 : 2; }
0058
0059   ColinearityKinematicConstraintT<Dim>* clone() const override {
0060     return new ColinearityKinematicConstraintT<Dim>(*this);
0061   }
0062
0063 private:
0064   /**
0065    * fills a vector of values of constraint
0066    * equations at the point where the input
0067    * particles are defined.
0068    */
0069   void fillValue() const override;
0070
0071   /**
0072    * fills a matrix of derivatives of
0073    * constraint equations w.r.t.
0074    * particle parameters
0075    */
0076   void fillParametersDerivative() const override;
0077
0078   /**
0079    * Returns a matrix of derivatives of
0080    * constraint equations w.r.t.
0081    * vertex position
0082    */
0083   void fillPositionDerivative() const override;
0084 };
0085
0086 template <enum colinearityKinematic::ConstraintDim Dim>
0087 void ColinearityKinematicConstraintT<Dim>::fillValue() const {
0088   typename super::valueType& vl = super::vl();
0089
0090   double p1vx = p1(3) - a_1 * (point.y() - p1(1));
0091   double p1vy = p1(4) + a_1 * (point.x() - p1(0));
0092
0093   double p2vx = p2(3) - a_2 * (point.y() - p2(1));
0094   double p2vy = p2(4) + a_2 * (point.x() - p2(0));
0095
0096   // H_phi:
0097   vl(0) = atan2(p1vy, p1vx) - atan2(p2vy, p2vx);
0098   if (vl(0) > M_PI)
0099     vl(0) -= 2.0 * M_PI;
0100   if (vl(0) <= -M_PI)
0101     vl(0) += 2.0 * M_PI;
0102   // H_theta:
0103   if (Dim == colinearityKinematic::PhiTheta) {
0104     double pt1 = sqrt(p1(3) * p1(3) + p1(4) * p1(4));
0105     double pt2 = sqrt(p2(3) * p2(3) + p2(4) * p2(4));
0106     vl(1) = atan2(pt1, p1(5)) - atan2(pt2, p2(5));
0107     if (vl(1) > M_PI)
0108       vl(1) -= 2.0 * M_PI;
0109     if (vl(1) <= -M_PI)
0110       vl(1) += 2.0 * M_PI;
0111   }
0112 }
0113
0114 template <enum colinearityKinematic::ConstraintDim Dim>
0115 void ColinearityKinematicConstraintT<Dim>::fillParametersDerivative() const {
0116   typename super::parametersDerivativeType& jac_d = super::jac_d();
0117
0118   double p1vx = p1(3) - a_1 * (point.y() - p1(1));
0119   double p1vy = p1(4) + a_1 * (point.x() - p1(0));
0120   double k1 = 1.0 / (p1vx * p1vx + p1vy * p1vy);
0121
0122   double p2vx = p2(3) - a_2 * (point.y() - p2(1));
0123   double p2vy = p2(4) + a_2 * (point.x() - p2(0));
0124   double k2 = 1.0 / (p2vx * p2vx + p2vy * p2vy);
0125
0126   // H_phi:
0127
0128   //x1 and x2 derivatives: 1st and 8th elements
0129   jac_d(0, 0) = -k1 * a_1 * p1vx;
0130   jac_d(0, 7) = k2 * a_2 * p2vx;
0131
0132   //y1 and y2 derivatives: 2nd and 9th elements:
0133   jac_d(0, 1) = -k1 * a_1 * p1vy;
0134   jac_d(0, 8) = k2 * a_2 * p2vy;
0135
0136   //z1 and z2 components: 3d and 10th elmnets stay 0:
0137   jac_d(0, 2) = 0.;
0138   jac_d(0, 9) = 0.;
0139
0140   //px1 and px2 components: 4th and 11th elements:
0141   jac_d(0, 3) = -k1 * p1vy;
0142   jac_d(0, 10) = k2 * p2vy;
0143
0144   //py1 and py2 components: 5th and 12 elements:
0145   jac_d(0, 4) = k1 * p1vx;
0146   jac_d(0, 11) = -k2 * p2vx;
0147
0148   //pz1 and pz2 components: 6th and 13 elements:
0149   jac_d(0, 5) = 0.;
0150   jac_d(0, 12) = 0.;
0151   //mass components: 7th and 14th elements:
0152   jac_d(0, 6) = 0.;
0153   jac_d(0, 13) = 0.;
0154
0155   if (Dim == colinearityKinematic::PhiTheta) {
0156     double pt1 = sqrt(p1(3) * p1(3) + p1(4) * p1(4));
0157     double pTot1 = p1(3) * p1(3) + p1(4) * p1(4) + p1(5) * p1(5);
0158     double pt2 = sqrt(p2(3) * p2(3) + p2(4) * p2(4));
0159     double pTot2 = p2(3) * p2(3) + p2(4) * p2(4) + p2(5) * p2(5);
0160
0161     // H_theta:
0162     //x1 and x2 derivatives: 1st and 8th elements
0163     jac_d(1, 0) = 0.;
0164     jac_d(1, 7) = 0.;
0165
0166     //y1 and y2 derivatives: 2nd and 9th elements:
0167     jac_d(1, 1) = 0.;
0168     jac_d(1, 8) = 0.;
0169
0170     //z1 and z2 components: 3d and 10th elmnets stay 0:
0171     jac_d(1, 2) = 0.;
0172     jac_d(1, 9) = 0.;
0173
0174     jac_d(1, 3) = p1(3) * (p1(5) / (pTot1 * pt1));
0175     jac_d(1, 10) = p2(3) * (-p2(5) / (pTot2 * pt2));
0176
0177     //py1 and py2 components: 5th and 12 elements:
0178     jac_d(1, 4) = p1(4) * (p1(5) / (pTot1 * pt1));
0179     jac_d(1, 11) = p2(4) * (-p2(5) / (pTot2 * pt2));
0180
0181     //pz1 and pz2 components: 6th and 13 elements:
0182     jac_d(1, 5) = -pt1 / pTot1;
0183     jac_d(1, 12) = pt2 / pTot2;
0184
0185     //mass components: 7th and 14th elements:
0186     jac_d(1, 6) = 0.;
0187     jac_d(1, 13) = 0.;
0188   }
0189 }
0190
0191 template <enum colinearityKinematic::ConstraintDim Dim>
0192 void ColinearityKinematicConstraintT<Dim>::fillPositionDerivative() const {
0193   typename super::positionDerivativeType& jac_e = super::jac_e();
0194
0195   double p1vx = p1(3) - a_1 * (point.y() - p1(1));
0196   double p1vy = p1(4) + a_1 * (point.x() - p1(0));
0197   double k1 = 1.0 / (p1vx * p1vx + p1vy * p1vy);
0198
0199   double p2vx = p2(3) - a_2 * (point.y() - p2(1));
0200   double p2vy = p2(4) + a_2 * (point.x() - p2(0));
0201   double k2 = 1.0 / (p2vx * p2vx + p2vy * p2vy);
0202
0203   // H_phi:
0204
0205   // xv component
0206   jac_e(0, 0) = k1 * a_1 * p1vx - k2 * a_2 * p2vx;
0207
0208   //yv component
0209   jac_e(0, 1) = k1 * a_1 * p1vy - k2 * a_2 * p2vy;
0210
0211   //zv component
0212   jac_e(0, 2) = 0.;
0213
0214   // H_theta: no correlation with vertex position
0215   if (Dim == colinearityKinematic::PhiTheta) {
0216     jac_e(1, 0) = 0.;
0217     jac_e(1, 1) = 0.;
0218     jac_e(1, 2) = 0.;
0219   }
0220 }
0221
0222 #endif