SmartPointingConstraint.cc

Go to the documentation of this file.

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

std::pair<AlgebraicVector, AlgebraicVector> SmartPointingConstraint::value(const AlgebraicVector& exPoint) const {
  if (exPoint.num_row() == 0)
    throw VertexException("PointingKinematicConstraint::value requested for zero Linearization point");

  //security check for extended cartesian parametrization
  int inSize = exPoint.num_row();
  if ((inSize % 7) != 0)
    throw VertexException("PointingKinematicConstraint::linearization point has a wrong dimension");
  int nStates = inSize / 7;
  if (nStates != 1)
    throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");

  AlgebraicVector lPar = exPoint;
  AlgebraicVector vl(2, 0);

  //vector of values 1x2  for given particle
  AlgebraicVector lValue = makeValue(lPar).first;
  vl(1) = lValue(1);
  vl(2) = lValue(2);
  return std::pair<AlgebraicVector, AlgebraicVector>(vl, lPar);
}

std::pair<AlgebraicMatrix, AlgebraicVector> SmartPointingConstraint::derivative(const AlgebraicVector& exPoint) const {
  if (exPoint.num_row() == 0)
    throw VertexException("PointingKinematicConstraint::value requested for zero Linearization point");

  //security check for extended cartesian parametrization
  int inSize = exPoint.num_row();
  if ((inSize % 7) != 0)
    throw VertexException("PointingKinematicConstraint::linearization point has a wrong dimension");
  int nStates = inSize / 7;
  if (nStates != 1)
    throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
  AlgebraicVector lPar = exPoint;

  //2x7 derivative matrix for given particle
  AlgebraicMatrix lDeriv = makeDerivative(lPar).first;
  AlgebraicMatrix dr(2, 7, 0);
  dr.sub(1, 1, lDeriv);
  return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, lPar);
}

std::pair<AlgebraicMatrix, AlgebraicVector> SmartPointingConstraint::derivative(
    const std::vector<RefCountedKinematicParticle>& par) const {
  int nStates = par.size();
  if (nStates == 0)
    throw VertexException("PointingKinematicConstraint::Empty vector of particles passed");
  if (nStates != 1)
    throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");

  AlgebraicMatrix dr(2, 7, 0);
  AlgebraicVector lPoint = asHepVector<7>(par.front()->currentState().kinematicParameters().vector());

  //2x7 derivative matrix for given state
  AlgebraicMatrix lDeriv = makeDerivative(lPoint).first;
  dr.sub(1, 1, lDeriv);
  // cout<<"Derivative returned: "<<dr<<endl;
  // cout<<"For the value: "<<lPoint<<endl;
  return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, lPoint);
}

std::pair<AlgebraicVector, AlgebraicVector> SmartPointingConstraint::value(
    const std::vector<RefCountedKinematicParticle>& par) const {
  int nStates = par.size();
  if (nStates == 0)
    throw VertexException("PointingKinematicConstraint::Empty vector of particles passed");
  if (nStates != 1)
    throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
  AlgebraicVector vl(2, 0);
  AlgebraicVector lPoint = asHepVector<7>(par.front()->currentState().kinematicParameters().vector());
  vl(1) = makeValue(lPoint).first(1);
  vl(2) = makeValue(lPoint).first(2);
  // cout<<"Value returned: "<<vl<<endl;
  // cout<<"For the point: "<<lPoint<<endl;

  return std::pair<AlgebraicVector, AlgebraicVector>(vl, lPoint);
}

AlgebraicVector SmartPointingConstraint::deviations(int nStates) const { return AlgebraicVector(7 * nStates, 0); }

int SmartPointingConstraint::numberOfEquations() const { return 2; }

