ColinearityKinematicConstraintT.h

Go to the documentation of this file.

#ifndef ColinearityKinematicConstraintT_H
#define ColinearityKinematicConstraintT_H
 
#include "RecoVertex/KinematicFitPrimitives/interface/MultiTrackKinematicConstraintT.h"
#include "RecoVertex/KinematicFitPrimitives/interface/KinematicState.h"
#include "DataFormats/CLHEP/interface/AlgebraicObjects.h"
 
#include "RecoVertex/VertexPrimitives/interface/VertexException.h"
 
namespace colinearityKinematic {
  enum ConstraintDim { Phi = 1, PhiTheta = 2 };
}
 
template <enum colinearityKinematic::ConstraintDim Dim>
class ColinearityKinematicConstraintT : public MultiTrackKinematicConstraintT<2, int(Dim)> {
private:
  double a_1;
  double a_2;
 
  AlgebraicVector7 p1;
  AlgebraicVector7 p2;
 
  GlobalPoint point;
 
public:
  typedef MultiTrackKinematicConstraintT<2, int(Dim)> super;
 
  ColinearityKinematicConstraintT() {}
 
  // initialize the constraint so it can precompute common qualtities to the three next call
  void init(const std::vector<KinematicState>& states,
            const GlobalPoint& ipoint,
            const GlobalVector& fieldValue) override {
    if (states.size() != 2)
      throw VertexException("ColinearityKinematicConstraint::<2 states passed");
 
    point = ipoint;
 
    a_1 = -states[0].particleCharge() * fieldValue.z();
    a_2 = -states[1].particleCharge() * fieldValue.z();
 
    p1 = states[0].kinematicParameters().vector();
    p2 = states[1].kinematicParameters().vector();
  }
 
  int numberOfEquations() const override { return Dim == colinearityKinematic::Phi ? 1 : 2; }
 
  ColinearityKinematicConstraintT<Dim>* clone() const override {
    return new ColinearityKinematicConstraintT<Dim>(*this);
  }
 
private:
  void fillValue() const override;
 
  void fillParametersDerivative() const override;
 
  void fillPositionDerivative() const override;
};
 
template <enum colinearityKinematic::ConstraintDim Dim>
void ColinearityKinematicConstraintT<Dim>::fillValue() const {
  typename super::valueType& vl = super::vl();
 
  double p1vx = p1(3) - a_1 * (point.y() - p1(1));
  double p1vy = p1(4) + a_1 * (point.x() - p1(0));
 
  double p2vx = p2(3) - a_2 * (point.y() - p2(1));
  double p2vy = p2(4) + a_2 * (point.x() - p2(0));
 
  // H_phi:
  vl(0) = atan2(p1vy, p1vx) - atan2(p2vy, p2vx);
  if (vl(0) > M_PI)
    vl(0) -= 2.0 * M_PI;
  if (vl(0) <= -M_PI)
    vl(0) += 2.0 * M_PI;
  // H_theta:
  if (Dim == colinearityKinematic::PhiTheta) {
    double pt1 = sqrt(p1(3) * p1(3) + p1(4) * p1(4));
    double pt2 = sqrt(p2(3) * p2(3) + p2(4) * p2(4));
    vl(1) = atan2(pt1, p1(5)) - atan2(pt2, p2(5));
    if (vl(1) > M_PI)
      vl(1) -= 2.0 * M_PI;
    if (vl(1) <= -M_PI)
      vl(1) += 2.0 * M_PI;
  }
}
 
