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