CMS 3D CMS Logo
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Groups Pages
SimplePointingConstraint.cc
Go to the documentation of this file.
1 #include "RecoVertex/KinematicFit/interface/SimplePointingConstraint.h"
2 #include "RecoVertex/VertexPrimitives/interface/VertexException.h"
3 
4 std::pair<AlgebraicVector, AlgebraicVector> SimplePointingConstraint::value(const AlgebraicVector& exPoint) const {
5  if (exPoint.num_row() == 0)
6  throw VertexException("PointingKinematicConstraint::value requested for zero Linearization point");
7 
8  //security check for extended cartesian parametrization
9  int inSize = exPoint.num_row();
10  if ((inSize % 7) != 0)
11  throw VertexException("PointingKinematicConstraint::linearization point has a wrong dimension");
12  int nStates = inSize / 7;
13  if (nStates != 1)
14  throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
15 
16  AlgebraicVector lPar = exPoint;
17  AlgebraicVector vl(2, 0);
18 
19  //vector of values 1x2 for given particle
20  AlgebraicVector lValue = makeValue(lPar).first;
21  vl(1) = lValue(1);
22  vl(2) = lValue(2);
23  return std::pair<AlgebraicVector, AlgebraicVector>(vl, lPar);
24 }
25 
26 std::pair<AlgebraicMatrix, AlgebraicVector> SimplePointingConstraint::derivative(const AlgebraicVector& exPoint) const {
27  if (exPoint.num_row() == 0)
28  throw VertexException("PointingKinematicConstraint::value requested for zero Linearization point");
29 
30  //security check for extended cartesian parametrization
31  int inSize = exPoint.num_row();
32  if ((inSize % 7) != 0)
33  throw VertexException("PointingKinematicConstraint::linearization point has a wrong dimension");
34  int nStates = inSize / 7;
35  if (nStates != 1)
36  throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
37  AlgebraicVector lPar = exPoint;
38 
39  //2x7 derivative matrix for given particle
40  AlgebraicMatrix lDeriv = makeDerivative(lPar).first;
41  AlgebraicMatrix dr(2, 7, 0);
42  dr.sub(1, 1, lDeriv);
43  return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, lPar);
44 }
45 
46 std::pair<AlgebraicMatrix, AlgebraicVector> SimplePointingConstraint::derivative(
47  const std::vector<RefCountedKinematicParticle>& par) const {
48  int nStates = par.size();
49  if (nStates == 0)
50  throw VertexException("PointingKinematicConstraint::Empty vector of particles passed");
51  if (nStates != 1)
52  throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
53 
54  AlgebraicMatrix dr(2, 7, 0);
55  AlgebraicVector lPoint = asHepVector<7>(par.front()->currentState().kinematicParameters().vector());
56 
57  //2x7 derivative matrix for given state
58  AlgebraicMatrix lDeriv = makeDerivative(lPoint).first;
59  dr.sub(1, 1, lDeriv);
60  // cout<<"Derivative returned: "<<dr<<endl;
61  // cout<<"For the value: "<<lPoint<<endl;
62  return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, lPoint);
63 }
64 
65 std::pair<AlgebraicVector, AlgebraicVector> SimplePointingConstraint::value(
66  const std::vector<RefCountedKinematicParticle>& par) const {
67  int nStates = par.size();
68  if (nStates == 0)
69  throw VertexException("PointingKinematicConstraint::Empty vector of particles passed");
70  if (nStates != 1)
71  throw VertexException("PointingKinematicConstraint::Current version does not support the multistate refit");
72  AlgebraicVector vl(2, 0);
73  AlgebraicVector lPoint = asHepVector<7>(par.front()->currentState().kinematicParameters().vector());
74  vl(1) = makeValue(lPoint).first(1);
75  vl(2) = makeValue(lPoint).first(2);
76  // cout<<"Value returned: "<<vl<<endl;
77  // cout<<"For the point: "<<lPoint<<endl;
78 
79  return std::pair<AlgebraicVector, AlgebraicVector>(vl, lPoint);
80 }
81 
82 AlgebraicVector SimplePointingConstraint::deviations(int nStates) const { return AlgebraicVector(7 * nStates, 0); }
83 
84 int SimplePointingConstraint::numberOfEquations() const { return 2; }
85 
86 std::pair<AlgebraicVector, AlgebraicVector> SimplePointingConstraint::makeValue(const AlgebraicVector& exPoint) const {
87  // cout<<"Make value called"<<endl;
88  AlgebraicVector vl(2, 0);
89  AlgebraicVector point = exPoint;
90  double dx = point(1) - refPoint.x();
91  double dy = point(2) - refPoint.y();
92  double dz = point(3) - refPoint.z();
93  double px = point(4);
94  double py = point(5);
95  double pz = point(6);
96 
97  //half angle solution: sin((alpha - betha)/2)
98  double cos_phi_p = px / sqrt(px * px + py * py);
99  double cos_phi_x = dx / sqrt(dx * dx + dy * dy);
100  // cout<<"mom cos phi"<<cos_phi_p<<endl;
101  // cout<<"x cos phi"<<cos_phi_x<<endl;
102 
103  double cos_theta_p = sqrt(px * px + py * py) / sqrt(px * px + py * py + pz * pz);
104  double cos_theta_x = sqrt(dx * dx + dy * dy) / sqrt(dx * dx + dy * dy + dz * dz);
105 
106  float feq = sqrt((1 - cos_phi_p) * (1 + cos_phi_x)) - sqrt((1 + cos_phi_p) * (1 - cos_phi_x));
107  float seq = sqrt((1 - cos_theta_p) * (1 + cos_theta_x)) - sqrt((1 + cos_theta_p) * (1 - cos_theta_x));
108 
109  // cout<<"First factor: "<<feq/2<<endl;
110  // cout<<"Second factor: "<<seq/2<<endl;
111 
112  vl(1) = feq / 2;
113  vl(2) = seq / 2;
114 
115  // cout<<"Value "<<vl<<endl;
116  //half angle corrected
117  // vl(1) = (sin_x/(1+cos_x)) - (sin_p/(1+cos_p));
118  // vl(2) = (sin_xt/(1+cos_xt)) - (sin_pt/(1+cos_pt));
119  return std::pair<AlgebraicVector, AlgebraicVector>(vl, point);
120 }
121 
122 std::pair<AlgebraicMatrix, AlgebraicVector> SimplePointingConstraint::makeDerivative(
123  const AlgebraicVector& exPoint) const {
124  AlgebraicMatrix dr(2, 7, 0);
125  AlgebraicVector point = exPoint;
126  double dx = point(1) - refPoint.x();
127  double dy = point(2) - refPoint.y();
128  double dz = point(3) - refPoint.z();
129  double px = point(4);
130  double py = point(5);
131  double pz = point(6);
132 
133  //half angle solution
134  //d/dx_i
135  dr(1, 1) = (sqrt((1 + dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 - px / sqrt(pow(px, 2) + pow(py, 2)))) -
136  sqrt((1 - dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 + px / sqrt(pow(px, 2) + pow(py, 2))))) /
137  2.;
138 
139  dr(1, 2) = (((-(pow(dx, 2) / pow(pow(dx, 2) + pow(dy, 2), 1.5)) + 1 / sqrt(pow(dx, 2) + pow(dy, 2))) *
140  (1 - px / sqrt(pow(px, 2) + pow(py, 2)))) /
141  (2. * sqrt((1 + dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 - px / sqrt(pow(px, 2) + pow(py, 2))))) -
142  ((pow(dx, 2) / pow(pow(dx, 2) + pow(dy, 2), 1.5) - 1 / sqrt(pow(dx, 2) + pow(dy, 2))) *
143  (1 + px / sqrt(pow(px, 2) + pow(py, 2)))) /
144  (2. * sqrt((1 - dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 + px / sqrt(pow(px, 2) + pow(py, 2)))))) /
145  2.;
146 
147  dr(1, 3) = 0;
148 
149  //d/dp_i
150  //debug: x->p index xhange in denominator
151  dr(1, 4) = (-(dx * dy * (1 - px / sqrt(pow(px, 2) + pow(py, 2)))) /
152  (2. * pow(pow(dx, 2) + pow(dy, 2), 1.5) *
153  sqrt((1 + dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 - px / sqrt(pow(px, 2) + pow(py, 2))))) -
154  (dx * dy * (1 + px / sqrt(pow(px, 2) + pow(py, 2)))) /
155  (2. * pow(pow(dx, 2) + pow(dy, 2), 1.5) *
156  sqrt((1 - dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 + px / sqrt(pow(px, 2) + pow(py, 2)))))) /
157  2.;
158 
159  dr(1, 5) = (((1 + dx / sqrt(pow(dx, 2) + pow(dy, 2))) * px * py) /
160  (2. * pow(pow(px, 2) + pow(py, 2), 1.5) *
161  sqrt((1 + dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 - px / sqrt(pow(px, 2) + pow(py, 2))))) +
162  ((1 - dx / sqrt(pow(dx, 2) + pow(dy, 2))) * px * py) /
163  (2. * pow(pow(px, 2) + pow(py, 2), 1.5) *
164  sqrt((1 - dx / sqrt(pow(dx, 2) + pow(dy, 2))) * (1 + px / sqrt(pow(px, 2) + pow(py, 2)))))) /
165  2.;
166 
167  dr(1, 6) = 0;
168  dr(1, 7) = 0;
169 
170  //2nd equation
171  //d/dx_i
172 
173  dr(2, 1) = (((-((dx * sqrt(pow(dx, 2) + pow(dy, 2))) / pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5)) +
174  dx / (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)))) *
175  (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))) /
176  (2. * sqrt((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
177  (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) -
178  (((dx * sqrt(pow(dx, 2) + pow(dy, 2))) / pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) -
179  dx / (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)))) *
180  (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))) /
181  (2. * sqrt((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
182  (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))))) /
183  2.;
184 
185  dr(2, 2) = (((-((dy * sqrt(pow(dx, 2) + pow(dy, 2))) / pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5)) +
186  dy / (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)))) *
187  (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))) /
188  (2. * sqrt((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
189  (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) -
190  (((dy * sqrt(pow(dx, 2) + pow(dy, 2))) / pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) -
191  dy / (sqrt(pow(dx, 2) + pow(dy, 2)) * sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)))) *
192  (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))) /
193  (2. * sqrt((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
194  (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))))) /
195  2.;
196 
197  dr(2, 3) = (-(sqrt(pow(dx, 2) + pow(dy, 2)) * dz *
198  (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))) /
199  (2. * pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) *
200  sqrt((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
201  (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) -
202  (sqrt(pow(dx, 2) + pow(dy, 2)) * dz *
203  (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))) /
204  (2. * pow(pow(dx, 2) + pow(dy, 2) + pow(dz, 2), 1.5) *
205  sqrt((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
206  (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))))) /
207  2.;
208 
209  //d/dp_i
210  //debug: x->p index xhange in denominator
211 
212  dr(2, 4) = (((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
213  ((px * sqrt(pow(px, 2) + pow(py, 2))) / pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5) -
214  px / (sqrt(pow(px, 2) + pow(py, 2)) * sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) /
215  (2. * sqrt((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
216  (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) -
217  ((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
218  (-((px * sqrt(pow(px, 2) + pow(py, 2))) / pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5)) +
219  px / (sqrt(pow(px, 2) + pow(py, 2)) * sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) /
220  (2. * sqrt((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
221  (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))))) /
222  2.;
223 
224  dr(2, 5) = (((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
225  ((py * sqrt(pow(px, 2) + pow(py, 2))) / pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5) -
226  py / (sqrt(pow(px, 2) + pow(py, 2)) * sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) /
227  (2. * sqrt((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
228  (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) -
229  ((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
230  (-((py * sqrt(pow(px, 2) + pow(py, 2))) / pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5)) +
231  py / (sqrt(pow(px, 2) + pow(py, 2)) * sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) /
232  (2. * sqrt((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
233  (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))))) /
234  2.;
235 
236  dr(2, 6) = (((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
237  sqrt(pow(px, 2) + pow(py, 2)) * pz) /
238  (2. * pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5) *
239  sqrt((1 + sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
240  (1 - sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2))))) +
241  ((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
242  sqrt(pow(px, 2) + pow(py, 2)) * pz) /
243  (2. * pow(pow(px, 2) + pow(py, 2) + pow(pz, 2), 1.5) *
244  sqrt((1 - sqrt(pow(dx, 2) + pow(dy, 2)) / sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2))) *
245  (1 + sqrt(pow(px, 2) + pow(py, 2)) / sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)))))) /
246  2.;
247 
248  dr(2, 7) = 0;
249 
250  // cout<<"derivative matrix "<<dr<<endl;
251  return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, point);
252 }
VertexException
Common base class.
Definition: VertexException.h:12
PV3DBase::y
T y() const
Definition: PV3DBase.h:60
SimplePointingConstraint::deviations
AlgebraicVector deviations(int nStates) const override
Definition: SimplePointingConstraint.cc:82
AlgebraicMatrix
CLHEP::HepMatrix AlgebraicMatrix
Definition: AlgebraicObjects.h:14
mathSSE::sqrt
T sqrt(T t)
Definition: SSEVec.h:19
PV3DBase::z
T z() const
Definition: PV3DBase.h:61
SimplePointingConstraint::derivative
std::pair< AlgebraicMatrix, AlgebraicVector > derivative(const AlgebraicVector &exPoint) const override
Definition: SimplePointingConstraint.cc:26
SimplePointingConstraint::makeValue
std::pair< AlgebraicVector, AlgebraicVector > makeValue(const AlgebraicVector &exPoint) const
Definition: SimplePointingConstraint.cc:86
SimplePointingConstraint::numberOfEquations
int numberOfEquations() const override
Definition: SimplePointingConstraint.cc:84
AlgebraicVector
CLHEP::HepVector AlgebraicVector
Definition: AlgebraicObjects.h:13
SimplePointingConstraint::value
std::pair< AlgebraicVector, AlgebraicVector > value(const AlgebraicVector &exPoint) const override
Definition: SimplePointingConstraint.cc:4
PV3DBase::x
T x() const
Definition: PV3DBase.h:59
funct::pow
Power< A, B >::type pow(const A &a, const B &b)
Definition: Power.h:29
point
*vegas h *****************************************************used in the default bin number in original ***version of VEGAS is ***a higher bin number might help to derive a more precise ***grade subtle point
Definition: invegas.h:5
SimplePointingConstraint::makeDerivative
std::pair< AlgebraicMatrix, AlgebraicVector > makeDerivative(const AlgebraicVector &exPoint) const
Definition: SimplePointingConstraint.cc:122
Generated for CMSSW Reference Manual by doxygen 1.8.5