KinematicFit/src/SimplePointingConstraint.cc

0001 #include "RecoVertex/KinematicFit/interface/SimplePointingConstraint.h"
0002 #include "RecoVertex/VertexPrimitives/interface/VertexException.h"
0003
0004 std::pair<AlgebraicVector, AlgebraicVector> SimplePointingConstraint::value(const AlgebraicVector& exPoint) const {
0005   if (exPoint.num_row() == 0)
0006     throw VertexException("PointingKinematicConstraint::value requested for zero Linearization point");
0007
0008   //security check for extended cartesian parametrization
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   //vector of values 1x2  for given particle
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> SimplePointingConstraint::derivative(const AlgebraicVector& exPoint) const {
0027   if (exPoint.num_row() == 0)
0028     throw VertexException("PointingKinematicConstraint::value requested for zero Linearization point");
0029
0030   //security check for extended cartesian parametrization
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   //2x7 derivative matrix for given particle
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> SimplePointingConstraint::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   //2x7 derivative matrix for given state
0058   AlgebraicMatrix lDeriv = makeDerivative(lPoint).first;
0059   dr.sub(1, 1, lDeriv);
0060   // cout<<"Derivative returned: "<<dr<<endl;
0061   // cout<<"For the value: "<<lPoint<<endl;
0062   return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, lPoint);
0063 }
0064
0065 std::pair<AlgebraicVector, AlgebraicVector> SimplePointingConstraint::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   // cout<<"Value returned: "<<vl<<endl;
0077   // cout<<"For the point: "<<lPoint<<endl;
0078
0079   return std::pair<AlgebraicVector, AlgebraicVector>(vl, lPoint);
0080 }
0081
0082 AlgebraicVector SimplePointingConstraint::deviations(int nStates) const { return AlgebraicVector(7 * nStates, 0); }
0083
0084 int SimplePointingConstraint::numberOfEquations() const { return 2; }
0085
0086 std::pair<AlgebraicVector, AlgebraicVector> SimplePointingConstraint::makeValue(const AlgebraicVector& exPoint) const {
0087   // cout<<"Make value called"<<endl;
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   //half angle solution: sin((alpha - betha)/2)
0098   double cos_phi_p = px / sqrt(px * px + py * py);
0099   double cos_phi_x = dx / sqrt(dx * dx + dy * dy);
0100   // cout<<"mom cos phi"<<cos_phi_p<<endl;
0101   // cout<<"x cos phi"<<cos_phi_x<<endl;
0102
0103   double cos_theta_p = sqrt(px * px + py * py) / sqrt(px * px + py * py + pz * pz);
0104   double cos_theta_x = sqrt(dx * dx + dy * dy) / sqrt(dx * dx + dy * dy + dz * dz);
0105
0106   float feq = sqrt((1 - cos_phi_p) * (1 + cos_phi_x)) - sqrt((1 + cos_phi_p) * (1 - cos_phi_x));
0107   float seq = sqrt((1 - cos_theta_p) * (1 + cos_theta_x)) - sqrt((1 + cos_theta_p) * (1 - cos_theta_x));
0108
0109   // cout<<"First factor: "<<feq/2<<endl;
0110   // cout<<"Second factor: "<<seq/2<<endl;
0111
0112   vl(1) = feq / 2;
0113   vl(2) = seq / 2;
0114
0115   // cout<<"Value "<<vl<<endl;
0116   //half angle corrected
0117   // vl(1) = (sin_x/(1+cos_x)) - (sin_p/(1+cos_p));
0118   // vl(2) = (sin_xt/(1+cos_xt)) - (sin_pt/(1+cos_pt));
0119   return std::pair<AlgebraicVector, AlgebraicVector>(vl, point);
0120 }
0121
0122 std::pair<AlgebraicMatrix, AlgebraicVector> SimplePointingConstraint::makeDerivative(
0123     const AlgebraicVector& exPoint) const {
0124   AlgebraicMatrix dr(2, 7, 0);
0125   AlgebraicVector point = exPoint;
0126   double dx = point(1) - refPoint.x();
0127   double dy = point(2) - refPoint.y();
0128   double dz = point(3) - refPoint.z();
0129   double px = point(4);
0130   double py = point(5);
0131   double pz = point(6);
0132
0133   //half angle solution
0134   //d/dx_i
0135   dr(1, 1) = (sqrt((1 + dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 - px / sqrt(pow(px, 2) + pow(py, 2)))) -
0136               sqrt((1 - dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 + px / sqrt(pow(px, 2) + pow(py, 2))))) /
0137              2.;
0138
0139   dr(1, 2) = (((-(pow(dx, 2) / pow(pow(dx, 2) + pow(dy, 2), 1.5)) + 1 / sqrt(pow(dx, 2) + pow(dy, 2))) *
0140                (1 - px / sqrt(pow(px, 2) + pow(py, 2)))) /
0141                   (2. * sqrt((1 + dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 - px / sqrt(pow(px, 2) + pow(py, 2))))) -
0142               ((pow(dx, 2) / pow(pow(dx, 2) + pow(dy, 2), 1.5) - 1 / sqrt(pow(dx, 2) + pow(dy, 2))) *
0143                (1 + px / sqrt(pow(px, 2) + pow(py, 2)))) /
0144                   (2. * sqrt((1 - dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 + px / sqrt(pow(px, 2) + pow(py, 2)))))) /
0145              2.;
0146
0147   dr(1, 3) = 0;
0148
0149   //d/dp_i
0150   //debug: x->p index xhange in denominator
0151   dr(1, 4) = (-(dx * dy * (1 - px / sqrt(pow(px, 2) + pow(py, 2)))) /
0152                   (2. * pow(pow(dx, 2) + pow(dy, 2), 1.5) *
0153                    sqrt((1 + dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 - px / sqrt(pow(px, 2) + pow(py, 2))))) -
0154               (dx * dy * (1 + px / sqrt(pow(px, 2) + pow(py, 2)))) /
0155                   (2. * pow(pow(dx, 2) + pow(dy, 2), 1.5) *
0156                    sqrt((1 - dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 + px / sqrt(pow(px, 2) + pow(py, 2)))))) /
0157              2.;
0158
0159   dr(1, 5) = (((1 + dx / sqrt(pow(dx, 2) + pow(dy, 2))) * px * py) /
0160                   (2. * pow(pow(px, 2) + pow(py, 2), 1.5) *
0161                    sqrt((1 + dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 - px / sqrt(pow(px, 2) + pow(py, 2))))) +
0162               ((1 - dx / sqrt(pow(dx, 2) + pow(dy, 2))) * px * py) /
0163                   (2. * pow(pow(px, 2) + pow(py, 2), 1.5) *
0164                    sqrt((1 - dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 + px / sqrt(pow(px, 2) + pow(py, 2)))))) /
0165              2.;
0166
0167   dr(1, 6) = 0;
0168   dr(1, 7) = 0;
0169
0170   //2nd equation
0171   //d/dx_i
0172
0173   dr(2, 1) = (((-((dx * sqrt(pow(dx, 2) + pow(dy, 2))) / pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5)) +
0174                 dx / (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)))) *
0175                (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))) /
0176                   (2. * sqrt((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0177                              (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) -
0178               (((dx * sqrt(pow(dx, 2) + pow(dy, 2))) / pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) -
0179                 dx / (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)))) *
0180                (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))) /
0181                   (2. * sqrt((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0182                              (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))))) /
0183              2.;
0184
0185   dr(2, 2) = (((-((dy * sqrt(pow(dx, 2) + pow(dy, 2))) / pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5)) +
0186                 dy / (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)))) *
0187                (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))) /
0188                   (2. * sqrt((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0189                              (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) -
0190               (((dy * sqrt(pow(dx, 2) + pow(dy, 2))) / pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) -
0191                 dy / (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)))) *
0192                (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))) /
0193                   (2. * sqrt((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0194                              (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))))) /
0195              2.;
0196
0197   dr(2, 3) = (-(sqrt(pow(dx, 2) + pow(dy, 2)) * dz *
0198                 (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))) /
0199                   (2. * pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) *
0200                    sqrt((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0201                         (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) -
0202               (sqrt(pow(dx, 2) + pow(dy, 2)) * dz *
0203                (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))) /
0204                   (2. * pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) *
0205                    sqrt((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0206                         (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))))) /
0207              2.;
0208
0209   //d/dp_i
0210   //debug: x->p index xhange in denominator
0211
0212   dr(2, 4) = (((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0213                ((px * sqrt(pow(px, 2) + pow(py, 2))) / pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5) -
0214                 px / (sqrt(pow(px, 2) + pow(py, 2)) * sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) /
0215                   (2. * sqrt((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0216                              (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) -
0217               ((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0218                (-((px * sqrt(pow(px, 2) + pow(py, 2))) / pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5)) +
0219                 px / (sqrt(pow(px, 2) + pow(py, 2)) * sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) /
0220                   (2. * sqrt((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0221                              (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))))) /
0222              2.;
0223
0224   dr(2, 5) = (((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0225                ((py * sqrt(pow(px, 2) + pow(py, 2))) / pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5) -
0226                 py / (sqrt(pow(px, 2) + pow(py, 2)) * sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) /
0227                   (2. * sqrt((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0228                              (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) -
0229               ((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0230                (-((py * sqrt(pow(px, 2) + pow(py, 2))) / pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5)) +
0231                 py / (sqrt(pow(px, 2) + pow(py, 2)) * sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) /
0232                   (2. * sqrt((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0233                              (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))))) /
0234              2.;
0235
0236   dr(2, 6) = (((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0237                sqrt(pow(px, 2) + pow(py, 2)) * pz) /
0238                   (2. * pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5) *
0239                    sqrt((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0240                         (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) +
0241               ((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0242                sqrt(pow(px, 2) + pow(py, 2)) * pz) /
0243                   (2. * pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5) *
0244                    sqrt((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
0245                         (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))))) /
0246              2.;
0247
0248   dr(2, 7) = 0;
0249
0250   // cout<<"derivative matrix "<<dr<<endl;
0251   return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, point);
0252 }