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