template <enum colinearityKinematic::ConstraintDim Dim>
void ColinearityKinematicConstraintT<Dim>::fillParametersDerivative() const {
  typename super::parametersDerivativeType& jac_d = super::jac_d();
 
  double p1vx = p1(3) - a_1 * (point.y() - p1(1));
  double p1vy = p1(4) + a_1 * (point.x() - p1(0));
  double k1 = 1.0 / (p1vx * p1vx + p1vy * p1vy);
 
  double p2vx = p2(3) - a_2 * (point.y() - p2(1));
  double p2vy = p2(4) + a_2 * (point.x() - p2(0));
  double k2 = 1.0 / (p2vx * p2vx + p2vy * p2vy);
 
  // H_phi:
 
  //x1 and x2 derivatives: 1st and 8th elements
  jac_d(0, 0) = -k1 * a_1 * p1vx;
  jac_d(0, 7) = k2 * a_2 * p2vx;
 
  //y1 and y2 derivatives: 2nd and 9th elements:
  jac_d(0, 1) = -k1 * a_1 * p1vy;
  jac_d(0, 8) = k2 * a_2 * p2vy;
 
  //z1 and z2 components: 3d and 10th elmnets stay 0:
  jac_d(0, 2) = 0.;
  jac_d(0, 9) = 0.;
 
  //px1 and px2 components: 4th and 11th elements:
  jac_d(0, 3) = -k1 * p1vy;
  jac_d(0, 10) = k2 * p2vy;
 
  //py1 and py2 components: 5th and 12 elements:
  jac_d(0, 4) = k1 * p1vx;
  jac_d(0, 11) = -k2 * p2vx;
 
  //pz1 and pz2 components: 6th and 13 elements:
  jac_d(0, 5) = 0.;
  jac_d(0, 12) = 0.;
  //mass components: 7th and 14th elements:
  jac_d(0, 6) = 0.;
  jac_d(0, 13) = 0.;
 
  if (Dim == colinearityKinematic::PhiTheta) {
    double pt1 = sqrt(p1(3) * p1(3) + p1(4) * p1(4));
    double pTot1 = p1(3) * p1(3) + p1(4) * p1(4) + p1(5) * p1(5);
    double pt2 = sqrt(p2(3) * p2(3) + p2(4) * p2(4));
    double pTot2 = p2(3) * p2(3) + p2(4) * p2(4) + p2(5) * p2(5);
 
    // H_theta:
    //x1 and x2 derivatives: 1st and 8th elements
    jac_d(1, 0) = 0.;
    jac_d(1, 7) = 0.;
 
    //y1 and y2 derivatives: 2nd and 9th elements:
    jac_d(1, 1) = 0.;
    jac_d(1, 8) = 0.;
 
    //z1 and z2 components: 3d and 10th elmnets stay 0:
    jac_d(1, 2) = 0.;
    jac_d(1, 9) = 0.;
 
    jac_d(1, 3) = p1(3) * (p1(5) / (pTot1 * pt1));
    jac_d(1, 10) = p2(3) * (-p2(5) / (pTot2 * pt2));
 
    //py1 and py2 components: 5th and 12 elements:
    jac_d(1, 4) = p1(4) * (p1(5) / (pTot1 * pt1));
    jac_d(1, 11) = p2(4) * (-p2(5) / (pTot2 * pt2));
 
    //pz1 and pz2 components: 6th and 13 elements:
    jac_d(1, 5) = -pt1 / pTot1;
    jac_d(1, 12) = pt2 / pTot2;
 
    //mass components: 7th and 14th elements:
    jac_d(1, 6) = 0.;
    jac_d(1, 13) = 0.;
  }
}
 
template <enum colinearityKinematic::ConstraintDim Dim>
void ColinearityKinematicConstraintT<Dim>::fillPositionDerivative() const {
  typename super::positionDerivativeType& jac_e = super::jac_e();
 
  double p1vx = p1(3) - a_1 * (point.y() - p1(1));
  double p1vy = p1(4) + a_1 * (point.x() - p1(0));
  double k1 = 1.0 / (p1vx * p1vx + p1vy * p1vy);
 
  double p2vx = p2(3) - a_2 * (point.y() - p2(1));
  double p2vy = p2(4) + a_2 * (point.x() - p2(0));
  double k2 = 1.0 / (p2vx * p2vx + p2vy * p2vy);
 
  // H_phi:
 
  // xv component
  jac_e(0, 0) = k1 * a_1 * p1vx - k2 * a_2 * p2vx;
 
  //yv component
  jac_e(0, 1) = k1 * a_1 * p1vy - k2 * a_2 * p2vy;
 
  //zv component
  jac_e(0, 2) = 0.;
 
  // H_theta: no correlation with vertex position
  if (Dim == colinearityKinematic::PhiTheta) {
    jac_e(1, 0) = 0.;
    jac_e(1, 1) = 0.;
    jac_e(1, 2) = 0.;
  }
}
 
#endif