File indexing completed on 2024-04-06 12:29:12
0001 #include "RecoVertex/KinematicFit/interface/SmartPointingConstraint.h"
0002 #include "RecoVertex/VertexPrimitives/interface/VertexException.h"
0003
0004 std::pair<AlgebraicVector, AlgebraicVector> SmartPointingConstraint::value(const AlgebraicVector& exPoint) const {
0005 if (exPoint.num_row() == 0)
0006 throw VertexException("PointingKinematicConstraint::value requested for zero Linearization point");
0007
0008
0009 int inSize = exPoint.num_row();
0010 if ((inSize % 7) != 0)
0011 throw VertexException("PointingKinematicConstraint::linearization point has a wrong dimension");
0012 int nStates = inSize / 7;
0013 if (nStates != 1)
0014 throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
0015
0016 AlgebraicVector lPar = exPoint;
0017 AlgebraicVector vl(2, 0);
0018
0019
0020 AlgebraicVector lValue = makeValue(lPar).first;
0021 vl(1) = lValue(1);
0022 vl(2) = lValue(2);
0023 return std::pair<AlgebraicVector, AlgebraicVector>(vl, lPar);
0024 }
0025
0026 std::pair<AlgebraicMatrix, AlgebraicVector> SmartPointingConstraint::derivative(const AlgebraicVector& exPoint) const {
0027 if (exPoint.num_row() == 0)
0028 throw VertexException("PointingKinematicConstraint::value requested for zero Linearization point");
0029
0030
0031 int inSize = exPoint.num_row();
0032 if ((inSize % 7) != 0)
0033 throw VertexException("PointingKinematicConstraint::linearization point has a wrong dimension");
0034 int nStates = inSize / 7;
0035 if (nStates != 1)
0036 throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
0037 AlgebraicVector lPar = exPoint;
0038
0039
0040 AlgebraicMatrix lDeriv = makeDerivative(lPar).first;
0041 AlgebraicMatrix dr(2, 7, 0);
0042 dr.sub(1, 1, lDeriv);
0043 return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, lPar);
0044 }
0045
0046 std::pair<AlgebraicMatrix, AlgebraicVector> SmartPointingConstraint::derivative(
0047 const std::vector<RefCountedKinematicParticle>& par) const {
0048 int nStates = par.size();
0049 if (nStates == 0)
0050 throw VertexException("PointingKinematicConstraint::Empty vector of particles passed");
0051 if (nStates != 1)
0052 throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
0053
0054 AlgebraicMatrix dr(2, 7, 0);
0055 AlgebraicVector lPoint = asHepVector<7>(par.front()->currentState().kinematicParameters().vector());
0056
0057
0058 AlgebraicMatrix lDeriv = makeDerivative(lPoint).first;
0059 dr.sub(1, 1, lDeriv);
0060
0061
0062 return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, lPoint);
0063 }
0064
0065 std::pair<AlgebraicVector, AlgebraicVector> SmartPointingConstraint::value(
0066 const std::vector<RefCountedKinematicParticle>& par) const {
0067 int nStates = par.size();
0068 if (nStates == 0)
0069 throw VertexException("PointingKinematicConstraint::Empty vector of particles passed");
0070 if (nStates != 1)
0071 throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
0072 AlgebraicVector vl(2, 0);
0073 AlgebraicVector lPoint = asHepVector<7>(par.front()->currentState().kinematicParameters().vector());
0074 vl(1) = makeValue(lPoint).first(1);
0075 vl(2) = makeValue(lPoint).first(2);
0076
0077
0078
0079 return std::pair<AlgebraicVector, AlgebraicVector>(vl, lPoint);
0080 }
0081
0082 AlgebraicVector SmartPointingConstraint::deviations(int nStates) const { return AlgebraicVector(7 * nStates, 0); }
0083
0084 int SmartPointingConstraint::numberOfEquations() const { return 2; }
0085
0086 std::pair<AlgebraicVector, AlgebraicVector> SmartPointingConstraint::makeValue(const AlgebraicVector& exPoint) const {
0087
0088 AlgebraicVector vl(2, 0);
0089 AlgebraicVector point = exPoint;
0090 double dx = point(1) - refPoint.x();
0091 double dy = point(2) - refPoint.y();
0092 double dz = point(3) - refPoint.z();
0093 double px = point(4);
0094 double py = point(5);
0095 double pz = point(6);
0096
0097
0098
0099 double cos_phi_p = px / sqrt(px * px + py * py);
0100 double sin_phi_p = py / sqrt(px * px + py * py);
0101 double cos_phi_x = dx / sqrt(dx * dx + dy * dy);
0102 double sin_phi_x = dy / sqrt(dx * dx + dy * dy);
0103
0104 double sin_theta_p = pz / sqrt(px * px + py * py + pz * pz);
0105 double sin_theta_x = dz / sqrt(dx * dx + dy * dy + dz * dz);
0106
0107 double cos_theta_p = sqrt(px * px + py * py) / sqrt(px * px + py * py + pz * pz);
0108 double cos_theta_x = sqrt(dx * dx + dy * dy) / sqrt(dx * dx + dy * dy + dz * dz);
0109
0110 float feq = sin_phi_p * cos_phi_x - cos_phi_p * sin_phi_x;
0111 float seq = sin_theta_p * cos_theta_x - cos_theta_p * sin_theta_x;
0112
0113 vl(1) = feq;
0114 vl(2) = seq;
0115
0116 return std::pair<AlgebraicVector, AlgebraicVector>(vl, point);
0117 }
0118
0119 std::pair<AlgebraicMatrix, AlgebraicVector> SmartPointingConstraint::makeDerivative(
0120 const AlgebraicVector& exPoint) const {
0121 AlgebraicMatrix dr(2, 7, 0);
0122 AlgebraicVector point = exPoint;
0123 double dx = point(1) - refPoint.x();
0124 double dy = point(2) - refPoint.y();
0125 double dz = point(3) - refPoint.z();
0126 double px = point(4);
0127 double py = point(5);
0128 double pz = point(6);
0129
0130
0131
0132
0133
0134 dr(1, 1) = (dy * (dx * px + dy * py)) / (pow(pow(dx, 2) + pow(dy, 2), 1.5) * sqrt(pow(px, 2) + pow(py, 2)));
0135
0136 dr(1, 2) = -((dx * (dx * px + dy * py)) / (pow(pow(dx, 2) + pow(dy, 2), 1.5) * sqrt(pow(px, 2) + pow(py, 2))));
0137
0138 dr(1, 3) = 0;
0139
0140
0141
0142 dr(1, 4) = -((py * (dx * px + dy * py)) / (sqrt(pow(dx, 2) + pow(dy, 2)) * pow(pow(px, 2) + pow(py, 2), 1.5)));
0143
0144 dr(1, 5) = (px * (dx * px + dy * py)) / (sqrt(pow(dx, 2) + pow(dy, 2)) * pow(pow(px, 2) + pow(py, 2), 1.5));
0145
0146 dr(1, 6) = 0;
0147 dr(1, 7) = 0;
0148
0149
0150
0151
0152 dr(2, 1) = (dx * dz * (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(px, 2) + pow(py, 2)) + dz * pz)) /
0153 (sqrt(pow(dx, 2) + pow(dy, 2)) * pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) *
0154 sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)));
0155
0156 dr(2, 2) = (dy * dz * (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(px, 2) + pow(py, 2)) + dz * pz)) /
0157 (sqrt(pow(dx, 2) + pow(dy, 2)) * pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) *
0158 sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)));
0159
0160 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) /
0161 (pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) * sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)));
0162
0163
0164
0165
0166 dr(2, 4) = -((px * pz * (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(px, 2) + pow(py, 2)) + dz * pz)) /
0167 (sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)) * sqrt(pow(px, 2) + pow(py, 2)) *
0168 pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5)));
0169
0170 dr(2, 5) = -((py * pz * (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(px, 2) + pow(py, 2)) + dz * pz)) /
0171 (sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)) * sqrt(pow(px, 2) + pow(py, 2)) *
0172 pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5)));
0173
0174 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) /
0175 (sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)) * pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5));
0176
0177 dr(2, 7) = 0;
0178
0179
0180 return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, point);
0181 }