ColinearityKinematicConstraint.cc

Go to the documentation of this file.

#include "RecoVertex/KinematicFit/interface/ColinearityKinematicConstraint.h"
#include "RecoVertex/VertexPrimitives/interface/VertexException.h"

ColinearityKinematicConstraint::ColinearityKinematicConstraint(ConstraintDim dim) {
  dimension = dim;
  if (dimension == Phi)
    size = 1;
  else
    size = 2;
}

AlgebraicVector ColinearityKinematicConstraint::value(const std::vector<KinematicState>& states,
                                                      const GlobalPoint& point) const {
  if (states.size() < 2)
    throw VertexException("ColinearityKinematicConstraint::<2 states passed");
  AlgebraicVector res(size, 0);

  double a_1 = -states[0].particleCharge() * states[0].magneticField()->inInverseGeV(states[0].globalPosition()).z();
  double a_2 = -states[1].particleCharge() * states[1].magneticField()->inInverseGeV(states[1].globalPosition()).z();

  AlgebraicVector7 p1 = states[0].kinematicParameters().vector();
  AlgebraicVector7 p2 = states[1].kinematicParameters().vector();

  double p1vx = p1(3) - a_1 * (point.y() - p1(1));
  double p1vy = p1(4) + a_1 * (point.x() - p1(0));
  double pt1 = sqrt(p1(3) * p1(3) + p1(4) * p1(4));

  double p2vx = p2(3) - a_2 * (point.y() - p2(1));
  double p2vy = p2(4) + a_2 * (point.x() - p2(0));
  double pt2 = sqrt(p2(3) * p2(3) + p2(4) * p2(4));

  // H_phi:
  res(1) = atan2(p1vy, p1vx) - atan2(p2vy, p2vx);
  if (res(1) > M_PI)
    res(1) -= 2.0 * M_PI;
  if (res(1) <= -M_PI)
    res(1) += 2.0 * M_PI;
  // H_theta:
  if (dimension == PhiTheta) {
    res(2) = atan2(pt1, p1(5)) - atan2(pt2, p2(5));
    if (res(2) > M_PI)
      res(2) -= 2.0 * M_PI;
    if (res(2) <= -M_PI)
      res(2) += 2.0 * M_PI;
  }

  // cout << res(1) << " "<<res(2) << "\n ";// res(2)

  return res;
}

AlgebraicMatrix ColinearityKinematicConstraint::parametersDerivative(const std::vector<KinematicState>& states,
                                                                     const GlobalPoint& point) const {
  int n_st = states.size();
  if (n_st < 2)
    throw VertexException("ColinearityKinematicConstraint::<2 states passed");
  AlgebraicMatrix res(size, n_st * 7, 0);

  double a_1 = -states[0].particleCharge() * states[0].magneticField()->inInverseGeV(states[0].globalPosition()).z();
  double a_2 = -states[1].particleCharge() * states[1].magneticField()->inInverseGeV(states[1].globalPosition()).z();

  AlgebraicVector7 p1 = states[0].kinematicParameters().vector();
  AlgebraicVector7 p2 = states[1].kinematicParameters().vector();

  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 pt1 = sqrt(p1(3) * p1(3) + p1(4) * p1(4));
  double pTot1 = sqrt(p1(3) * p1(3) + p1(4) * p1(4) + p1(5) * p1(5));

  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);
  double pt2 = sqrt(p2(3) * p2(3) + p2(4) * p2(4));
  double pTot2 = sqrt(p2(3) * p2(3) + p2(4) * p2(4) + p2(5) * p2(5));

  // H_phi:

  //x1 and x2 derivatives: 1st and 8th elements
  res(1, 1) = -k1 * a_1 * p1vx;
  res(1, 8) = k2 * a_2 * p2vx;

  //y1 and y2 derivatives: 2nd and 9th elements:
  res(1, 2) = -k1 * a_1 * p1vy;
  res(1, 9) = k2 * a_2 * p2vy;

  //z1 and z2 components: 3d and 10th elmnets stay 0:
  res(1, 3) = 0.;
  res(1, 10) = 0.;

  //px1 and px2 components: 4th and 11th elements:
  res(1, 4) = -k1 * p1vy;
  res(1, 11) = k2 * p2vy;

  //py1 and py2 components: 5th and 12 elements:
  res(1, 5) = k1 * p1vx;
  res(1, 12) = -k2 * p2vx;

  //pz1 and pz2 components: 6th and 13 elements:
  res(1, 6) = 0.;
  res(1, 13) = 0.;
  //mass components: 7th and 14th elements:
  res(1, 7) = 0.;
  res(1, 14) = 0.;

  if (dimension == PhiTheta) {
    // H_theta:
    //x1 and x2 derivatives: 1st and 8th elements
    res(2, 1) = 0.;
    res(2, 8) = 0.;

    //y1 and y2 derivatives: 2nd and 9th elements:
    res(2, 2) = 0.;
    res(2, 9) = 0.;

    //z1 and z2 components: 3d and 10th elmnets stay 0:
    res(2, 3) = 0.;
    res(2, 10) = 0.;

    res(2, 4) = p1(5) * p1(3) / (pTot1 * pTot1 * pt1);
    res(2, 11) = -p2(5) * p2(3) / (pTot2 * pTot2 * pt2);

    //py1 and py2 components: 5th and 12 elements:
    res(2, 5) = p1(5) * p1(4) / (pTot1 * pTot1 * pt1);
    res(2, 12) = -p2(5) * p2(4) / (pTot2 * pTot2 * pt2);

    //pz1 and pz2 components: 6th and 13 elements:
    res(2, 6) = -pt1 / (pTot1 * pTot1);
    res(2, 13) = pt2 / (pTot2 * pTot2);
    //mass components: 7th and 14th elements:
    res(2, 7) = 0.;
    res(2, 14) = 0.;
  }

  return res;
}

AlgebraicMatrix ColinearityKinematicConstraint::positionDerivative(const std::vector<KinematicState>& states,
                                                                   const GlobalPoint& point) const {
  AlgebraicMatrix res(size, 3, 0);
  if (states.size() < 2)
    throw VertexException("ColinearityKinematicConstraint::<2 states passed");

  double a_1 = -states[0].particleCharge() * states[0].magneticField()->inInverseGeV(states[0].globalPosition()).z();
  double a_2 = -states[1].particleCharge() * states[1].magneticField()->inInverseGeV(states[1].globalPosition()).z();

  AlgebraicVector7 p1 = states[0].kinematicParameters().vector();
  AlgebraicVector7 p2 = states[1].kinematicParameters().vector();

  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 pt1 = sqrt(p1(3)*p1(3)+p1(4)*p1(4));

  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);
  //double pt2  = sqrt(p2(3)*p2(3)+p2(4)*p2(4));

  // H_phi:

  // xv component
  res(1, 1) = k1 * a_1 * p1vx - k2 * a_2 * p2vx;

  //yv component
  res(1, 2) = k1 * a_1 * p1vy - k2 * a_2 * p2vy;

  //zv component
  res(1, 3) = 0.;

  // H_theta: no correlation with vertex position
  if (dimension == PhiTheta) {
    res(2, 1) = 0.;
    res(2, 2) = 0.;
    res(2, 3) = 0.;
  }

  return res;
}