std::pair<AlgebraicVector, AlgebraicVector> SmartPointingConstraint::makeValue(const AlgebraicVector& exPoint) const {
  // cout<<"Make value called"<<endl;
  AlgebraicVector vl(2, 0);
  AlgebraicVector point = exPoint;
  double dx = point(1) - refPoint.x();
  double dy = point(2) - refPoint.y();
  double dz = point(3) - refPoint.z();
  double px = point(4);
  double py = point(5);
  double pz = point(6);

  //full angle solution: sin(alpha - betha) = 0
  //sign swap allowed
  double cos_phi_p = px / sqrt(px * px + py * py);
  double sin_phi_p = py / sqrt(px * px + py * py);
  double cos_phi_x = dx / sqrt(dx * dx + dy * dy);
  double sin_phi_x = dy / sqrt(dx * dx + dy * dy);

  double sin_theta_p = pz / sqrt(px * px + py * py + pz * pz);
  double sin_theta_x = dz / sqrt(dx * dx + dy * dy + dz * dz);

  double cos_theta_p = sqrt(px * px + py * py) / sqrt(px * px + py * py + pz * pz);
  double cos_theta_x = sqrt(dx * dx + dy * dy) / sqrt(dx * dx + dy * dy + dz * dz);

  float feq = sin_phi_p * cos_phi_x - cos_phi_p * sin_phi_x;
  float seq = sin_theta_p * cos_theta_x - cos_theta_p * sin_theta_x;

  vl(1) = feq;
  vl(2) = seq;

  return std::pair<AlgebraicVector, AlgebraicVector>(vl, point);
}

std::pair<AlgebraicMatrix, AlgebraicVector> SmartPointingConstraint::makeDerivative(
    const AlgebraicVector& exPoint) const {
  AlgebraicMatrix dr(2, 7, 0);
  AlgebraicVector point = exPoint;
  double dx = point(1) - refPoint.x();
  double dy = point(2) - refPoint.y();
  double dz = point(3) - refPoint.z();
  double px = point(4);
  double py = point(5);
  double pz = point(6);

  //angular functuions:

  //half angle solution
  //d/dx_i
  dr(1, 1) = (dy * (dx * px + dy * py)) / (pow(pow(dx, 2) + pow(dy, 2), 1.5) * sqrt(pow(px, 2) + pow(py, 2)));

  dr(1, 2) = -((dx * (dx * px + dy * py)) / (pow(pow(dx, 2) + pow(dy, 2), 1.5) * sqrt(pow(px, 2) + pow(py, 2))));

  dr(1, 3) = 0;

  //d/dp_i
  //debug: x->p index xhange in denominator
  dr(1, 4) = -((py * (dx * px + dy * py)) / (sqrt(pow(dx, 2) + pow(dy, 2)) * pow(pow(px, 2) + pow(py, 2), 1.5)));

  dr(1, 5) = (px * (dx * px + dy * py)) / (sqrt(pow(dx, 2) + pow(dy, 2)) * pow(pow(px, 2) + pow(py, 2), 1.5));

  dr(1, 6) = 0;
  dr(1, 7) = 0;

  //2nd equation
  //d/dx_i

  dr(2, 1) = (dx * dz * (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(px, 2) + pow(py, 2)) + dz * pz)) /
             (sqrt(pow(dx, 2) + pow(dy, 2)) * pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) *
              sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)));

  dr(2, 2) = (dy * dz * (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(px, 2) + pow(py, 2)) + dz * pz)) /
             (sqrt(pow(dx, 2) + pow(dy, 2)) * pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) *
              sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)));

  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) /
             (pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) * sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)));

  //d/dp_i
  //debug: x->p index xhange in denominator

  dr(2, 4) = -((px * pz * (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(px, 2) + pow(py, 2)) + dz * pz)) /
               (sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)) * sqrt(pow(px, 2) + pow(py, 2)) *
                pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5)));

  dr(2, 5) = -((py * pz * (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(px, 2) + pow(py, 2)) + dz * pz)) /
               (sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)) * sqrt(pow(px, 2) + pow(py, 2)) *
                pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5)));

  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) /
             (sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)) * pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5));

  dr(2, 7) = 0;

  // cout<<"derivative matrix "<<dr<<endl;
  return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, point);
}