BOSS 8.0.0
BESIII Offline Software System
Loading...
Searching...
No Matches
KalmanKinematicFit.cxx
Go to the documentation of this file.
1#include "VertexFit/KalmanKinematicFit.h"
2#include "TGraph2D.h"
3#include "TStopwatch.h"
4#include "VertexFit/HTrackParameter.h"
5#include "VertexFit/KinematicConstraints.h"
6#include <cmath>
7
8const int KalmanKinematicFit::NTRKPAR = 4;
9const int KalmanKinematicFit::Resonance = 1;
10const int KalmanKinematicFit::TotalEnergy = 2;
11const int KalmanKinematicFit::TotalMomentum = 4;
12const int KalmanKinematicFit::Position = 8;
13const int KalmanKinematicFit::ThreeMomentum = 16;
14const int KalmanKinematicFit::FourMomentum = 32;
15const int KalmanKinematicFit::EqualMass = 64;
16// TGraph2D *g = new
17// TGraph2D("/ihepbatch/bes/yanl/6.5.0//TestRelease/TestRelease-00-00-62/run/gamma/new/graph.dat");
18
19KalmanKinematicFit* KalmanKinematicFit::m_pointer = 0;
20
21KalmanKinematicFit* KalmanKinematicFit::instance() {
22 if ( m_pointer ) return m_pointer;
23 m_pointer = new KalmanKinematicFit();
24 return m_pointer;
25}
26
27KalmanKinematicFit::KalmanKinematicFit() { ; }
28
29KalmanKinematicFit::~KalmanKinematicFit() {
30 // if(m_pointer) delete m_pointer;
31 delete m_pointer;
32}
33
34void KalmanKinematicFit::init() {
35 clearWTrackOrigin();
36 clearWTrackInfit();
37 clearWTrackList();
38 // gamma shape
39 clearGammaShape();
40 clearGammaShapeList();
41 clearMapkinematic();
42 clearMappositionA();
43 clearMappositionB();
44 clearone();
45 cleartwo();
46 setBeamPosition( HepPoint3D( 0.0, 0.0, 0.0 ) );
47 setVBeamPosition( HepSymMatrix( 3, 0 ) );
48 //=============
49 m_kc.clear();
50 m_chisq.clear();
51 m_chi = 9999.;
52 m_niter = 10;
53 m_chicut = 200.;
54 m_chiter = 0.005;
55 m_espread = 0.0;
56 m_collideangle = 11e-3;
57 m_flag = 0;
58 m_dynamicerror = 0;
59 m_nc = 0;
60 m_cpu = HepVector( 10, 0 );
61 m_virtual_wtrk.clear();
62 m_graph2d = 0;
63 // m_graph2d =
64 // TGraph2D("/ihepbatch/bes/yanl/6.5.0//TestRelease/TestRelease-00-00-62/run/gamma/new/graph.dat");
65}
66
67//
68// Add Resonance
69//
70
71void KalmanKinematicFit::AddResonance( int number, double mres, int n1 ) {
72 std::vector<int> tlis = AddList( n1 );
73 AddResonance( number, mres, tlis );
74}
75
76void KalmanKinematicFit::AddResonance( int number, double mres, int n1, int n2 ) {
77 std::vector<int> tlis = AddList( n1, n2 );
78 AddResonance( number, mres, tlis );
79}
80
81void KalmanKinematicFit::AddResonance( int number, double mres, int n1, int n2, int n3 ) {
82 std::vector<int> tlis = AddList( n1, n2, n3 );
83 AddResonance( number, mres, tlis );
84}
85
86void KalmanKinematicFit::AddResonance( int number, double mres, int n1, int n2, int n3,
87 int n4 ) {
88 std::vector<int> tlis = AddList( n1, n2, n3, n4 );
89 AddResonance( number, mres, tlis );
90}
91
92void KalmanKinematicFit::AddResonance( int number, double mres, int n1, int n2, int n3, int n4,
93 int n5 ) {
94 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5 );
95 AddResonance( number, mres, tlis );
96}
97
98void KalmanKinematicFit::AddResonance( int number, double mres, int n1, int n2, int n3, int n4,
99 int n5, int n6 ) {
100 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6 );
101 AddResonance( number, mres, tlis );
102}
103
104void KalmanKinematicFit::AddResonance( int number, double mres, int n1, int n2, int n3, int n4,
105 int n5, int n6, int n7 ) {
106 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7 );
107 AddResonance( number, mres, tlis );
108}
109
110void KalmanKinematicFit::AddResonance( int number, double mres, int n1, int n2, int n3, int n4,
111 int n5, int n6, int n7, int n8 ) {
112 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7, n8 );
113 AddResonance( number, mres, tlis );
114}
115
116void KalmanKinematicFit::AddResonance( int number, double mres, int n1, int n2, int n3, int n4,
117 int n5, int n6, int n7, int n8, int n9 ) {
118 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7, n8, n9 );
119 AddResonance( number, mres, tlis );
120}
121
122void KalmanKinematicFit::AddResonance( int number, double mres, int n1, int n2, int n3, int n4,
123 int n5, int n6, int n7, int n8, int n9, int n10 ) {
124 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7, n8, n9, n10 );
125 AddResonance( number, mres, tlis );
126}
127
128void KalmanKinematicFit::AddResonance( int number, double mres, int n1, int n2, int n3, int n4,
129 int n5, int n6, int n7, int n8, int n9, int n10,
130 int n11 ) {
131 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7, n8, n9, n10, n11 );
132 AddResonance( number, mres, tlis );
133}
134
135void KalmanKinematicFit::AddResonance( int number, double mres, int n1, int n2, int n3, int n4,
136 int n5, int n6, int n7, int n8, int n9, int n10,
137 int n11, int n12 ) {
138 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7, n8, n9, n10, n11, n12 );
139 AddResonance( number, mres, tlis );
140}
141
142void KalmanKinematicFit::AddResonance( int number, double mres, std::vector<int> tlis ) {
143 KinematicConstraints kc;
144 HepSymMatrix Vre = HepSymMatrix( 1, 0 );
145 kc.ResonanceConstraints( mres, tlis, Vre );
146 m_kc.push_back( kc );
147 if ( (unsigned int)number != m_kc.size() - 1 )
148 std::cout << "wrong kinematic constraints index" << std::endl;
149}
150
151//
152// Add TotalMomentum
153//
154
155void KalmanKinematicFit::AddTotalMomentum( int number, double ptot, int n1 ) {
156 std::vector<int> tlis = AddList( n1 );
157 AddTotalMomentum( number, ptot, tlis );
158}
159void KalmanKinematicFit::AddTotalMomentum( int number, double ptot, int n1, int n2 ) {
160 std::vector<int> tlis = AddList( n1, n2 );
161 AddTotalMomentum( number, ptot, tlis );
162}
163
164void KalmanKinematicFit::AddTotalMomentum( int number, double ptot, int n1, int n2, int n3 ) {
165 std::vector<int> tlis = AddList( n1, n2, n3 );
166 AddTotalMomentum( number, ptot, tlis );
167}
168
169void KalmanKinematicFit::AddTotalMomentum( int number, double ptot, int n1, int n2, int n3,
170 int n4 ) {
171 std::vector<int> tlis = AddList( n1, n2, n3, n4 );
172 AddTotalMomentum( number, ptot, tlis );
173}
174
175void KalmanKinematicFit::AddTotalMomentum( int number, double ptot, int n1, int n2, int n3,
176 int n4, int n5 ) {
177 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5 );
178 AddTotalMomentum( number, ptot, tlis );
179}
180
181void KalmanKinematicFit::AddTotalMomentum( int number, double ptot, int n1, int n2, int n3,
182 int n4, int n5, int n6 ) {
183 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6 );
184 AddTotalMomentum( number, ptot, tlis );
185}
186
187void KalmanKinematicFit::AddTotalMomentum( int number, double ptot, int n1, int n2, int n3,
188 int n4, int n5, int n6, int n7 ) {
189 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7 );
190 AddTotalMomentum( number, ptot, tlis );
191}
192
193void KalmanKinematicFit::AddTotalMomentum( int number, double ptot, int n1, int n2, int n3,
194 int n4, int n5, int n6, int n7, int n8 ) {
195 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7, n8 );
196 AddTotalMomentum( number, ptot, tlis );
197}
198
199void KalmanKinematicFit::AddTotalMomentum( int number, double ptot, int n1, int n2, int n3,
200 int n4, int n5, int n6, int n7, int n8, int n9 ) {
201 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7, n8, n9 );
202 AddTotalMomentum( number, ptot, tlis );
203}
204
205void KalmanKinematicFit::AddTotalMomentum( int number, double ptot, int n1, int n2, int n3,
206 int n4, int n5, int n6, int n7, int n8, int n9,
207 int n10 ) {
208 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7, n8, n9, n10 );
209 AddTotalMomentum( number, ptot, tlis );
210}
211
212void KalmanKinematicFit::AddTotalMomentum( int number, double ptot, int n1, int n2, int n3,
213 int n4, int n5, int n6, int n7, int n8, int n9,
214 int n10, int n11 ) {
215 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7, n8, n9, n10, n11 );
216 AddTotalMomentum( number, ptot, tlis );
217}
218
219void KalmanKinematicFit::AddTotalMomentum( int number, double ptot, int n1, int n2, int n3,
220 int n4, int n5, int n6, int n7, int n8, int n9,
221 int n10, int n11, int n12 ) {
222 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7, n8, n9, n10, n11, n12 );
223 AddTotalMomentum( number, ptot, tlis );
224}
225
226void KalmanKinematicFit::AddTotalMomentum( int number, double ptot, std::vector<int> tlis ) {
227 KinematicConstraints kc;
228 kc.TotalMomentumConstraints( ptot, tlis );
229 m_kc.push_back( kc );
230 if ( (unsigned int)number != m_kc.size() - 1 )
231 std::cout << "wrong kinematic constraints index" << std::endl;
232}
233
234//
235// Add TotalEnergy
236//
237
238void KalmanKinematicFit::AddTotalEnergy( int number, double etot, int n1 ) {
239 std::vector<int> tlis = AddList( n1 );
240 AddTotalEnergy( number, etot, tlis );
241}
242void KalmanKinematicFit::AddTotalEnergy( int number, double etot, int n1, int n2 ) {
243 std::vector<int> tlis = AddList( n1, n2 );
244 AddTotalEnergy( number, etot, tlis );
245}
246
247void KalmanKinematicFit::AddTotalEnergy( int number, double etot, int n1, int n2, int n3 ) {
248 std::vector<int> tlis = AddList( n1, n2, n3 );
249 AddTotalEnergy( number, etot, tlis );
250}
251
252void KalmanKinematicFit::AddTotalEnergy( int number, double etot, int n1, int n2, int n3,
253 int n4 ) {
254 std::vector<int> tlis = AddList( n1, n2, n3, n4 );
255 AddTotalEnergy( number, etot, tlis );
256}
257
258void KalmanKinematicFit::AddTotalEnergy( int number, double etot, int n1, int n2, int n3,
259 int n4, int n5 ) {
260 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5 );
261 AddTotalEnergy( number, etot, tlis );
262}
263
264void KalmanKinematicFit::AddTotalEnergy( int number, double etot, int n1, int n2, int n3,
265 int n4, int n5, int n6 ) {
266 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6 );
267 AddTotalEnergy( number, etot, tlis );
268}
269
270void KalmanKinematicFit::AddTotalEnergy( int number, double etot, int n1, int n2, int n3,
271 int n4, int n5, int n6, int n7 ) {
272 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7 );
273 AddTotalEnergy( number, etot, tlis );
274}
275
276void KalmanKinematicFit::AddTotalEnergy( int number, double etot, int n1, int n2, int n3,
277 int n4, int n5, int n6, int n7, int n8 ) {
278 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7, n8 );
279 AddTotalEnergy( number, etot, tlis );
280}
281
282void KalmanKinematicFit::AddTotalEnergy( int number, double etot, int n1, int n2, int n3,
283 int n4, int n5, int n6, int n7, int n8, int n9 ) {
284 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7, n8, n9 );
285 AddTotalEnergy( number, etot, tlis );
286}
287
288void KalmanKinematicFit::AddTotalEnergy( int number, double etot, int n1, int n2, int n3,
289 int n4, int n5, int n6, int n7, int n8, int n9,
290 int n10 ) {
291 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7, n8, n9, n10 );
292 AddTotalEnergy( number, etot, tlis );
293}
294
295void KalmanKinematicFit::AddTotalEnergy( int number, double etot, int n1, int n2, int n3,
296 int n4, int n5, int n6, int n7, int n8, int n9,
297 int n10, int n11 ) {
298 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7, n8, n9, n10, n11 );
299 AddTotalEnergy( number, etot, tlis );
300}
301
302void KalmanKinematicFit::AddTotalEnergy( int number, double etot, int n1, int n2, int n3,
303 int n4, int n5, int n6, int n7, int n8, int n9,
304 int n10, int n11, int n12 ) {
305 std::vector<int> tlis = AddList( n1, n2, n3, n4, n5, n6, n7, n8, n9, n10, n11, n12 );
306 AddTotalEnergy( number, etot, tlis );
307}
308
309void KalmanKinematicFit::AddTotalEnergy( int number, double etot, std::vector<int> tlis ) {
310 KinematicConstraints kc;
311 kc.TotalEnergyConstraints( etot, tlis );
312 m_kc.push_back( kc );
313 if ( (unsigned int)number != m_kc.size() - 1 )
314 std::cout << "wrong kinematic constraints index" << std::endl;
315}
316//
317// Equal Mass constraints
318//
319
320void KalmanKinematicFit::AddEqualMass( int number, std::vector<int> tlis1,
321 std::vector<int> tlis2 ) {
322 KinematicConstraints kc;
323 HepSymMatrix Vne = HepSymMatrix( 1, 0 );
324 kc.EqualMassConstraints( tlis1, tlis2, Vne );
325 m_kc.push_back( kc );
326 if ( (unsigned int)number != m_kc.size() - 1 )
327 std::cout << "wrong kinematic constraints index" << std::endl;
328}
329
330//
331// Total 3-momentum constraints
332//
333
334void KalmanKinematicFit::AddThreeMomentum( int number, Hep3Vector p3 ) {
335 std::vector<int> tlis;
336 tlis.clear();
337 WTrackParameter wtrk;
338 KinematicConstraints kc;
339
340 for ( int i = 0; i < numberWTrack(); i++ ) { tlis.push_back( i ); }
341 kc.ThreeMomentumConstraints( p3, tlis );
342 m_kc.push_back( kc );
343 if ( (unsigned int)number != m_kc.size() - 1 )
344 std::cout << "wrong kinematic constraints index" << std::endl;
345}
346
347//
348// Total 4-momentum constraints
349//
350
351void KalmanKinematicFit::AddFourMomentum( int number, HepLorentzVector p4 ) {
352
353 std::vector<int> tlis;
354 tlis.clear();
355 KinematicConstraints kc;
356
357 for ( int i = 0; i < numberWTrack(); i++ ) { tlis.push_back( i ); }
358
359 HepSymMatrix Vme = HepSymMatrix( 4, 0 );
360 Vme[0][0] = 2 * m_espread * m_espread * sin( m_collideangle ) * sin( m_collideangle );
361 Vme[0][3] = 2 * m_espread * m_espread * sin( m_collideangle );
362 Vme[3][3] = 2 * m_espread * m_espread;
363
364 kc.FourMomentumConstraints( p4, tlis, Vme );
365 m_kc.push_back( kc );
366 if ( (unsigned int)number != m_kc.size() - 1 )
367 std::cout << "wrong kinematic constraints index" << std::endl;
368}
369
370void KalmanKinematicFit::AddFourMomentum( int number, double etot ) {
371
372 HepLorentzVector p4( 0.0, 0.0, 0.0, etot );
373 std::vector<int> tlis;
374 tlis.clear();
375 KinematicConstraints kc;
376
377 for ( int i = 0; i < numberWTrack(); i++ ) { tlis.push_back( i ); }
378 HepSymMatrix Vme = HepSymMatrix( 4, 0 );
379 Vme[3][3] = 2 * m_espread * m_espread;
380 kc.FourMomentumConstraints( p4, tlis, Vme );
381 m_kc.push_back( kc );
382 if ( (unsigned int)number != m_kc.size() - 1 )
383 std::cout << "wrong kinematic constraints index" << std::endl;
384}
385
386void KalmanKinematicFit::fit() {
387
388 KinematicConstraints kc;
389 int nc = m_nc;
390 int ntrk = numberWTrack();
391
392 TStopwatch timer;
393
394 timer.Start();
395 // cout<<" =====calculate AC_{0}A^{T}====="<<endl;
396 HepSymMatrix AC( m_nc, 0 );
397 AC = m_C0.similarity( m_A );
398 timer.Stop();
399 m_cpu[1] += timer.CpuTime();
400
401 timer.Start();
402 // cout<<" =====calculate W====="<<endl;
403 int ifail;
404 m_W = HepSymMatrix( m_nc, 0 );
405 m_W = ( m_Vm + m_C0.similarity( m_A ) ).inverse( ifail );
406 // cout<<" =====calculate D , D^{-1}====="<<endl;
407 int ifailD;
408 m_Dinv = m_D0inv + m_W.similarity( m_B.T() );
409 m_D = m_Dinv.inverse( ifailD );
410 timer.Stop();
411 m_cpu[2] += timer.CpuTime();
412
413 timer.Start();
414 // cout<<"===== G equals r ====="<<endl;
415 HepVector G( m_nc, 0 );
416 G = m_A * ( m_p0 - m_p ) + m_B * ( m_q0 - m_q ) + m_G;
417 // cout<<"===== calculate KQ, Kp======"<<endl;
418 m_KQ = HepMatrix( numbertwo(), m_nc, 0 );
419 m_KQ = m_D * m_BT * m_W;
420
421 m_KP = HepMatrix( numberone(), m_nc, 0 );
422 m_KP = m_C0 * m_AT * m_W - m_C0 * m_AT * m_W * m_B * m_KQ;
423 // ===== update track par =====
424 m_p = m_p0 - m_KP * G;
425 m_q = m_q0 - m_KQ * G;
426 timer.Stop();
427 m_cpu[4] += timer.CpuTime();
428
429 timer.Start();
430 //===== caluculate chi2 =====
431 m_chi = ( m_W.similarity( G.T() ) - m_Dinv.similarity( ( m_q - m_q0 ).T() ) )[0][0];
432 timer.Stop();
433 m_cpu[3] += timer.CpuTime();
434 timer.Start();
435 if ( m_dynamicerror == 1 ) gda();
436
437 timer.Stop();
438 m_cpu[6] += timer.CpuTime();
439}
440
441void KalmanKinematicFit::upCovmtx() {
442 HepMatrix E( numberone(), numbertwo(), 0 );
443 E = -m_C0 * m_A.T() * m_KQ.T();
444 m_C = m_C0 - m_W.similarity( ( m_A * m_C0 ).T() ) + m_Dinv.similarity( E );
445}
446
447void KalmanKinematicFit::fit( int n ) {
448 if ( m_chisq.size() == 0 )
449 {
450 for ( unsigned int i = 0; i < m_kc.size(); i++ ) m_chisq.push_back( 9999.0 );
451 }
452
453 KinematicConstraints kc;
454 int nc = m_nc;
455 int ntrk = numberWTrack();
456
457 // cout<<" =====calculate AC_{0}A^{T}====="<<endl;
458 HepSymMatrix AC( m_nc, 0 );
459 AC = m_C0.similarity( m_A );
460
461 // cout<<" =====calculate W====="<<endl;
462 int ifail;
463 m_W = HepSymMatrix( m_nc, 0 );
464 m_W = ( m_Vm + m_C0.similarity( m_A ) ).inverse( ifail );
465 // cout<<" =====calculate D , D^{-1}====="<<endl;
466 int ifailD;
467 m_Dinv = m_D0inv + m_W.similarity( m_B.T() );
468 m_D = m_Dinv.inverse( ifailD );
469
470 // cout<<"===== G equals r ====="<<endl;
471 HepVector G( m_nc, 0 );
472 G = m_A * ( m_p0 - m_p ) + m_B * ( m_q0 - m_q ) + m_G;
473 // cout<<"===== calculate KQ, Kp======"<<endl;
474 m_KQ = HepMatrix( numbertwo(), m_nc, 0 );
475 m_KQ = m_D * m_BT * m_W;
476
477 m_KP = HepMatrix( numberone(), m_nc, 0 );
478 m_KP = m_C0 * m_AT * m_W - m_C0 * m_AT * m_W * m_B * m_KQ;
479 // ===== update track par =====
480 m_p = m_p0 - m_KP * G;
481 m_q = m_q0 - m_KQ * G;
482
483 //===== caluculate chi2 =====
484 m_chisq[n] = ( m_W.similarity( G.T() ) - m_Dinv.similarity( ( m_q - m_q0 ).T() ) )[0][0];
485 if ( m_dynamicerror == 1 ) gda();
486}
487
488bool KalmanKinematicFit::Fit() {
489 bool okfit = false;
490 TStopwatch timer;
491 m_nktrk = numberWTrack();
492 m_p0 = HepVector( numberone(), 0 );
493 m_p = HepVector( numberone(), 0 );
494 m_q0 = HepVector( numbertwo(), 0 );
495 m_q = HepVector( numbertwo(), 0 );
496 m_C0 = HepSymMatrix( numberone(), 0 );
497 m_C = HepSymMatrix( numberone(), 0 );
498 m_D0inv = HepSymMatrix( numbertwo(), 0 );
499 m_D = HepSymMatrix( numbertwo(), 0 );
500
501 HepVector m_p_tmp = HepVector( numberone(), 0 );
502 HepVector m_q_tmp = HepVector( numbertwo(), 0 );
503 HepMatrix m_A_tmp;
504 HepSymMatrix m_W_tmp;
505 HepMatrix m_KQ_tmp;
506 HepSymMatrix m_Dinv_tmp;
507 for ( int i = 0; i < numberWTrack(); i++ )
508 {
509 setWTrackInfit( i, wTrackOrigin( i ) );
510 int pa = mappositionA()[i] + 1;
511 int pb = mappositionB()[i] + 1;
512 int ptype = mapkinematic()[i];
513 switch ( ptype )
514 {
515 case 0: {
516 HepVector w1( 4, 0 );
517 HepSymMatrix Ew1( 4, 0 );
518 for ( int j = 0; j < 3; j++ ) { w1[j] = wTrackOrigin( i ).w()[j]; }
519 w1[3] = wTrackOrigin( i ).mass();
520
521 for ( int m = 0; m < 3; m++ )
522 {
523 for ( int n = 0; n < 3; n++ ) { Ew1[m][n] = wTrackOrigin( i ).Ew()[m][n]; }
524 }
525 setPOrigin( pa, w1 );
526 setPInfit( pa, w1 );
527 setCOrigin( pa, Ew1 );
528 break;
529 }
530 case 1: {
531 setPOrigin( pa, ( wTrackOrigin( i ).plmp() ).sub( 1, 4 ) );
532 setPInfit( pa, ( wTrackOrigin( i ).plmp() ).sub( 1, 4 ) );
533 setCOrigin( pa, ( wTrackOrigin( i ).Vplm() ).sub( 1, 4 ) );
534 break;
535 }
536 case 2: {
537 setQOrigin( pb, ( wTrackOrigin( i ).w() ).sub( 1, NTRKPAR ) );
538 setQInfit( pb, ( wTrackOrigin( i ).w() ).sub( 1, NTRKPAR ) );
539 HepSymMatrix Dinv( 4, 0 );
540 setDOriginInv( pb, Dinv );
541 break;
542 }
543 case 3: {
544 setPOrigin( pa, ( wTrackOrigin( i ).plmp() ).sub( 3, 3 ) );
545 setPInfit( pa, ( wTrackOrigin( i ).plmp() ).sub( 3, 3 ) );
546 setCOrigin( pa, ( wTrackOrigin( i ).Vplm() ).sub( 3, 3 ) );
547 setQOrigin( pb, ( wTrackOrigin( i ).plmp() ).sub( 1, 2 ) );
548 setQInfit( pb, ( wTrackOrigin( i ).plmp() ).sub( 1, 2 ) );
549 setQOrigin( pb + 2, ( wTrackOrigin( i ).plmp() ).sub( 4, 4 ) );
550 setQInfit( pb + 2, ( wTrackOrigin( i ).plmp() ).sub( 4, 4 ) );
551 HepSymMatrix Dinv( 3, 0 );
552 setDOriginInv( pb, Dinv );
553 break;
554 }
555 case 4: {
556 setPOrigin( pa, ( wTrackOrigin( i ).plmp() ).sub( 1, 2 ) );
557 setPInfit( pa, ( wTrackOrigin( i ).plmp() ).sub( 1, 2 ) );
558 setCOrigin( pa, ( wTrackOrigin( i ).Vplm() ).sub( 1, 2 ) );
559 setQOrigin( pb, ( wTrackOrigin( i ).plmp() ).sub( 3, 4 ) );
560 setQInfit( pb, ( wTrackOrigin( i ).plmp() ).sub( 3, 4 ) );
561 HepSymMatrix Dinv( 2, 0 );
562 setDOriginInv( pb, Dinv );
563 break;
564 }
565 case 5: {
566 setPOrigin( pa, ( wTrackOrigin( i ).plmp() ).sub( 1, 3 ) );
567 setPInfit( pa, ( wTrackOrigin( i ).plmp() ).sub( 1, 3 ) );
568 setCOrigin( pa, ( wTrackOrigin( i ).Vplm() ).sub( 1, 3 ) );
569 setQOrigin( pb, ( wTrackOrigin( i ).plmp() ).sub( 4, 4 ) );
570 setQInfit( pb, ( wTrackOrigin( i ).plmp() ).sub( 4, 4 ) );
571 HepSymMatrix Dinv( 1, 0 );
572 setDOriginInv( pb, Dinv );
573 break;
574 }
575 case 6: {
576 setPOrigin( pa, ( wTrackOrigin( i ).w() ).sub( 5, 7 ) );
577 setPOrigin( pa + 3, ( wTrackOrigin( i ).w() ).sub( 4, 4 ) );
578 setPInfit( pa, ( wTrackOrigin( i ).w() ).sub( 5, 7 ) );
579 setPInfit( pa + 3, ( wTrackOrigin( i ).w() ).sub( 4, 4 ) );
580 setCOrigin( pa, ( wTrackOrigin( i ).Ew() ).sub( 5, 7 ) );
581 setCOrigin( pa + 3, ( wTrackOrigin( i ).Ew() ).sub( 4, 4 ) );
582 HepVector beam( 3, 0 );
583 beam[0] = getBeamPosition().x();
584 beam[1] = getBeamPosition().y();
585 beam[2] = getBeamPosition().z();
586 setQOrigin( pb, beam );
587 setQInfit( pb, beam );
588 HepSymMatrix Dinv( 3, 0 );
589 int ifail;
590 Dinv = getVBeamPosition().inverse( ifail );
591 setDOriginInv( pb, Dinv );
592 break;
593 }
594 case 7: {
595 HepVector w1( 4, 0 );
596 HepSymMatrix Ew1( 4, 0 );
597 for ( int j = 0; j < 3; j++ ) { w1[j] = wTrackOrigin( i ).w()[j]; }
598 w1[3] = wTrackOrigin( i ).mass();
599
600 for ( int m = 0; m < 3; m++ )
601 {
602 for ( int n = 0; n < 3; n++ ) { Ew1[m][n] = wTrackOrigin( i ).Ew()[m][n]; }
603 }
604 setPOrigin( pa, w1 );
605 setPInfit( pa, w1 );
606 setCOrigin( pa, Ew1 );
607 break;
608 }
609 }
610 }
611
612 //
613 // iteration
614 //
615
616 std::vector<double> chisq;
617 chisq.clear();
618 int nc = 0;
619 for ( int i = 0; i < m_kc.size(); i++ ) nc += m_kc[i].nc();
620
621 m_A = HepMatrix( nc, numberone(), 0 );
622 m_AT = HepMatrix( numberone(), nc, 0 );
623 m_B = HepMatrix( nc, numbertwo(), 0 );
624 m_BT = HepMatrix( numbertwo(), nc, 0 );
625 m_G = HepVector( nc, 0 );
626 m_Vm = HepSymMatrix( nc, 0 );
627 int num1 = 0;
628 for ( unsigned int i = 0; i < m_kc.size(); i++ )
629 {
630 KinematicConstraints kc = m_kc[i];
631 m_Vm.sub( num1 + 1, kc.Vmeasure() );
632 num1 = num1 + kc.nc();
633 }
634
635 double tmp_chisq = 999;
636 bool flag_break = 0;
637 for ( int it = 0; it < m_niter; it++ )
638 {
639 timer.Start();
640 m_nc = 0;
641 for ( unsigned int i = 0; i < m_kc.size(); i++ )
642 {
643 KinematicConstraints kc = m_kc[i];
644 updateConstraints( kc );
645 }
646 timer.Stop();
647 m_cpu[0] += timer.CpuTime();
648 fit();
649 //
650 // reset origin parameters for virtual particle
651 //
652 // if(it == 0){
653 // for(int i = 0; i < numberWTrack(); i++) {
654 // // setWTrackInfit(i, wTrackOrigin(i));
655 // int pa = mappositionA()[i] + 1;
656 // int pb = mappositionB()[i] + 1;
657 // int ptype = mapkinematic()[i];
658 // switch(ptype) {
659 // case 3 : {
660 // setPOrigin(pa, m_p.sub(pa, pa));
661 // setPInfit(pa, m_p.sub(pa, pa));
662 // setQOrigin(pb, m_q.sub(pb, pb+2));
663 // setQInfit(pb, m_q.sub(pb, pb+2));
664 // break;
665 // }
666 // case 4 : {
667 // setPOrigin(pa, (wTrackOrigin(i).plmp()).sub(1,
668 // 2)); setPInfit(pa,
669 // (wTrackOrigin(i).plmp()).sub(1, 2));
670 // setCOrigin(pa, (wTrackOrigin(i).Vplm()).sub(1,
671 // 2)); setQOrigin(pb,
672 // (wTrackOrigin(i).plmp()).sub(3, 4));
673 // setQInfit(pb, (wTrackOrigin(i).plmp()).sub(3,
674 // 4)); HepSymMatrix Dinv(2,0); setDOriginInv(pb,
675 // Dinv); break;
676 // }
677 // case 5 : {
678 // setPOrigin(pa, (wTrackOrigin(i).plmp()).sub(1,
679 // 3)); setPInfit(pa,
680 // (wTrackOrigin(i).plmp()).sub(1, 3));
681 // setCOrigin(pa, (wTrackOrigin(i).Vplm()).sub(1,
682 // 3)); setQOrigin(pb,
683 // (wTrackOrigin(i).plmp()).sub(4, 4));
684 // setQInfit(pb, (wTrackOrigin(i).plmp()).sub(4,
685 // 4)); HepSymMatrix Dinv(1,0); setDOriginInv(pb,
686 // Dinv); break;
687 // }
688 // case 6 : {
689 // setPOrigin(pa, (wTrackOrigin(i).w()).sub(5, 7));
690 // setPOrigin(pa+3, (wTrackOrigin(i).w()).sub(4,
691 // 4)); setPInfit(pa, (wTrackOrigin(i).w()).sub(5,
692 // 7)); setPInfit(pa+3, (wTrackOrigin(i).w()).sub(4,
693 // 4)); setCOrigin(pa,
694 // (wTrackOrigin(i).Ew()).sub(5,7));
695 // setCOrigin(pa+3,
696 // (wTrackOrigin(i).Ew()).sub(4,4)); HepVector
697 // beam(3,0); beam[0] = getBeamPosition().x();
698 // beam[1] = getBeamPosition().y();
699 // beam[2] = getBeamPosition().z();
700 // setQOrigin(pb, beam);
701 // setQInfit(pb, beam);
702 // HepSymMatrix Dinv(3, 0);
703 // int ifail;
704 // Dinv = getVBeamPosition().inverse(ifail);
705 // setDOriginInv(pb, Dinv);
706 // break;
707 // }
708 //
709 // }
710 // }
711 //
712 // }
713 //
714 //
715 //===================reset over=============================
716 //
717 chisq.push_back( m_chi );
718 if ( it > 0 )
719 {
720 double delchi = chisq[it] - chisq[it - 1];
721 if ( fabs( delchi ) < m_chiter )
722 {
723 flag_break = 1;
724 break;
725 }
726 }
727 }
728 if ( !flag_break ) { return okfit; }
729 if ( m_chi > m_chicut ) return okfit;
730 timer.Start();
731 timer.Stop();
732 m_cpu[5] += timer.CpuTime();
733 okfit = true;
734 return okfit;
735}
736
737bool KalmanKinematicFit::Fit( int n ) {
738 bool okfit = false;
739 if ( n < 0 || (unsigned int)n >= m_kc.size() ) return okfit;
740 m_nktrk = numberWTrack();
741 m_p0 = HepVector( numberone(), 0 );
742 m_p = HepVector( numberone(), 0 );
743 m_q0 = HepVector( numbertwo(), 0 );
744 m_q = HepVector( numbertwo(), 0 );
745 m_C0 = HepSymMatrix( numberone(), 0 );
746 m_C = HepSymMatrix( numberone(), 0 );
747 m_D0inv = HepSymMatrix( numbertwo(), 0 );
748 m_D = HepSymMatrix( numbertwo(), 0 );
749
750 for ( int i = 0; i < numberWTrack(); i++ )
751 {
752 setWTrackInfit( i, wTrackOrigin( i ) );
753 int pa = mappositionA()[i] + 1;
754 int pb = mappositionB()[i] + 1;
755 int ptype = mapkinematic()[i];
756 switch ( ptype )
757 {
758 case 0: {
759 HepVector w1( 4, 0 );
760 HepSymMatrix Ew1( 4, 0 );
761 for ( int j = 0; j < 3; j++ ) { w1[j] = wTrackOrigin( i ).w()[j]; }
762 w1[3] = wTrackOrigin( i ).mass();
763
764 for ( int m = 0; m < 3; m++ )
765 {
766 for ( int n = 0; n < 3; n++ ) { Ew1[m][n] = wTrackOrigin( i ).Ew()[m][n]; }
767 }
768 setPOrigin( pa, w1 );
769 setPInfit( pa, w1 );
770 setCOrigin( pa, Ew1 );
771 break;
772 }
773 case 1: {
774 setPOrigin( pa, ( wTrackOrigin( i ).plmp() ).sub( 1, 4 ) );
775 setPInfit( pa, ( wTrackOrigin( i ).plmp() ).sub( 1, 4 ) );
776 setCOrigin( pa, ( wTrackOrigin( i ).Vplm() ).sub( 1, 4 ) );
777 break;
778 }
779 case 2: {
780 setQOrigin( pb, ( wTrackOrigin( i ).w() ).sub( 1, NTRKPAR ) );
781 setQInfit( pb, ( wTrackOrigin( i ).w() ).sub( 1, NTRKPAR ) );
782 HepSymMatrix Dinv( 4, 0 );
783 setDOriginInv( pb, Dinv );
784 break;
785 }
786 case 3: {
787 setPOrigin( pa, ( wTrackOrigin( i ).plmp() ).sub( 3, 3 ) );
788 setPInfit( pa, ( wTrackOrigin( i ).plmp() ).sub( 3, 3 ) );
789 setCOrigin( pa, ( wTrackOrigin( i ).Vplm() ).sub( 3, 3 ) );
790 setQOrigin( pb, ( wTrackOrigin( i ).w() ).sub( 1, 3 ) );
791 setQInfit( pb, ( wTrackOrigin( i ).w() ).sub( 1, 3 ) );
792 HepSymMatrix Dinv( 3, 0 );
793 setDOriginInv( pb, Dinv );
794 break;
795 }
796 case 4: {
797 setPOrigin( pa, ( wTrackOrigin( i ).plmp() ).sub( 1, 2 ) );
798 setPInfit( pa, ( wTrackOrigin( i ).plmp() ).sub( 1, 2 ) );
799 setCOrigin( pa, ( wTrackOrigin( i ).Vplm() ).sub( 1, 2 ) );
800 setQOrigin( pb, ( wTrackOrigin( i ).plmp() ).sub( 3, 4 ) );
801 setQInfit( pb, ( wTrackOrigin( i ).plmp() ).sub( 3, 4 ) );
802 HepSymMatrix Dinv( 2, 0 );
803 setDOriginInv( pb, Dinv );
804 break;
805 }
806 case 5: {
807 setPOrigin( pa, ( wTrackOrigin( i ).plmp() ).sub( 1, 3 ) );
808 setPInfit( pa, ( wTrackOrigin( i ).plmp() ).sub( 1, 3 ) );
809 setCOrigin( pa, ( wTrackOrigin( i ).Vplm() ).sub( 1, 3 ) );
810 setQOrigin( pb, ( wTrackOrigin( i ).plmp() ).sub( 4, 4 ) );
811 setQInfit( pb, ( wTrackOrigin( i ).plmp() ).sub( 4, 4 ) );
812 HepSymMatrix Dinv( 1, 0 );
813 setDOriginInv( pb, Dinv );
814 break;
815 }
816 case 6: {
817 setPOrigin( pa, ( wTrackOrigin( i ).w() ).sub( 5, 7 ) );
818 setPOrigin( pa + 3, ( wTrackOrigin( i ).w() ).sub( 4, 4 ) );
819 setPInfit( pa, ( wTrackOrigin( i ).w() ).sub( 5, 7 ) );
820 setPInfit( pa + 3, ( wTrackOrigin( i ).w() ).sub( 4, 4 ) );
821 setCOrigin( pa, ( wTrackOrigin( i ).Ew() ).sub( 5, 7 ) );
822 setCOrigin( pa + 3, ( wTrackOrigin( i ).Ew() ).sub( 4, 4 ) );
823 HepVector beam( 3, 0 );
824 beam[0] = getBeamPosition().x();
825 beam[1] = getBeamPosition().y();
826 beam[2] = getBeamPosition().z();
827 setQOrigin( pb, beam );
828 setQInfit( pb, beam );
829 HepSymMatrix Dinv( 3, 0 );
830 int ifail;
831 Dinv = getVBeamPosition().inverse( ifail );
832
833 setDOriginInv( pb, Dinv );
834 break;
835 }
836
837 case 7: {
838 HepVector w1( 4, 0 );
839 HepSymMatrix Ew1( 4, 0 );
840 for ( int j = 0; j < 3; j++ ) { w1[j] = wTrackOrigin( i ).w()[j]; }
841 w1[3] = wTrackOrigin( i ).mass();
842
843 for ( int m = 0; m < 3; m++ )
844 {
845 for ( int n = 0; n < 3; n++ ) { Ew1[m][n] = wTrackOrigin( i ).Ew()[m][n]; }
846 }
847 setPOrigin( pa, w1 );
848 setPInfit( pa, w1 );
849 setCOrigin( pa, Ew1 );
850 break;
851 }
852 }
853 }
854
855 //
856 // iteration
857 //
858
859 std::vector<double> chisq;
860 chisq.clear();
861 int nc = 0;
862 // for(int i = 0; i < m_kc.size(); i++)
863 nc += m_kc[n].nc();
864
865 m_A = HepMatrix( nc, numberone(), 0 );
866 m_AT = HepMatrix( numberone(), nc, 0 );
867 m_B = HepMatrix( nc, numbertwo(), 0 );
868 m_BT = HepMatrix( numbertwo(), nc, 0 );
869 m_G = HepVector( nc, 0 );
870 m_Vm = HepSymMatrix( nc, 0 );
871 int num1 = 0;
872 KinematicConstraints kc = m_kc[n];
873 m_Vm.sub( num1 + 1, kc.Vmeasure() );
874 num1 = kc.nc();
875 for ( int it = 0; it < m_niter; it++ )
876 {
877 m_nc = 0;
878 KinematicConstraints kc = m_kc[n];
879 updateConstraints( kc );
880 fit( n );
881
882 chisq.push_back( m_chisq[n] );
883
884 if ( it > 0 )
885 {
886
887 double delchi = chisq[it] - chisq[it - 1];
888 if ( fabs( delchi ) < m_chiter ) break;
889 }
890 }
891 if ( m_chisq[n] >= m_chicut ) { return okfit; }
892 // update track parameter and its covariance matrix
893 // upCovmtx();
894
895 okfit = true;
896
897 return okfit;
898}
899
900void KalmanKinematicFit::BuildVirtualParticle( int n ) {
901 upCovmtx();
902 KinematicConstraints kc = m_kc[n];
903 int ntrk = ( kc.Ltrk() ).size();
904 int charge = 0;
905 HepVector w( 7, 0 );
906 HepSymMatrix ew1( 7, 0 );
907 HepSymMatrix ew2( 7, 0 );
908 HepVector w4( 4, 0 );
909 HepSymMatrix ew4( 4, 0 );
910 HepMatrix dwdp( 4, 4, 0 );
911 dwdp[0][0] = 1;
912 dwdp[1][1] = 1;
913 dwdp[2][2] = 1;
914 dwdp[3][3] = 1;
915 for ( int i = 0; i < ntrk; i++ )
916 {
917 int itk = ( kc.Ltrk() )[i];
918 charge += wTrackInfit( itk ).charge();
919 w4 = w4 + dwdp * pInfit( itk );
920 // ew = ew + (getCInfit(itk)).similarity(dwdp);
921 }
922 HepMatrix I( 4, numberone(), 0 );
923 for ( int j2 = 0; j2 < ntrk; j2++ )
924 {
925 I[0][0 + j2 * 4] = 1;
926 I[1][1 + j2 * 4] = 1;
927 I[2][2 + j2 * 4] = 1;
928 I[3][3 + j2 * 4] = 1;
929 }
930 ew4 = m_C.similarity( I );
931 HepMatrix J( 7, 4, 0 );
932 double px = w4[0];
933 double py = w4[1];
934 double pz = w4[2];
935 double e = w4[3];
936 double pt = sqrt( px * px + py * py );
937 double p0 = sqrt( px * px + py * py + pz * pz );
938 double m = sqrt( e * e - p0 * p0 );
939 J[0][0] = -py;
940 J[0][1] = -( pz * px * pt ) / ( p0 * p0 );
941 J[0][2] = -m * px / ( p0 * p0 );
942 J[0][3] = e * px / ( p0 * p0 );
943 J[1][0] = px;
944 J[1][1] = -( pz * py * pt ) / ( p0 * p0 );
945 J[1][2] = -m * py / ( p0 * p0 );
946 J[1][3] = e * py / ( p0 * p0 );
947 J[2][0] = 0;
948 J[2][1] = pt * pt * pt / ( p0 * p0 );
949 J[2][2] = -m * pz / ( p0 * p0 );
950 J[2][3] = e * pz / ( p0 * p0 );
951 J[3][0] = 0;
952 J[3][1] = 0;
953 J[3][2] = 0;
954 J[3][3] = 1;
955 ew1 = ew4.similarity( J );
956 ew2[0][0] = ew1[0][0];
957 ew2[1][1] = ew1[1][1];
958 ew2[2][2] = ew1[2][2];
959 ew2[3][3] = ew1[3][3];
960 w[0] = w4[0];
961 w[1] = w4[1];
962 w[2] = w4[2];
963 w[3] = w4[3];
964 WTrackParameter vwtrk;
965 vwtrk.setCharge( charge );
966 vwtrk.setW( w );
967 vwtrk.setEw( ew2 );
968 vwtrk.setMass( m );
969 m_virtual_wtrk.push_back( vwtrk );
970}
971
972void KalmanKinematicFit::gda() {
973 // TGraph2D *g = new
974 // TGraph2D("/ihepbatch/bes/yanl/6.5.0//TestRelease/TestRelease-00-00-62/run/gamma/new/graph.dat");
975 for ( int i = 0; i < numberWTrack(); i++ )
976 {
977 if ( ( wTrackOrigin( i ) ).type() == 2 )
978 {
979 int n=0;
980 for ( int j = 0; j < numberGammaShape(); j++ )
981 {
982 if ( i == GammaShapeList( j ) ) n = j;
983 }
984 int pa = mappositionA()[i] + 1;
985 int pb = mappositionB()[i] + 1;
986 int ptype = mapkinematic()[i];
987 HepSymMatrix Ew( NTRKPAR, 0 );
988 HepLorentzVector p1 = p4Infit( i );
989 HepLorentzVector p2 = p4Origin( i );
990 double eorigin = pOrigin( i )[3];
991 HepMatrix dwda1( NTRKPAR, 3, 0 );
992 dwda1[0][0] = 1;
993 dwda1[1][1] = -( p2.e() * p2.e() ) / ( p2.px() * p2.px() + p2.py() * p2.py() );
994 // dwda1[1][1] = - (p1.e()*p1.e())/(p1.px()*p1.px() + p1.py()*p1.py());
995 dwda1[3][2] = 1;
996 HepSymMatrix emcmea1( 3, 0 );
997 double pk = p1[3];
998 // double pk =
999 // GammaShapeValue(n).peak(p1[3]);
1000 emcmea1[0][0] = GammaShapeValue( n ).getdphi() * GammaShapeValue( n ).getdphi();
1001 emcmea1[1][1] = GammaShapeValue( n ).getdthe() * GammaShapeValue( n ).getdthe() *
1002 ( 1 / ( 1 - p2.cosTheta() * p2.cosTheta() ) ) *
1003 ( 1 / ( 1 - p2.cosTheta() * p2.cosTheta() ) );
1004 // cout<<"delta lambda ="<<emcmea1[1][1]<<endl;
1005 emcmea1[2][2] =
1006 GammaShapeValue( n ).de( eorigin, pk ) * GammaShapeValue( n ).de( eorigin, pk );
1007 // double tmp_e22 = m_graph2d->Interpolate(eorigin, pk);
1008 // if(fabs((eorigin/pk)-1)<0.05&&tmp_e22==0)emcmea1[2][2] =
1009 // 0.025*pk*0.025; else emcmea1[2][2] = (tmp_e22*tmp_e22);
1010 // emcmea1[2][2] = m_graph2d->Interpolate(eorigin,
1011 // pk)*m_graph2d->Interpolate(eorigin, pk);
1012 // emcmea1[2][2] =
1013 // GammaShapeValue(n).de(eorigin,pk) * GammaShapeValue(n).de(eorigin,pk);
1014 // cout<<"dy_e ="<<GammaShapeValue(n).de(eorigin,pk)<<endl;
1015 // Ew = emcmea1.similarity(dwda1);
1016 Ew[0][0] = emcmea1[0][0];
1017 Ew[1][1] = emcmea1[1][1];
1018 Ew[3][3] = emcmea1[2][2];
1019 // cout<<"Ew ="<<Ew<<endl;
1020 if ( ptype == 6 ) setCOrigin( pa + 3, Ew.sub( 4, 4 ) );
1021 else setCOrigin( pa, Ew );
1022 }
1023 }
1024}
1025
1026HepVector KalmanKinematicFit::pull( int n ) {
1027 upCovmtx();
1028 int pa = mappositionA()[n] + 1;
1029 int pb = mappositionB()[n] + 1;
1030 int ptype = mapkinematic()[n];
1031 switch ( ptype )
1032 {
1033 case 0: {
1034 HepVector W( 7, 0 );
1035 HepSymMatrix Ew( 7, 0 );
1036 HepVector W1( 7, 0 );
1037 HepSymMatrix Ew1( 7, 0 );
1038 WTrackParameter wtrk = wTrackOrigin( n );
1039 W = wTrackOrigin( n ).w();
1040 Ew = wTrackOrigin( n ).Ew();
1041 for ( int i = 0; i < 4; i++ ) { W[i] = pInfit( n )[i]; }
1042 for ( int j = 0; j < 4; j++ )
1043 {
1044 for ( int k = 0; k < 4; k++ ) { Ew[j][k] = getCInfit( n )[j][k]; }
1045 }
1046 W1 = W;
1047 double px = p4Infit( n ).px();
1048 double py = p4Infit( n ).py();
1049 double pz = p4Infit( n ).pz();
1050 double m = p4Infit( n ).m();
1051 double e = p4Infit( n ).e();
1052 HepMatrix J( 7, 7, 0 );
1053 J[0][0] = 1;
1054 J[1][1] = 1;
1055 J[2][2] = 1;
1056 J[3][0] = px / e;
1057 J[3][1] = py / e;
1058 J[3][2] = pz / e;
1059 J[3][3] = m / e;
1060 J[4][4] = 1;
1061 J[5][5] = 1;
1062 J[6][6] = 1;
1063 Ew1 = Ew.similarity( J );
1064
1065 wtrk.setW( W1 );
1066 wtrk.setEw( Ew1 );
1067 setWTrackInfit( n, wtrk );
1068
1069 HTrackParameter horigin = HTrackParameter( wTrackOrigin( n ) );
1070 HTrackParameter hinfit = HTrackParameter( wTrackInfit( n ) );
1071 HepVector a0 = horigin.hel();
1072 HepVector a1 = hinfit.hel();
1073 HepSymMatrix v0 = horigin.eHel();
1074 HepSymMatrix v1 = hinfit.eHel();
1075 HepVector pull( 9, 0 );
1076 for ( int k = 0; k < 5; k++ )
1077 { pull[k] = ( a0[k] - a1[k] ) / sqrt( abs( v0[k][k] - v1[k][k] ) ); }
1078 for ( int l = 5; l < 9; l++ )
1079 {
1080 pull[l] = ( wTrackOrigin( n ).w()[l - 5] - wTrackInfit( n ).w()[l - 5] ) /
1081 sqrt( abs( wTrackOrigin( n ).Ew()[l - 5][l - 5] -
1082 wTrackInfit( n ).Ew()[l - 5][l - 5] ) );
1083 }
1084 return pull;
1085 break;
1086 }
1087 case 1: {
1088 HepVector a0( 4, 0 );
1089 HepVector a1( 4, 0 );
1090 a0 = m_p0.sub( pa, pa + 3 );
1091 a1 = m_p.sub( pa, pa + 3 );
1092 HepSymMatrix v1 = getCInfit( n );
1093 HepSymMatrix v0 = getCOrigin( n );
1094 HepVector pull( 3, 0 );
1095 for ( int k = 0; k < 2; k++ )
1096 { pull[k] = ( a0[k] - a1[k] ) / sqrt( v0[k][k] - v1[k][k] ); }
1097 pull[2] = ( a0[3] - a1[3] ) / sqrt( v0[3][3] - v1[3][3] );
1098 return pull;
1099 break;
1100 }
1101 // case 2 : {
1102 // return pull;
1103 // break;
1104 // }
1105 // case 3 : {
1106 // return pull;
1107 // break;
1108 // }
1109 // case 4 : {
1110 // return pull;
1111 // break;
1112 // }
1113 case 5: {
1114 HepLorentzVector p0 = p4Origin( n );
1115 HepLorentzVector p1 = p4Infit( n );
1116 HepVector a0( 2, 0 );
1117 HepVector a1( 2, 0 );
1118 a0[0] = p4Origin( n ).phi();
1119 a1[0] = p4Infit( n ).phi();
1120 a0[1] = p4Origin( n ).pz() / p4Origin( n ).perp();
1121 a1[1] = p4Infit( n ).pz() / p4Infit( n ).perp();
1122 HepMatrix Jacobi( 2, 4, 0 );
1123 Jacobi[0][0] = -p4Infit( n ).py() / p4Infit( n ).perp2();
1124 Jacobi[0][1] = p4Infit( n ).px() / p4Infit( n ).perp2();
1125 Jacobi[1][0] = -( p4Infit( n ).px() / p4Infit( n ).perp() ) *
1126 ( p4Infit( n ).pz() / p4Infit( n ).perp2() );
1127 Jacobi[1][1] = -( p4Infit( n ).py() / p4Infit( n ).perp() ) *
1128 ( p4Infit( n ).pz() / p4Infit( n ).perp2() );
1129 Jacobi[1][2] = 1 / p4Infit( n ).perp();
1130 HepSymMatrix v1 = getCInfit( n ).similarity( Jacobi );
1131 HepSymMatrix v0 = wTrackOrigin( n ).Vplm().sub( 1, 2 );
1132 HepVector pull( 2, 0 );
1133 for ( int k = 0; k < 2; k++ )
1134 { pull[k] = ( a0[k] - a1[k] ) / sqrt( v0[k][k] - v1[k][k] ); }
1135 return pull;
1136 break;
1137 }
1138 }
1139
1140 std::cerr << __FILE__ << ":" << __LINE__ << " Should not reach here!" << std::endl;
1141 exit( 1 );
1142}
1143
1144HepVector KalmanKinematicFit::pInfit( int n ) const {
1145 int pa = mappositionA()[n] + 1;
1146 int pb = mappositionB()[n] + 1;
1147 int ptype = mapkinematic()[n];
1148 switch ( ptype )
1149 {
1150 case 0: {
1151 double mass = m_p.sub( pa + 3, pa + 3 )[0];
1152 HepVector p4( 4, 0 );
1153 p4[0] = m_p.sub( pa, pa )[0];
1154 p4[1] = m_p.sub( pa + 1, pa + 1 )[0];
1155 p4[2] = m_p.sub( pa + 2, pa + 2 )[0];
1156 p4[3] = sqrt( p4[0] * p4[0] + p4[1] * p4[1] + p4[2] * p4[2] + mass * mass );
1157 return p4;
1158 break;
1159 }
1160 case 1: {
1161 double phi = m_p.sub( pa, pa )[0];
1162 double lambda = m_p.sub( pa + 1, pa + 1 )[0];
1163 double mass = m_p.sub( pa + 2, pa + 2 )[0];
1164 double E = m_p.sub( pa + 3, pa + 3 )[0];
1165 double p0 = sqrt( E * E - mass * mass );
1166 HepVector p4( 4, 0 );
1167 p4[0] = p0 * cos( phi ) / sqrt( 1 + lambda * lambda );
1168 p4[1] = p0 * sin( phi ) / sqrt( 1 + lambda * lambda );
1169 p4[2] = p0 * lambda / sqrt( 1 + lambda * lambda );
1170 p4[3] = sqrt( p0 * p0 + mass * mass );
1171 return p4;
1172 break;
1173 }
1174 case 2: {
1175 return m_q.sub( pb, pb + 3 );
1176 break;
1177 }
1178 case 3: {
1179 double mass = ( m_p.sub( pa, pa ) )[0];
1180 double phi = m_q.sub( pb, pb )[0];
1181 double lambda = m_q.sub( pb + 1, pb + 1 )[0];
1182 double E = m_q.sub( pb + 2, pb + 2 )[0];
1183 double p0 = sqrt( E * E - mass * mass );
1184 HepVector p4( 4, 0 );
1185 p4[0] = p0 * cos( phi ) / sqrt( 1 + lambda * lambda );
1186 p4[1] = p0 * sin( phi ) / sqrt( 1 + lambda * lambda );
1187 p4[2] = p0 * lambda / sqrt( 1 + lambda * lambda );
1188 p4[3] = sqrt( p0 * p0 + mass * mass );
1189 return p4;
1190 break;
1191 }
1192 case 4: {
1193 double phi = m_p.sub( pa, pa )[0];
1194 double lambda = m_p.sub( pa + 1, pa + 1 )[0];
1195 double mass = m_q.sub( pb, pb )[0];
1196 double E = m_q.sub( pb + 1, pb + 1 )[0];
1197 double p0 = sqrt( E * E - mass * mass );
1198 HepVector p4( 4, 0 );
1199 p4[0] = p0 * cos( phi ) / sqrt( 1 + lambda * lambda );
1200 p4[1] = p0 * sin( phi ) / sqrt( 1 + lambda * lambda );
1201 p4[2] = p0 * lambda / sqrt( 1 + lambda * lambda );
1202 p4[3] = sqrt( p0 * p0 + mass * mass );
1203 return p4;
1204 break;
1205 }
1206 case 5: {
1207 double phi = m_p.sub( pa, pa )[0];
1208 double lambda = m_p.sub( pa + 1, pa + 1 )[0];
1209 double mass = m_p.sub( pa + 2, pa + 2 )[0];
1210 double p0 = m_q.sub( pb, pb )[0];
1211 HepVector p4( 4, 0 );
1212 p4[0] = p0 * cos( phi ) / sqrt( 1 + lambda * lambda );
1213 p4[1] = p0 * sin( phi ) / sqrt( 1 + lambda * lambda );
1214 p4[2] = p0 * lambda / sqrt( 1 + lambda * lambda );
1215 p4[3] = sqrt( p0 * p0 + mass * mass );
1216 return p4;
1217 break;
1218 }
1219 case 6: {
1220 double x = m_p.sub( pa, pa )[0] - m_q.sub( pb, pb )[0];
1221 double y = m_p.sub( pa + 1, pa + 1 )[0] - m_q.sub( pb + 1, pb + 1 )[0];
1222 double z = m_p.sub( pa + 2, pa + 2 )[0] - m_q.sub( pb + 2, pb + 2 )[0];
1223 double p0 = m_p.sub( pa + 3, pa + 3 )[0];
1224 double R = sqrt( x * x + y * y + z * z );
1225 HepVector p4( 4, 0 );
1226 p4[0] = p0 * x / R;
1227 p4[1] = p0 * y / R;
1228 p4[2] = p0 * z / R;
1229 p4[3] = p0;
1230 return p4;
1231 break;
1232 }
1233 case 7: {
1234 double mass = m_p.sub( pa + 3, pa + 3 )[0];
1235 HepVector p4( 4, 0 );
1236 p4[0] = m_p.sub( pa, pa )[0];
1237 p4[1] = m_p.sub( pa + 1, pa + 1 )[0];
1238 p4[2] = m_p.sub( pa + 2, pa + 2 )[0];
1239 p4[3] = sqrt( p4[0] * p4[0] + p4[1] * p4[1] + p4[2] * p4[2] + mass * mass );
1240 return p4;
1241 break;
1242 }
1243 }
1244
1245 std::cerr << __FILE__ << ":" << __LINE__ << " Should not reach here!" << std::endl;
1246 exit( 1 );
1247}
1248
1249HepVector KalmanKinematicFit::pOrigin( int n ) const {
1250 int pa = mappositionA()[n] + 1;
1251 int pb = mappositionB()[n] + 1;
1252 int ptype = mapkinematic()[n];
1253 switch ( ptype )
1254 {
1255 case 0: {
1256 double mass = m_p0.sub( pa + 3, pa + 3 )[0];
1257 HepVector p4( 4, 0 );
1258 p4[0] = m_p0.sub( pa, pa )[0];
1259 p4[1] = m_p0.sub( pa + 1, pa + 1 )[0];
1260 p4[2] = m_p0.sub( pa + 2, pa + 2 )[0];
1261 p4[3] = sqrt( p4[0] * p4[0] + p4[1] * p4[1] + p4[2] * p4[2] + mass * mass );
1262 return p4;
1263 break;
1264 }
1265 case 1: {
1266 double phi = m_p0.sub( pa, pa )[0];
1267 double lambda = m_p0.sub( pa + 1, pa + 1 )[0];
1268 double mass = m_p0.sub( pa + 2, pa + 2 )[0];
1269 double E = m_p0.sub( pa + 3, pa + 3 )[0];
1270 double p0 = sqrt( E * E - mass * mass );
1271 HepVector p4( 4, 0 );
1272 p4[0] = p0 * cos( phi ) / sqrt( 1 + lambda * lambda );
1273 p4[1] = p0 * sin( phi ) / sqrt( 1 + lambda * lambda );
1274 p4[2] = p0 * lambda / sqrt( 1 + lambda * lambda );
1275 p4[3] = sqrt( p0 * p0 + mass * mass );
1276 return p4;
1277 break;
1278 }
1279 case 2: {
1280 return m_q0.sub( pb, pb + 3 );
1281 break;
1282 }
1283 case 3: {
1284 double mass = ( m_p0.sub( pa, pa ) )[0];
1285 double phi = m_q0.sub( pb, pb )[0];
1286 double lambda = m_q0.sub( pb + 1, pb + 1 )[0];
1287 double E = m_q0.sub( pb + 2, pb + 2 )[0];
1288 double p0 = sqrt( E * E - mass * mass );
1289 HepVector p4( 4, 0 );
1290 p4[0] = p0 * cos( phi ) / sqrt( 1 + lambda * lambda );
1291 p4[1] = p0 * sin( phi ) / sqrt( 1 + lambda * lambda );
1292 p4[2] = p0 * lambda / sqrt( 1 + lambda * lambda );
1293 p4[3] = sqrt( p0 * p0 + mass * mass );
1294 return p4;
1295 break;
1296 }
1297 case 4: {
1298 double phi = m_p0.sub( pa, pa )[0];
1299 double lambda = m_p0.sub( pa + 1, pa + 1 )[0];
1300 double mass = m_q0.sub( pb, pb )[0];
1301 double E = m_q0.sub( pb + 1, pb + 1 )[0];
1302 double p0 = sqrt( E * E - mass * mass );
1303 HepVector p4( 4, 0 );
1304 p4[0] = p0 * cos( phi ) / sqrt( 1 + lambda * lambda );
1305 p4[1] = p0 * sin( phi ) / sqrt( 1 + lambda * lambda );
1306 p4[2] = p0 * lambda / sqrt( 1 + lambda * lambda );
1307 p4[3] = sqrt( p0 * p0 + mass * mass );
1308 return p4;
1309 break;
1310 }
1311 case 5: {
1312 double phi = m_p0.sub( pa, pa )[0];
1313 double lambda = m_p0.sub( pa + 1, pa + 1 )[0];
1314 double mass = m_p0.sub( pa + 2, pa + 2 )[0];
1315 double p0 = m_q0.sub( pb, pb )[0];
1316 HepVector p4( 4, 0 );
1317 p4[0] = p0 * cos( phi ) / sqrt( 1 + lambda * lambda );
1318 p4[1] = p0 * sin( phi ) / sqrt( 1 + lambda * lambda );
1319 p4[2] = p0 * lambda / sqrt( 1 + lambda * lambda );
1320 p4[3] = sqrt( p0 * p0 + mass * mass );
1321 return p4;
1322 break;
1323 }
1324 case 6: {
1325 double x = m_p0.sub( pa, pa )[0] - m_q0.sub( pb, pb )[0];
1326 double y = m_p0.sub( pa + 1, pa + 1 )[0] - m_q0.sub( pb + 1, pb + 1 )[0];
1327 double z = m_p0.sub( pa + 2, pa + 2 )[0] - m_q0.sub( pb + 2, pb + 2 )[0];
1328 double p0 = m_p0.sub( pa + 3, pa + 3 )[0];
1329 double R = sqrt( x * x + y * y + z * z );
1330 HepVector p4( 4, 0 );
1331 p4[0] = p0 * x / R;
1332 p4[1] = p0 * y / R;
1333 p4[2] = p0 * z / R;
1334 p4[3] = p0;
1335 return p4;
1336 break;
1337 }
1338 case 7: {
1339 double mass = m_p0.sub( pa + 3, pa + 3 )[0];
1340 HepVector p4( 4, 0 );
1341 p4[0] = m_p0.sub( pa, pa )[0];
1342 p4[1] = m_p0.sub( pa + 1, pa + 1 )[0];
1343 p4[2] = m_p0.sub( pa + 2, pa + 2 )[0];
1344 p4[3] = sqrt( p4[0] * p4[0] + p4[1] * p4[1] + p4[2] * p4[2] + mass * mass );
1345 return p4;
1346 break;
1347 }
1348 }
1349
1350 std::cerr << __FILE__ << ":" << __LINE__ << " Should not reach here!" << std::endl;
1351 exit( 1 );
1352}
1353
1354HepSymMatrix KalmanKinematicFit::getCOrigin( int n ) const {
1355 int pa = mappositionA()[n] + 1;
1356 int pb = mappositionB()[n] + 1;
1357 int ptype = mapkinematic()[n];
1358 switch ( ptype )
1359 {
1360 case 0: {
1361 return m_C0.sub( pa, pa + NTRKPAR - 1 );
1362 break;
1363 }
1364 case 1: {
1365 return m_C0.sub( pa, pa + NTRKPAR - 1 );
1366 break;
1367 }
1368 case 3: {
1369 return m_C0.sub( pa, pa );
1370 break;
1371 }
1372 case 4: {
1373 return m_C0.sub( pa, pa + 1 );
1374 break;
1375 }
1376 case 5: {
1377 return m_C0.sub( pa, pa + 2 );
1378 break;
1379 }
1380 case 7: {
1381 return m_C0.sub( pa, pa + NTRKPAR - 1 );
1382 break;
1383 }
1384 }
1385
1386 std::cerr << __FILE__ << ":" << __LINE__ << " Should not reach here!" << std::endl;
1387 exit( 1 );
1388}
1389
1390HepSymMatrix KalmanKinematicFit::getCInfit( int n ) const {
1391 int pa = mappositionA()[n] + 1;
1392 int pb = mappositionB()[n] + 1;
1393 int ptype = mapkinematic()[n];
1394 switch ( ptype )
1395 {
1396 case 0: {
1397 return m_C.sub( pa, pa + NTRKPAR - 1 );
1398 break;
1399 }
1400 case 1: {
1401 return m_C.sub( pa, pa + NTRKPAR - 1 );
1402 break;
1403 }
1404 case 3: {
1405 return m_C.sub( pa, pa );
1406 break;
1407 }
1408 case 4: {
1409 return m_C.sub( pa, pa + 1 );
1410 break;
1411 }
1412 case 5: {
1413 return m_C.sub( pa, pa + 2 );
1414 break;
1415 }
1416 case 7: {
1417 return m_C.sub( pa, pa + NTRKPAR - 1 );
1418 break;
1419 }
1420 }
1421
1422 std::cerr << __FILE__ << ":" << __LINE__ << " Should not reach here!" << std::endl;
1423 exit( 1 );
1424}
1425
1426void KalmanKinematicFit::updateConstraints( KinematicConstraints k ) {
1427 KinematicConstraints kc = k;
1428
1429 int type = kc.Type();
1430 switch ( type )
1431 {
1432 case Resonance: {
1433 //
1434 // E^2 - px^2 - py^2 - pz^2 = mres^2
1435 //
1436 double mres = kc.mres();
1437 HepLorentzVector pmis;
1438 for ( unsigned int j = 0; j < ( kc.Ltrk() ).size(); j++ )
1439 {
1440 int n = ( kc.Ltrk() )[j];
1441 pmis = pmis + p4Infit( n );
1442 }
1443 for ( unsigned int j = 0; j < ( kc.Ltrk() ).size(); j++ )
1444 {
1445 int n = ( kc.Ltrk() )[j];
1446 double lambda = p4Infit( n ).pz() / p4Infit( n ).perp();
1447 double a1 = 1 + lambda * lambda;
1448 int pa = mappositionA()[n] + 1;
1449 int pb = mappositionB()[n] + 1;
1450 int ptype = mapkinematic()[n];
1451 switch ( ptype )
1452 {
1453 case 0: {
1454 HepMatrix ta( 1, NTRKPAR, 0 );
1455 ta[0][0] = -2 * pmis.px() + 2 * pmis.e() * p4Infit( n ).px() / p4Infit( n ).e();
1456 ta[0][1] = -2 * pmis.py() + 2 * pmis.e() * p4Infit( n ).py() / p4Infit( n ).e();
1457 ta[0][2] = -2 * pmis.pz() + 2 * pmis.e() * p4Infit( n ).pz() / p4Infit( n ).e();
1458 ta[0][3] = 2 * pmis.e() * p4Infit( n ).m() / p4Infit( n ).e();
1459 setA( m_nc, pa, ta );
1460 setAT( pa, m_nc, ta.T() );
1461 break;
1462 }
1463 case 1: {
1464 HepMatrix ta( 1, NTRKPAR, 0 );
1465 double a1 = lambda * lambda + 1;
1466 ta[0][0] = 2 * pmis.px() * p4Infit( n ).py() - 2 * pmis.py() * p4Infit( n ).px();
1467 ta[0][1] = 2 * pmis.px() * p4Infit( n ).px() * lambda / sqrt( a1 ) +
1468 2 * pmis.py() * ( p4Infit( n ) ).py() * lambda / sqrt( a1 ) -
1469 2 * pmis.pz() * ( p4Infit( n ) ).pz() / ( sqrt( a1 ) * lambda );
1470 ta[0][2] = 2 * pmis.px() * ( p4Infit( n ) ).px() * p4Infit( n ).m() /
1471 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() ) +
1472 2 * pmis.py() * ( p4Infit( n ) ).py() * p4Infit( n ).m() /
1473 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() ) +
1474 2 * pmis.pz() * ( p4Infit( n ) ).pz() * p4Infit( n ).m() /
1475 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() );
1476 ta[0][3] = 2 * pmis.e() -
1477 2 * pmis.px() * ( p4Infit( n ) ).px() * p4Infit( n ).e() /
1478 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() ) -
1479 2 * pmis.py() * ( p4Infit( n ) ).py() * p4Infit( n ).e() /
1480 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() ) -
1481 2 * pmis.pz() * ( p4Infit( n ) ).pz() * p4Infit( n ).e() /
1482 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() );
1483 setA( m_nc, pa, ta );
1484 setAT( pa, m_nc, ta.T() );
1485 break;
1486 }
1487 case 2: {
1488 HepMatrix tb( 1, 4, 0 );
1489 tb[0][0] = -2 * pmis.px();
1490 tb[0][1] = -2 * pmis.py();
1491 tb[0][2] = -2 * pmis.pz();
1492 tb[0][3] = 2 * pmis.e();
1493 setB( m_nc, pb, tb );
1494 setBT( pb, m_nc, tb.T() );
1495 break;
1496 }
1497 case 3: {
1498 HepMatrix ta( 1, 1, 0 );
1499 double a1 = lambda * lambda + 1;
1500 ta[0][0] = 2 * pmis.px() * ( p4Infit( n ) ).px() * p4Infit( n ).m() /
1501 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() ) +
1502 2 * pmis.py() * ( p4Infit( n ) ).py() * p4Infit( n ).m() /
1503 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() ) +
1504 2 * pmis.pz() * ( p4Infit( n ) ).pz() * p4Infit( n ).m() /
1505 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() );
1506 setA( m_nc, pa, ta );
1507 setAT( pa, m_nc, ta.T() );
1508 HepMatrix tb( 1, 3, 0 );
1509 tb[0][0] = 2 * pmis.px() * p4Infit( n ).py() - 2 * pmis.py() * p4Infit( n ).px();
1510 tb[0][1] = 2 * pmis.px() * p4Infit( n ).px() * lambda / sqrt( a1 ) +
1511 2 * pmis.py() * ( p4Infit( n ) ).py() * lambda / sqrt( a1 ) -
1512 2 * pmis.pz() * ( p4Infit( n ) ).pz() / ( sqrt( a1 ) * lambda );
1513 tb[0][2] = 2 * pmis.e() -
1514 2 * pmis.px() * ( p4Infit( n ) ).px() * p4Infit( n ).e() /
1515 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() ) -
1516 2 * pmis.py() * ( p4Infit( n ) ).py() * p4Infit( n ).e() /
1517 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() ) -
1518 2 * pmis.pz() * ( p4Infit( n ) ).pz() * p4Infit( n ).e() /
1519 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() );
1520 setB( m_nc, pb, tb );
1521 setBT( pb, m_nc, tb.T() );
1522 break;
1523 }
1524 case 4: {
1525 HepMatrix ta( 1, 2, 0 );
1526 double a1 = lambda * lambda + 1;
1527 ta[0][0] = 2 * pmis.px() * p4Infit( n ).py() - 2 * pmis.py() * p4Infit( n ).px();
1528 ta[0][1] = 2 * pmis.px() * p4Infit( n ).px() * lambda / sqrt( a1 ) +
1529 2 * pmis.py() * ( p4Infit( n ) ).py() * lambda / sqrt( a1 ) -
1530 2 * pmis.pz() * ( p4Infit( n ) ).pz() / ( sqrt( a1 ) * lambda );
1531 setA( m_nc, pa, ta );
1532 setAT( pa, m_nc, ta.T() );
1533
1534 HepMatrix tb( 1, 2, 0 );
1535 tb[0][0] = 2 * pmis.px() * ( p4Infit( n ) ).px() * p4Infit( n ).m() /
1536 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() ) +
1537 2 * pmis.py() * ( p4Infit( n ) ).py() * p4Infit( n ).m() /
1538 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() ) +
1539 2 * pmis.pz() * ( p4Infit( n ) ).pz() * p4Infit( n ).m() /
1540 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() );
1541 tb[0][1] = 2 * pmis.e() -
1542 2 * pmis.px() * ( p4Infit( n ) ).px() * p4Infit( n ).e() /
1543 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() ) -
1544 2 * pmis.py() * ( p4Infit( n ) ).py() * p4Infit( n ).e() /
1545 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() ) -
1546 2 * pmis.pz() * ( p4Infit( n ) ).pz() * p4Infit( n ).e() /
1547 ( ( p4Infit( n ) ).rho() * p4Infit( n ).rho() );
1548 setB( m_nc, pb, tb );
1549 setBT( pb, m_nc, tb.T() );
1550 break;
1551 }
1552 case 5: {
1553 HepMatrix ta( 1, 3, 0 );
1554 ta[0][0] = 2 * pmis.px() * p4Infit( n ).py() - 2 * pmis.py() * p4Infit( n ).px();
1555 ta[0][1] = 2 * pmis.px() * p4Infit( n ).px() * lambda / sqrt( a1 ) +
1556 2 * pmis.py() * ( p4Infit( n ) ).py() * lambda / sqrt( a1 ) -
1557 2 * pmis.pz() * ( p4Infit( n ) ).pz() / ( sqrt( a1 ) * lambda );
1558 ta[0][2] = 2 * pmis.e() * ( p4Infit( n ) ).m() / ( p4Infit( n ) ).e();
1559 setA( m_nc, pa, ta );
1560 setAT( pa, m_nc, ta.T() );
1561 HepMatrix tb( 1, 1, 0 );
1562 tb[0][0] = 2 * pmis.e() * ( p4Infit( n ) ).rho() / ( p4Infit( n ) ).e() -
1563 2 * pmis.px() * ( p4Infit( n ) ).px() / ( p4Infit( n ) ).rho() -
1564 2 * pmis.py() * ( p4Infit( n ) ).py() / ( p4Infit( n ) ).rho() -
1565 2 * pmis.pz() * ( p4Infit( n ) ).pz() / ( p4Infit( n ) ).rho();
1566 setB( m_nc, pb, tb );
1567 setBT( pb, m_nc, tb.T() );
1568 break;
1569 }
1570 case 7: {
1571 HepMatrix ta( 1, NTRKPAR, 0 );
1572 ta[0][0] = -2 * pmis.px() + 2 * pmis.e() * p4Infit( n ).px() / p4Infit( n ).e();
1573 ta[0][1] = -2 * pmis.py() + 2 * pmis.e() * p4Infit( n ).py() / p4Infit( n ).e();
1574 ta[0][2] = -2 * pmis.pz() + 2 * pmis.e() * p4Infit( n ).pz() / p4Infit( n ).e();
1575 ta[0][3] = 2 * pmis.e() * p4Infit( n ).m() / p4Infit( n ).e();
1576 setA( m_nc, pa, ta );
1577 setAT( pa, m_nc, ta.T() );
1578 break;
1579 }
1580 }
1581 }
1582
1583 HepVector dc( 1, 0 );
1584 dc[0] = pmis.m2() - mres * mres;
1585 m_G[m_nc] = dc[0];
1586 m_nc += 1;
1587
1588 break;
1589 }
1590 case TotalEnergy: {
1591 //
1592 // E - Etot = 0
1593 //
1594 double etot = kc.etot();
1595 HepLorentzVector pmis;
1596 for ( unsigned int j = 0; j < ( kc.Ltrk() ).size(); j++ )
1597 {
1598 int n = ( kc.Ltrk() )[j];
1599 pmis = pmis + p4Infit( n );
1600 }
1601
1602 for ( unsigned int j = 0; j < ( kc.Ltrk() ).size(); j++ )
1603 {
1604 int n = ( kc.Ltrk() )[j];
1605 int pa = mappositionA()[n] + 1;
1606 int pb = mappositionB()[n] + 1;
1607 int ptype = mapkinematic()[n];
1608 switch ( ptype )
1609 {
1610 case 0: {
1611 HepMatrix ta( 1, NTRKPAR, 0 );
1612 ta[0][0] = p4Infit( n ).px() / p4Infit( n ).e();
1613 ta[0][1] = p4Infit( n ).py() / p4Infit( n ).e();
1614 ta[0][2] = p4Infit( n ).pz() / p4Infit( n ).e();
1615 ta[0][3] = p4Infit( n ).m() / p4Infit( n ).e();
1616 setA( m_nc, pa, ta );
1617 setAT( pa, m_nc, ta.T() );
1618 break;
1619 }
1620 case 1: {
1621 HepMatrix ta( 1, NTRKPAR, 0 );
1622 ta[0][3] = 1.0;
1623 setA( m_nc, pa, ta );
1624 setAT( pa, m_nc, ta.T() );
1625 break;
1626 }
1627 case 2: {
1628 HepMatrix tb( 1, 4, 0 );
1629 tb[0][3] = 1.0;
1630 setA( m_nc, pb, tb );
1631 setAT( pb, m_nc, tb.T() );
1632 break;
1633 }
1634 case 3: {
1635 HepMatrix ta( 1, 1, 0 );
1636 ta[0][0] = p4Infit( n ).m() / p4Infit( n ).e();
1637 setA( m_nc, pa, ta );
1638 setAT( pa, m_nc, ta.T() );
1639
1640 HepMatrix tb( 1, 3, 0 );
1641 setB( m_nc, pb, tb );
1642 setBT( pb, m_nc, tb.T() );
1643 break;
1644 }
1645 case 4: {
1646 HepMatrix ta( 1, 2, 0 );
1647 setA( m_nc, pa, ta );
1648 setAT( pa, m_nc, ta.T() );
1649
1650 HepMatrix tb( 1, 2, 0 );
1651 tb[0][0] = 0.0;
1652 tb[0][1] = 1.0;
1653 // tb[0][0] = p4Infit(n).m()/p4Infit(n).e();
1654 // tb[0][1] = p4Infit(n).rho()/p4Infit(n).e();
1655 setB( m_nc, pb, tb );
1656 setBT( pb, m_nc, tb.T() );
1657 break;
1658 }
1659 case 5: {
1660 HepMatrix ta( 1, 3, 0 );
1661 ta[0][2] = p4Infit( n ).m() / p4Infit( n ).e();
1662 setA( m_nc, pa, ta );
1663 setAT( pa, m_nc, ta.T() );
1664
1665 HepMatrix tb( 1, 1, 0 );
1666 tb[0][0] = p4Infit( n ).rho() / p4Infit( n ).e();
1667 setB( m_nc, pb, tb );
1668 setBT( pb, m_nc, tb.T() );
1669 break;
1670 }
1671 case 7: {
1672 HepMatrix ta( 1, NTRKPAR, 0 );
1673 ta[0][0] = p4Infit( n ).px() / p4Infit( n ).e();
1674 ta[0][1] = p4Infit( n ).py() / p4Infit( n ).e();
1675 ta[0][2] = p4Infit( n ).pz() / p4Infit( n ).e();
1676 ta[0][3] = p4Infit( n ).m() / p4Infit( n ).e();
1677 setA( m_nc, pa, ta );
1678 setAT( pa, m_nc, ta.T() );
1679 break;
1680 }
1681 }
1682 }
1683
1684 HepVector dc( 1, 0 );
1685 dc[0] = pmis.e() - etot;
1686 m_G[m_nc] = dc[0];
1687 m_nc += 1;
1688 break;
1689 }
1690 case TotalMomentum: {
1691 //
1692 // sqrt(px^2+py^2+pz^2) - ptot = 0
1693 //
1694 double ptot = kc.ptot();
1695 HepLorentzVector pmis;
1696 for ( unsigned int j = 0; j < ( kc.Ltrk() ).size(); j++ )
1697 {
1698 int n = ( kc.Ltrk() )[j];
1699 pmis = pmis + p4Infit( n );
1700 }
1701
1702 for ( unsigned int j = 0; j < ( kc.Ltrk() ).size(); j++ )
1703 {
1704 int n = ( kc.Ltrk() )[j];
1705 int pa = mappositionA()[n] + 1;
1706 int pb = mappositionB()[n] + 1;
1707 int ptype = mapkinematic()[n];
1708 double lambda = p4Infit( n ).pz() / p4Infit( n ).perp();
1709 switch ( ptype )
1710 {
1711 case 0: {
1712 HepMatrix ta( 1, NTRKPAR, 0 );
1713 ta[0][0] = pmis.px() / pmis.rho();
1714 ta[0][1] = pmis.py() / pmis.rho();
1715 ta[0][2] = pmis.pz() / pmis.rho();
1716 setA( m_nc, pa, ta );
1717 setAT( pa, m_nc, ta.T() );
1718 break;
1719 }
1720 case 1: {
1721 HepMatrix ta( 1, NTRKPAR, 0 );
1722 ta[0][0] = -( pmis.px() / pmis.rho() ) * p4Infit( n ).py() +
1723 ( pmis.px() / pmis.rho() ) * p4Infit( n ).px();
1724 ta[0][1] = -( pmis.px() / pmis.rho() ) * p4Infit( n ).px() *
1725 ( lambda / ( 1 + lambda * lambda ) ) -
1726 ( pmis.py() / pmis.rho() ) * p4Infit( n ).py() *
1727 ( lambda / ( 1 + lambda * lambda ) ) +
1728 ( pmis.pz() / pmis.rho() ) * p4Infit( n ).pz() *
1729 ( 1 / ( lambda * ( 1 + lambda * lambda ) ) );
1730 ta[0][2] = -( ( pmis.px() / pmis.rho() ) * p4Infit( n ).m() /
1731 ( p4Infit( n ).rho() * p4Infit( n ).rho() ) ) *
1732 ( p4Infit( n ).px() + p4Infit( n ).py() + p4Infit( n ).pz() );
1733 ta[0][3] = ( ( pmis.px() / pmis.rho() ) * p4Infit( n ).e() /
1734 ( p4Infit( n ).rho() * p4Infit( n ).rho() ) ) *
1735 ( p4Infit( n ).px() + p4Infit( n ).py() + p4Infit( n ).pz() );
1736
1737 setA( m_nc, pa, ta );
1738 setAT( pa, m_nc, ta.T() );
1739 break;
1740 }
1741 case 2: {
1742 HepMatrix tb( 1, 4, 0 );
1743 tb[0][0] = pmis.px() / pmis.rho();
1744 tb[0][1] = pmis.py() / pmis.rho();
1745 tb[0][2] = pmis.pz() / pmis.rho();
1746 setB( m_nc, pb, tb );
1747 setBT( pb, m_nc, tb.T() );
1748 break;
1749 }
1750 case 3: {
1751 HepMatrix ta( 1, 1, 0 );
1752 setA( m_nc, pa, ta );
1753 setAT( pa, m_nc, ta.T() );
1754 HepMatrix tb( 1, 3, 0 );
1755 tb[0][0] = pmis.px() / pmis.rho();
1756 tb[0][1] = pmis.py() / pmis.rho();
1757 tb[0][2] = pmis.pz() / pmis.rho();
1758 setB( m_nc, pb, tb );
1759 setBT( pb, m_nc, tb.T() );
1760 break;
1761 }
1762 case 4: {
1763 HepMatrix ta( 1, 2, 0 );
1764 ta[0][0] = -( pmis.px() / pmis.rho() ) * p4Infit( n ).py() +
1765 ( pmis.px() / pmis.rho() ) * p4Infit( n ).px();
1766 ta[0][1] = -( pmis.px() / pmis.rho() ) * p4Infit( n ).px() *
1767 ( lambda / ( 1 + lambda * lambda ) ) -
1768 ( pmis.py() / pmis.rho() ) * p4Infit( n ).py() *
1769 ( lambda / ( 1 + lambda * lambda ) ) +
1770 ( pmis.pz() / pmis.rho() ) * p4Infit( n ).pz() *
1771 ( 1 / ( lambda * ( 1 + lambda * lambda ) ) );
1772 setA( m_nc, pa, ta );
1773 setAT( pa, m_nc, ta.T() );
1774
1775 HepMatrix tb( 1, 2, 0 );
1776 tb[0][0] = -( ( pmis.px() / pmis.rho() ) * p4Infit( n ).m() /
1777 ( p4Infit( n ).rho() * p4Infit( n ).rho() ) ) *
1778 ( p4Infit( n ).px() + p4Infit( n ).py() + p4Infit( n ).pz() );
1779 tb[0][1] = ( ( pmis.px() / pmis.rho() ) * p4Infit( n ).e() /
1780 ( p4Infit( n ).rho() * p4Infit( n ).rho() ) ) *
1781 ( p4Infit( n ).px() + p4Infit( n ).py() + p4Infit( n ).pz() );
1782 // tb[0][1] = (pmis.px()/pmis.rho()) * (p4Infit(n).px()/p4Infit(n).rho())
1783 // + (pmis.py()/pmis.rho()) * (p4Infit(n).py()/p4Infit(n).rho()) +
1784 // (pmis.pz()/pmis.rho()) * (p4Infit(n).pz()/p4Infit(n).rho());
1785 setB( m_nc, pb, tb );
1786 setBT( pb, m_nc, tb.T() );
1787 break;
1788 }
1789 case 5: {
1790 HepMatrix ta( 1, 3, 0 );
1791 ta[0][0] = -( pmis.px() / pmis.rho() ) * p4Infit( n ).py() +
1792 ( pmis.px() / pmis.rho() ) * p4Infit( n ).px();
1793 ta[0][1] = -( pmis.px() / pmis.rho() ) * p4Infit( n ).px() *
1794 ( lambda / ( 1 + lambda * lambda ) ) -
1795 ( pmis.py() / pmis.rho() ) * p4Infit( n ).py() *
1796 ( lambda / ( 1 + lambda * lambda ) ) +
1797 ( pmis.pz() / pmis.rho() ) * p4Infit( n ).pz() *
1798 ( 1 / ( lambda * ( 1 + lambda * lambda ) ) );
1799 setA( m_nc, pa, ta );
1800 setAT( pa, m_nc, ta.T() );
1801
1802 HepMatrix tb( 1, 1, 0 );
1803 tb[0][0] = ( pmis.px() / pmis.rho() ) * ( p4Infit( n ).px() / p4Infit( n ).rho() ) +
1804 ( pmis.py() / pmis.rho() ) * ( p4Infit( n ).py() / p4Infit( n ).rho() ) +
1805 ( pmis.pz() / pmis.rho() ) * ( p4Infit( n ).pz() / p4Infit( n ).rho() );
1806 setB( m_nc, pb, tb );
1807 setBT( pb, m_nc, tb.T() );
1808 break;
1809 }
1810 case 7: {
1811 HepMatrix ta( 1, NTRKPAR, 0 );
1812 ta[0][0] = pmis.px() / pmis.rho();
1813 ta[0][1] = pmis.py() / pmis.rho();
1814 ta[0][2] = pmis.pz() / pmis.rho();
1815 setA( m_nc, pa, ta );
1816 setAT( pa, m_nc, ta.T() );
1817 break;
1818 }
1819 }
1820 }
1821
1822 HepVector dc( 1, 0 );
1823 dc[0] = pmis.rho() - ptot;
1824 m_G[m_nc] = dc[0];
1825 m_nc += 1;
1826 break;
1827 }
1828
1829 case ThreeMomentum: {
1830 //
1831 // px - p3x = 0
1832 // py - p3y = 0
1833 // pz - p3z = 0
1834 //
1835 Hep3Vector p3 = kc.p3();
1836 HepLorentzVector pmis;
1837 for ( unsigned int j = 0; j < ( kc.Ltrk() ).size(); j++ )
1838 {
1839 int n = ( kc.Ltrk() )[j];
1840 pmis = pmis + p4Infit( n );
1841 }
1842 for ( unsigned int j = 0; j < ( kc.Ltrk() ).size(); j++ )
1843 {
1844 int n = ( kc.Ltrk() )[j];
1845 int pa = mappositionA()[n] + 1;
1846 int pb = mappositionB()[n] + 1;
1847 int ptype = mapkinematic()[n];
1848 double lambda = p4Infit( n ).pz() / p4Infit( n ).perp();
1849 switch ( ptype )
1850 {
1851 case 0: {
1852 HepMatrix ta( kc.nc(), NTRKPAR, 0 );
1853 ta[0][0] = 1.0;
1854 ta[1][1] = 1.0;
1855 ta[2][2] = 1.0;
1856 setA( m_nc, pa, ta );
1857 setAT( pa, m_nc, ta.T() );
1858 break;
1859 }
1860 case 1: {
1861 HepMatrix ta( kc.nc(), NTRKPAR, 0 );
1862 ta[0][0] = -p4Infit( n ).py();
1863 ta[0][1] = -p4Infit( n ).px() * ( lambda / sqrt( 1 + lambda * lambda ) );
1864 ta[0][2] = -p4Infit( n ).m() * p4Infit( n ).px() /
1865 ( p4Infit( n ).rho() * p4Infit( n ).rho() );
1866 ta[0][3] =
1867 p4Infit( n ).e() * p4Infit( n ).px() / ( p4Infit( n ).rho() * p4Infit( n ).rho() );
1868 ta[1][0] = p4Infit( n ).px();
1869 ta[1][1] = -p4Infit( n ).py() * ( lambda / sqrt( 1 + lambda * lambda ) );
1870 ta[1][2] = -p4Infit( n ).m() * p4Infit( n ).py() /
1871 ( p4Infit( n ).rho() * p4Infit( n ).rho() );
1872 ta[1][3] =
1873 p4Infit( n ).e() * p4Infit( n ).py() / ( p4Infit( n ).rho() * p4Infit( n ).rho() );
1874 ta[2][0] = 0;
1875 ta[2][1] = p4Infit( n ).pz() * ( 1 / ( lambda * ( 1 + lambda * lambda ) ) );
1876 ta[2][2] = -p4Infit( n ).m() * p4Infit( n ).pz() /
1877 ( p4Infit( n ).rho() * p4Infit( n ).rho() );
1878 ta[2][3] =
1879 p4Infit( n ).e() * p4Infit( n ).pz() / ( p4Infit( n ).rho() * p4Infit( n ).rho() );
1880 // ta[0][0] = 1.0;
1881 // ta[1][1] = 1.0;
1882 // ta[2][2] = 1.0;
1883 setA( m_nc, pa, ta );
1884 setAT( pa, m_nc, ta.T() );
1885 break;
1886 }
1887 case 2: {
1888 HepMatrix tb( kc.nc(), 4, 0 );
1889 tb[0][0] = 1.0;
1890 tb[1][1] = 1.0;
1891 tb[2][2] = 1.0;
1892 setB( m_nc, pb, tb );
1893 setBT( pb, m_nc, tb.T() );
1894 break;
1895 }
1896 case 3: {
1897 HepMatrix ta( kc.nc(), 1, 0 );
1898 setA( m_nc, pa, ta );
1899 setAT( pa, m_nc, ta.T() );
1900
1901 HepMatrix tb( kc.nc(), 3, 0 );
1902 tb[0][0] = 1.0;
1903 tb[1][1] = 1.0;
1904 tb[2][2] = 1.0;
1905 setB( m_nc, pb, tb );
1906 setBT( pb, m_nc, tb.T() );
1907
1908 break;
1909 }
1910 case 4: {
1911 HepMatrix ta( kc.nc(), 2, 0 );
1912 ta[0][0] = -p4Infit( n ).py();
1913 ta[0][1] = -p4Infit( n ).px() * ( lambda / sqrt( 1 + lambda * lambda ) );
1914 ta[1][0] = p4Infit( n ).px();
1915 ta[1][1] = -p4Infit( n ).py() * ( lambda / sqrt( 1 + lambda * lambda ) );
1916 ta[2][0] = 0;
1917 ta[2][1] = p4Infit( n ).pz() * ( 1 / ( lambda * ( 1 + lambda * lambda ) ) );
1918 setA( m_nc, pa, ta );
1919 setAT( pa, m_nc, ta.T() );
1920
1921 HepMatrix tb( kc.nc(), 2, 0 );
1922 tb[0][1] = -p4Infit( n ).m() * p4Infit( n ).px() /
1923 ( p4Infit( n ).rho() * p4Infit( n ).rho() );
1924 tb[1][1] = -p4Infit( n ).m() * p4Infit( n ).py() /
1925 ( p4Infit( n ).rho() * p4Infit( n ).rho() );
1926 tb[2][1] = -p4Infit( n ).m() * p4Infit( n ).pz() /
1927 ( p4Infit( n ).rho() * p4Infit( n ).rho() );
1928 setB( m_nc, pb, tb );
1929 setBT( pb, m_nc, tb.T() );
1930 break;
1931 }
1932 case 5: {
1933 HepMatrix ta( kc.nc(), 3, 0 );
1934 ta[0][0] = -p4Infit( n ).py();
1935 ta[0][1] = -p4Infit( n ).px() * ( lambda / sqrt( 1 + lambda * lambda ) );
1936 ta[1][0] = p4Infit( n ).px();
1937 ta[1][1] = -p4Infit( n ).py() * ( lambda / sqrt( 1 + lambda * lambda ) );
1938 ta[2][0] = 0;
1939 ta[2][1] = p4Infit( n ).pz() * ( 1 / ( lambda * ( 1 + lambda * lambda ) ) );
1940 setA( m_nc, pa, ta );
1941 setAT( pa, m_nc, ta.T() );
1942
1943 HepMatrix tb( kc.nc(), 1, 0 );
1944 tb[0][0] = p4Infit( n ).px() / p4Infit( n ).rho();
1945 tb[1][0] = p4Infit( n ).py() / p4Infit( n ).rho();
1946 tb[2][0] = p4Infit( n ).pz() / p4Infit( n ).rho();
1947 setB( m_nc, pb, tb );
1948 setBT( pb, m_nc, tb.T() );
1949 break;
1950 }
1951 case 7: {
1952 HepMatrix ta( kc.nc(), NTRKPAR, 0 );
1953 ta[0][0] = 1.0;
1954 ta[1][1] = 1.0;
1955 ta[2][2] = 1.0;
1956 setA( m_nc, pa, ta );
1957 setAT( pa, m_nc, ta.T() );
1958 break;
1959 }
1960 }
1961 }
1962 HepVector dc( kc.nc(), 0 );
1963 dc[0] = pmis.px() - p3.x();
1964 dc[1] = pmis.py() - p3.y();
1965 dc[2] = pmis.pz() - p3.z();
1966 for ( int i = 0; i < kc.nc(); i++ ) { m_G[m_nc + i] = dc[i]; }
1967
1968 m_nc += 3;
1969
1970 break;
1971 }
1972 case EqualMass: {
1973 //
1974 // (E_1 ^2 - Px_1 ^2 - Py_1 ^2 - Pz_1 ^2) - (E_2 ^2 - Px_2 ^2 - Py_2 ^2 - Pz_2 ^2) = 0
1975 //
1976
1977 int isiz = ( kc.numEqual() )[0];
1978 HepLorentzVector pmis1, pmis2;
1979 for ( int n = 0; n < isiz; n++ )
1980 {
1981 int n1 = ( kc.Ltrk() )[n];
1982 pmis1 = pmis1 + p4Infit( n1 );
1983 }
1984
1985 int jsiz = ( kc.numEqual() )[1];
1986 for ( int n = 0; n < jsiz; n++ )
1987 {
1988 int n2 = ( kc.Ltrk() )[n + isiz];
1989 pmis2 = pmis2 + p4Infit( n2 );
1990 }
1991
1992 for ( int i = 0; i < isiz; i++ )
1993 {
1994 int n1 = ( kc.Ltrk() )[i];
1995 double lambda = p4Infit( n1 ).pz() / p4Infit( n1 ).perp();
1996 int pa = mappositionA()[n1] + 1;
1997 int pb = mappositionB()[n1] + 1;
1998 int ptype = mapkinematic()[n1];
1999 switch ( ptype )
2000 {
2001 case 0: {
2002 HepMatrix ta( 1, NTRKPAR, 0 );
2003 ta[0][0] = -2 * pmis1.px() + 2 * pmis1.e() * p4Infit( n1 ).px() / p4Infit( n1 ).e();
2004 ta[0][1] = -2 * pmis1.py() + 2 * pmis1.e() * p4Infit( n1 ).py() / p4Infit( n1 ).e();
2005 ta[0][2] = -2 * pmis1.pz() + 2 * pmis1.e() * p4Infit( n1 ).pz() / p4Infit( n1 ).e();
2006 ta[0][3] = 2 * pmis1.e() * p4Infit( n1 ).m() / p4Infit( n1 ).e();
2007 setA( m_nc, pa, ta );
2008 setAT( pa, m_nc, ta.T() );
2009 break;
2010 }
2011 case 1: {
2012 HepMatrix ta( 1, NTRKPAR, 0 );
2013 double a1 = lambda * lambda + 1;
2014 ta[0][0] = 2 * pmis1.px() * p4Infit( n1 ).py() - 2 * pmis1.py() * p4Infit( n1 ).px();
2015 ta[0][1] = 2 * pmis1.px() * p4Infit( n1 ).px() * lambda / sqrt( a1 ) +
2016 2 * pmis1.py() * ( p4Infit( n1 ) ).py() * lambda / sqrt( a1 ) -
2017 2 * pmis1.pz() * ( p4Infit( n1 ) ).pz() / ( sqrt( a1 ) * lambda );
2018 ta[0][2] = 2 * pmis1.px() * ( p4Infit( n1 ) ).px() * p4Infit( n1 ).m() /
2019 ( ( p4Infit( n1 ) ).rho() * p4Infit( n1 ).rho() ) +
2020 2 * pmis1.py() * ( p4Infit( n1 ) ).py() * p4Infit( n1 ).m() /
2021 ( ( p4Infit( n1 ) ).rho() * p4Infit( n1 ).rho() ) +
2022 2 * pmis1.pz() * ( p4Infit( n1 ) ).pz() * p4Infit( n1 ).m() /
2023 ( ( p4Infit( n1 ) ).rho() * p4Infit( n1 ).rho() );
2024 ta[0][3] = 2 * pmis1.e() -
2025 2 * pmis1.px() * ( p4Infit( n1 ) ).px() * p4Infit( n1 ).e() /
2026 ( ( p4Infit( n1 ) ).rho() * p4Infit( n1 ).rho() ) -
2027 2 * pmis1.py() * ( p4Infit( n1 ) ).py() * p4Infit( n1 ).e() /
2028 ( ( p4Infit( n1 ) ).rho() * p4Infit( n1 ).rho() ) -
2029 2 * pmis1.pz() * ( p4Infit( n1 ) ).pz() * p4Infit( n1 ).e() /
2030 ( ( p4Infit( n1 ) ).rho() * p4Infit( n1 ).rho() );
2031 setA( m_nc, pa, ta );
2032 setAT( pa, m_nc, ta.T() );
2033 break;
2034 }
2035 case 2: {
2036 HepMatrix tb( 1, 4, 0 );
2037 tb[0][0] = -2 * pmis1.px();
2038 tb[0][1] = -2 * pmis1.py();
2039 tb[0][2] = -2 * pmis1.pz();
2040 tb[0][3] = 2 * pmis1.e();
2041 setB( m_nc, pb, tb );
2042 setBT( pb, m_nc, tb.T() );
2043 break;
2044 }
2045 case 3: {
2046 HepMatrix ta( 1, 1, 0 );
2047 ta[0][0] = 2 * pmis1.e() * ( p4Infit( n1 ).m() / p4Infit( n1 ).e() );
2048 setA( m_nc, pa, ta );
2049 setAT( pa, m_nc, ta.T() );
2050 HepMatrix tb( 1, 3, 0 );
2051 tb[0][0] = -2 * pmis1.px();
2052 tb[0][1] = -2 * pmis1.py();
2053 tb[0][2] = -2 * pmis1.pz();
2054 setB( m_nc, pb, tb );
2055 setBT( pb, m_nc, tb.T() );
2056 break;
2057 }
2058 case 4: {
2059 HepMatrix ta( 1, 2, 0 );
2060 double a1 = lambda * lambda + 1;
2061 ta[0][0] = 2 * pmis1.px() * p4Infit( n1 ).py() - 2 * pmis1.py() * p4Infit( n1 ).px();
2062 ta[0][1] = 2 * pmis1.px() * p4Infit( n1 ).px() * lambda / sqrt( a1 ) +
2063 2 * pmis1.py() * ( p4Infit( n1 ) ).py() * lambda / sqrt( a1 ) -
2064 2 * pmis1.pz() * ( p4Infit( n1 ) ).pz() / ( sqrt( a1 ) * lambda );
2065 setA( m_nc, pa, ta );
2066 setAT( pa, m_nc, ta.T() );
2067
2068 HepMatrix tb( 1, 2, 0 );
2069 tb[0][0] = 2 * pmis1.px() * ( p4Infit( n1 ) ).px() * p4Infit( n1 ).m() /
2070 ( ( p4Infit( n1 ) ).rho() * p4Infit( n1 ).rho() ) +
2071 2 * pmis1.py() * ( p4Infit( n1 ) ).py() * p4Infit( n1 ).m() /
2072 ( ( p4Infit( n1 ) ).rho() * p4Infit( n1 ).rho() ) +
2073 2 * pmis1.pz() * ( p4Infit( n1 ) ).pz() * p4Infit( n1 ).m() /
2074 ( ( p4Infit( n1 ) ).rho() * p4Infit( n1 ).rho() );
2075 tb[0][1] = 2 * pmis1.e() -
2076 2 * pmis1.px() * ( p4Infit( n1 ) ).px() * p4Infit( n1 ).e() /
2077 ( ( p4Infit( n1 ) ).rho() * p4Infit( n1 ).rho() ) -
2078 2 * pmis1.py() * ( p4Infit( n1 ) ).py() * p4Infit( n1 ).e() /
2079 ( ( p4Infit( n1 ) ).rho() * p4Infit( n1 ).rho() ) -
2080 2 * pmis1.pz() * ( p4Infit( n1 ) ).pz() * p4Infit( n1 ).e() /
2081 ( ( p4Infit( n1 ) ).rho() * p4Infit( n1 ).rho() );
2082 setB( m_nc, pb, tb );
2083 setBT( pb, m_nc, tb.T() );
2084 break;
2085 }
2086 case 5: {
2087 HepMatrix ta( 1, 3, 0 );
2088 double a1 = lambda * lambda + 1;
2089 ta[0][0] = 2 * pmis1.px() * p4Infit( n1 ).py() - 2 * pmis1.py() * p4Infit( n1 ).px();
2090 ta[0][1] = 2 * pmis1.px() * p4Infit( n1 ).px() * lambda / sqrt( a1 ) +
2091 2 * pmis1.py() * ( p4Infit( n1 ) ).py() * lambda / sqrt( a1 ) -
2092 2 * pmis1.pz() * ( p4Infit( n1 ) ).pz() / ( sqrt( a1 ) * lambda );
2093 ta[0][2] = 2 * pmis1.e() * ( p4Infit( n1 ) ).m() / ( p4Infit( n1 ) ).e();
2094 setA( m_nc, pa, ta );
2095 setAT( pa, m_nc, ta.T() );
2096 HepMatrix tb( 1, 1, 0 );
2097 tb[0][0] = 2 * pmis1.e() * ( p4Infit( n1 ) ).rho() / ( p4Infit( n1 ) ).e() -
2098 2 * pmis1.px() * ( p4Infit( n1 ) ).px() / ( p4Infit( n1 ) ).rho() -
2099 2 * pmis1.py() * ( p4Infit( n1 ) ).py() / ( p4Infit( n1 ) ).rho() -
2100 2 * pmis1.pz() * ( p4Infit( n1 ) ).pz() / ( p4Infit( n1 ) ).rho();
2101 setB( m_nc, pb, tb );
2102 setBT( pb, m_nc, tb.T() );
2103 break;
2104 }
2105 case 7: {
2106 HepMatrix ta( 1, NTRKPAR, 0 );
2107 ta[0][0] = -2 * pmis1.px() + 2 * pmis1.e() * p4Infit( n1 ).px() / p4Infit( n1 ).e();
2108 ta[0][1] = -2 * pmis1.py() + 2 * pmis1.e() * p4Infit( n1 ).py() / p4Infit( n1 ).e();
2109 ta[0][2] = -2 * pmis1.pz() + 2 * pmis1.e() * p4Infit( n1 ).pz() / p4Infit( n1 ).e();
2110 ta[0][3] = 2 * pmis1.e() * p4Infit( n1 ).m() / p4Infit( n1 ).e();
2111 setA( m_nc, pa, ta );
2112 setAT( pa, m_nc, ta.T() );
2113 break;
2114 }
2115 }
2116 }
2117
2118 for ( int i = isiz; i < isiz + jsiz; i++ )
2119 {
2120 int n2 = ( kc.Ltrk() )[i];
2121 double lambda = p4Infit( n2 ).pz() / p4Infit( n2 ).perp();
2122 int pa = mappositionA()[n2] + 1;
2123 int pb = mappositionB()[n2] + 1;
2124 int ptype = mapkinematic()[n2];
2125 switch ( ptype )
2126 {
2127 case 0: {
2128 HepMatrix ta( 1, NTRKPAR, 0 );
2129 ta[0][0] = -2 * pmis2.px() + 2 * pmis2.e() * p4Infit( n2 ).px() / p4Infit( n2 ).e();
2130 ta[0][1] = -2 * pmis2.py() + 2 * pmis2.e() * p4Infit( n2 ).py() / p4Infit( n2 ).e();
2131 ta[0][2] = -2 * pmis2.pz() + 2 * pmis2.e() * p4Infit( n2 ).pz() / p4Infit( n2 ).e();
2132 ta[0][3] = 2 * pmis2.e() * p4Infit( n2 ).m() / p4Infit( n2 ).e();
2133 setA( m_nc, pa, -ta );
2134 setAT( pa, m_nc, -ta.T() );
2135 break;
2136 }
2137 case 1: {
2138 HepMatrix ta( 1, NTRKPAR, 0 );
2139 double a1 = lambda * lambda + 1;
2140 ta[0][0] = 2 * pmis2.px() * p4Infit( n2 ).py() - 2 * pmis2.py() * p4Infit( n2 ).px();
2141 ta[0][1] = 2 * pmis2.px() * p4Infit( n2 ).px() * lambda / sqrt( a1 ) +
2142 2 * pmis2.py() * ( p4Infit( n2 ) ).py() * lambda / sqrt( a1 ) -
2143 2 * pmis2.pz() * ( p4Infit( n2 ) ).pz() / ( sqrt( a1 ) * lambda );
2144 ta[0][2] = 2 * pmis2.px() * ( p4Infit( n2 ) ).px() * p4Infit( n2 ).m() /
2145 ( ( p4Infit( n2 ) ).rho() * p4Infit( n2 ).rho() ) +
2146 2 * pmis2.py() * ( p4Infit( n2 ) ).py() * p4Infit( n2 ).m() /
2147 ( ( p4Infit( n2 ) ).rho() * p4Infit( n2 ).rho() ) +
2148 2 * pmis2.pz() * ( p4Infit( n2 ) ).pz() * p4Infit( n2 ).m() /
2149 ( ( p4Infit( n2 ) ).rho() * p4Infit( n2 ).rho() );
2150 ta[0][3] = 2 * pmis2.e() -
2151 2 * pmis2.px() * ( p4Infit( n2 ) ).px() * p4Infit( n2 ).e() /
2152 ( ( p4Infit( n2 ) ).rho() * p4Infit( n2 ).rho() ) -
2153 2 * pmis2.py() * ( p4Infit( n2 ) ).py() * p4Infit( n2 ).e() /
2154 ( ( p4Infit( n2 ) ).rho() * p4Infit( n2 ).rho() ) -
2155 2 * pmis2.pz() * ( p4Infit( n2 ) ).pz() * p4Infit( n2 ).e() /
2156 ( ( p4Infit( n2 ) ).rho() * p4Infit( n2 ).rho() );
2157 setA( m_nc, pa, -ta );
2158 setAT( pa, m_nc, -ta.T() );
2159 break;
2160 }
2161 case 2: {
2162 HepMatrix tb( 1, 4, 0 );
2163 tb[0][0] = -2 * pmis2.px();
2164 tb[0][1] = -2 * pmis2.py();
2165 tb[0][2] = -2 * pmis2.pz();
2166 tb[0][3] = 2 * pmis2.e();
2167 setB( m_nc, pb, -tb );
2168 setBT( pb, m_nc, -tb.T() );
2169 break;
2170 }
2171 case 3: {
2172 HepMatrix ta( 1, 1, 0 );
2173 ta[0][0] = 2 * pmis2.e() * ( p4Infit( n2 ).m() / p4Infit( n2 ).e() );
2174 setA( m_nc, pa, -ta );
2175 setAT( pa, m_nc, -ta.T() );
2176 HepMatrix tb( 1, 3, 0 );
2177 tb[0][0] = -2 * pmis2.px();
2178 tb[0][1] = -2 * pmis2.py();
2179 tb[0][2] = -2 * pmis2.pz();
2180 setB( m_nc, pb, -tb );
2181 setBT( pb, m_nc, -tb.T() );
2182 break;
2183 }
2184 case 4: {
2185 HepMatrix ta( 1, 2, 0 );
2186 double a1 = lambda * lambda + 1;
2187 ta[0][0] = 2 * pmis2.px() * p4Infit( n2 ).py() - 2 * pmis2.py() * p4Infit( n2 ).px();
2188 ta[0][1] = 2 * pmis2.px() * p4Infit( n2 ).px() * lambda / sqrt( a1 ) +
2189 2 * pmis2.py() * ( p4Infit( n2 ) ).py() * lambda / sqrt( a1 ) -
2190 2 * pmis2.pz() * ( p4Infit( n2 ) ).pz() / ( sqrt( a1 ) * lambda );
2191 setA( m_nc, pa, -ta );
2192 setAT( pa, m_nc, -ta.T() );
2193
2194 HepMatrix tb( 1, 2, 0 );
2195 tb[0][0] = 2 * pmis2.px() * ( p4Infit( n2 ) ).px() * p4Infit( n2 ).m() /
2196 ( ( p4Infit( n2 ) ).rho() * p4Infit( n2 ).rho() ) +
2197 2 * pmis2.py() * ( p4Infit( n2 ) ).py() * p4Infit( n2 ).m() /
2198 ( ( p4Infit( n2 ) ).rho() * p4Infit( n2 ).rho() ) +
2199 2 * pmis2.pz() * ( p4Infit( n2 ) ).pz() * p4Infit( n2 ).m() /
2200 ( ( p4Infit( n2 ) ).rho() * p4Infit( n2 ).rho() );
2201 tb[0][1] = 2 * pmis2.e() -
2202 2 * pmis2.px() * ( p4Infit( n2 ) ).px() * p4Infit( n2 ).e() /
2203 ( ( p4Infit( n2 ) ).rho() * p4Infit( n2 ).rho() ) -
2204 2 * pmis2.py() * ( p4Infit( n2 ) ).py() * p4Infit( n2 ).e() /
2205 ( ( p4Infit( n2 ) ).rho() * p4Infit( n2 ).rho() ) -
2206 2 * pmis2.pz() * ( p4Infit( n2 ) ).pz() * p4Infit( n2 ).e() /
2207 ( ( p4Infit( n2 ) ).rho() * p4Infit( n2 ).rho() );
2208 setB( m_nc, pb, -tb );
2209 setBT( pb, m_nc, -tb.T() );
2210 break;
2211 }
2212 case 5: {
2213 HepMatrix ta( 1, 3, 0 );
2214 double a1 = lambda * lambda + 1;
2215 ta[0][0] = 2 * pmis2.px() * p4Infit( n2 ).py() - 2 * pmis2.py() * p4Infit( n2 ).px();
2216 ta[0][1] = 2 * pmis2.px() * p4Infit( n2 ).px() * lambda / sqrt( a1 ) +
2217 2 * pmis2.py() * ( p4Infit( n2 ) ).py() * lambda / sqrt( a1 ) -
2218 2 * pmis2.pz() * ( p4Infit( n2 ) ).pz() / ( sqrt( a1 ) * lambda );
2219 ta[0][2] = 2 * pmis2.e() * ( p4Infit( n2 ) ).m() / ( p4Infit( n2 ) ).e();
2220 setA( m_nc, pa, -ta );
2221 setAT( pa, m_nc, -ta.T() );
2222 HepMatrix tb( 1, 1, 0 );
2223 tb[0][0] = 2 * pmis2.e() * ( p4Infit( n2 ) ).rho() / ( p4Infit( n2 ) ).e() -
2224 2 * pmis2.px() * ( p4Infit( n2 ) ).px() / ( p4Infit( n2 ) ).rho() -
2225 2 * pmis2.py() * ( p4Infit( n2 ) ).py() / ( p4Infit( n2 ) ).rho() -
2226 2 * pmis2.pz() * ( p4Infit( n2 ) ).pz() / ( p4Infit( n2 ) ).rho();
2227 setB( m_nc, pb, -tb );
2228 setBT( pb, m_nc, -tb.T() );
2229 break;
2230 }
2231 case 7: {
2232 HepMatrix ta( 1, NTRKPAR, 0 );
2233 ta[0][0] = -2 * pmis2.px() + 2 * pmis2.e() * p4Infit( n2 ).px() / p4Infit( n2 ).e();
2234 ta[0][1] = -2 * pmis2.py() + 2 * pmis2.e() * p4Infit( n2 ).py() / p4Infit( n2 ).e();
2235 ta[0][2] = -2 * pmis2.pz() + 2 * pmis2.e() * p4Infit( n2 ).pz() / p4Infit( n2 ).e();
2236 ta[0][3] = 2 * pmis2.e() * p4Infit( n2 ).m() / p4Infit( n2 ).e();
2237 setA( m_nc, pa, -ta );
2238 setAT( pa, m_nc, -ta.T() );
2239 break;
2240 }
2241 }
2242 }
2243
2244 HepVector dc( 1, 0 );
2245 dc[0] = pmis1.m2() - pmis2.m2();
2246 m_G[m_nc] = dc[0];
2247
2248 m_nc += 1;
2249
2250 break;
2251 }
2252
2253 case FourMomentum:
2254 default: {
2255 //
2256 // px - p4x = 0
2257 // py - p4y = 0
2258 // pz - p4z = 0
2259 // e - p4e = 0
2260 //
2261 HepLorentzVector p4 = kc.p4();
2262 HepLorentzVector pmis;
2263 for ( unsigned int j = 0; j < ( kc.Ltrk() ).size(); j++ )
2264 {
2265 int n = ( kc.Ltrk() )[j];
2266 pmis = pmis + p4Infit( n );
2267 }
2268 for ( unsigned int j = 0; j < ( kc.Ltrk() ).size(); j++ )
2269 {
2270 int n = ( kc.Ltrk() )[j];
2271 double lambda = p4Infit( n ).pz() / p4Infit( n ).perp();
2272 int pa = mappositionA()[n] + 1;
2273 int pb = mappositionB()[n] + 1;
2274 int ptype = mapkinematic()[n];
2275 switch ( ptype )
2276 {
2277 case 0: {
2278 HepMatrix ta( kc.nc(), NTRKPAR, 0 );
2279 ta[0][0] = 1.0;
2280 ta[1][1] = 1.0;
2281 ta[2][2] = 1.0;
2282 ta[3][0] = p4Infit( n ).px() / p4Infit( n ).e();
2283 ta[3][1] = p4Infit( n ).py() / p4Infit( n ).e();
2284 ta[3][2] = p4Infit( n ).pz() / p4Infit( n ).e();
2285 ta[3][3] = p4Infit( n ).m() / p4Infit( n ).e();
2286 setA( m_nc, pa, ta );
2287 setAT( pa, m_nc, ta.T() );
2288 break;
2289 }
2290 case 1: {
2291 HepMatrix ta( kc.nc(), NTRKPAR, 0 );
2292 ta[0][0] = -p4Infit( n ).py();
2293 ta[0][1] = -p4Infit( n ).px() * ( lambda / ( 1 + lambda * lambda ) );
2294 ta[0][2] = -p4Infit( n ).m() * p4Infit( n ).px() /
2295 ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2296 ta[0][3] =
2297 p4Infit( n ).e() * p4Infit( n ).px() / ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2298 ta[1][0] = p4Infit( n ).px();
2299 ta[1][1] = -p4Infit( n ).py() * ( lambda / ( 1 + lambda * lambda ) );
2300 ta[1][2] = -p4Infit( n ).m() * p4Infit( n ).py() /
2301 ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2302 ta[1][3] =
2303 p4Infit( n ).e() * p4Infit( n ).py() / ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2304 ta[2][0] = 0;
2305 ta[2][1] = p4Infit( n ).pz() * ( 1 / ( lambda * ( 1 + lambda * lambda ) ) );
2306 ta[2][2] = -p4Infit( n ).m() * p4Infit( n ).pz() /
2307 ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2308 ta[2][3] =
2309 p4Infit( n ).e() * p4Infit( n ).pz() / ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2310 ta[3][0] = 0;
2311 ta[3][1] = 0;
2312 ta[3][2] = 0;
2313 ta[3][3] = 1.0;
2314 setA( m_nc, pa, ta );
2315 setAT( pa, m_nc, ta.T() );
2316 break;
2317 }
2318 case 2: {
2319 HepMatrix tb( kc.nc(), 4, 0 );
2320 tb[0][0] = 1.0;
2321 tb[1][1] = 1.0;
2322 tb[2][2] = 1.0;
2323 tb[3][3] = 1.0;
2324 setB( m_nc, pb, tb );
2325 setBT( pb, m_nc, tb.T() );
2326 break;
2327 }
2328 case 3: {
2329 HepMatrix ta( kc.nc(), 1, 0 );
2330 ta[0][0] = -p4Infit( n ).m() * p4Infit( n ).px() /
2331 ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2332 ta[1][0] = -p4Infit( n ).m() * p4Infit( n ).py() /
2333 ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2334 ta[2][0] = -p4Infit( n ).m() * p4Infit( n ).pz() /
2335 ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2336 ta[3][0] = 0;
2337 setA( m_nc, pa, ta );
2338 setAT( pa, m_nc, ta.T() );
2339
2340 HepMatrix tb( kc.nc(), 3, 0 );
2341 tb[0][0] = -p4Infit( n ).py();
2342 tb[0][1] = -p4Infit( n ).px() * ( lambda / ( 1 + lambda * lambda ) );
2343 tb[0][2] =
2344 p4Infit( n ).e() * p4Infit( n ).px() / ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2345 tb[1][0] = p4Infit( n ).px();
2346 tb[1][1] = -p4Infit( n ).py() * ( lambda / ( 1 + lambda * lambda ) );
2347 tb[1][2] =
2348 p4Infit( n ).e() * p4Infit( n ).py() / ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2349 tb[2][0] = 0;
2350 tb[2][1] = p4Infit( n ).pz() * ( 1 / ( lambda * ( 1 + lambda * lambda ) ) );
2351 tb[2][2] =
2352 p4Infit( n ).e() * p4Infit( n ).pz() / ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2353 tb[3][0] = 0;
2354 tb[3][1] = 0;
2355 tb[3][2] = 1.0;
2356
2357 setB( m_nc, pb, tb );
2358 setBT( pb, m_nc, tb.T() );
2359
2360 break;
2361 }
2362 case 4: {
2363 HepMatrix ta( kc.nc(), 2, 0 );
2364 ta[0][0] = -p4Infit( n ).py();
2365 ta[0][1] = -p4Infit( n ).px() * ( lambda / sqrt( 1 + lambda * lambda ) );
2366 ta[1][0] = p4Infit( n ).px();
2367 ta[1][1] = -p4Infit( n ).py() * ( lambda / sqrt( 1 + lambda * lambda ) );
2368 ta[2][0] = 0;
2369 ta[2][1] = p4Infit( n ).pz() * ( 1 / ( lambda * ( 1 + lambda * lambda ) ) );
2370
2371 setA( m_nc, pa, ta );
2372 setAT( pa, m_nc, ta.T() );
2373
2374 HepMatrix tb( kc.nc(), 2, 0 );
2375 // tb[3][0] = p4Infit(n).m()/p4Infit(n).e();
2376 // tb[0][1] = p4Infit(n).px()/p4Infit(n).rho();
2377 // tb[1][1] = p4Infit(n).py()/p4Infit(n).rho();
2378 // tb[2][1] = p4Infit(n).pz()/p4Infit(n).rho();
2379 // tb[3][1] = p4Infit(n).rho()/p4Infit(n).e();
2380 tb[0][0] = -p4Infit( n ).m() * p4Infit( n ).px() /
2381 ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2382 tb[1][0] = -p4Infit( n ).m() * p4Infit( n ).py() /
2383 ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2384 tb[2][0] = -p4Infit( n ).m() * p4Infit( n ).pz() /
2385 ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2386 tb[0][1] =
2387 p4Infit( n ).e() * p4Infit( n ).px() / ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2388 tb[1][1] =
2389 p4Infit( n ).e() * p4Infit( n ).py() / ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2390 tb[2][1] =
2391 p4Infit( n ).e() * p4Infit( n ).pz() / ( p4Infit( n ).rho() * p4Infit( n ).rho() );
2392 tb[3][1] = 1;
2393 setB( m_nc, pb, tb );
2394 setBT( pb, m_nc, tb.T() );
2395 break;
2396 }
2397 case 5: {
2398 HepMatrix ta( kc.nc(), 3, 0 );
2399 ta[0][0] = -p4Infit( n ).py();
2400 ta[0][1] = -p4Infit( n ).px() * ( lambda / sqrt( 1 + lambda * lambda ) );
2401 ta[1][0] = p4Infit( n ).px();
2402 ta[1][1] = -p4Infit( n ).py() * ( lambda / sqrt( 1 + lambda * lambda ) );
2403 ta[2][0] = 0;
2404 ta[2][1] = p4Infit( n ).pz() * ( 1 / ( lambda * ( 1 + lambda * lambda ) ) );
2405 ta[3][2] = p4Infit( n ).m() / p4Infit( n ).e();
2406 setA( m_nc, pa, ta );
2407 setAT( pa, m_nc, ta.T() );
2408
2409 HepMatrix tb( kc.nc(), 1, 0 );
2410 tb[0][0] = p4Infit( n ).px() / p4Infit( n ).rho();
2411 tb[1][0] = p4Infit( n ).py() / p4Infit( n ).rho();
2412 tb[2][0] = p4Infit( n ).pz() / p4Infit( n ).rho();
2413 tb[3][0] = p4Infit( n ).rho() / p4Infit( n ).e();
2414 setB( m_nc, pb, tb );
2415 setBT( pb, m_nc, tb.T() );
2416 break;
2417 }
2418 case 6: {
2419 double ptrk = m_p.sub( pa + 3, pa + 3 )[0];
2420 double x = m_p.sub( pa, pa )[0] - m_q.sub( pb, pb )[0];
2421 double y = m_p.sub( pa + 1, pa + 1 )[0] - m_q.sub( pb + 1, pb + 1 )[0];
2422 double z = m_p.sub( pa + 2, pa + 2 )[0] - m_q.sub( pb + 2, pb + 2 )[0];
2423 double R = sqrt( x * x + y * y + z * z );
2424 HepMatrix ta( kc.nc(), 4, 0 );
2425 ta[0][0] = ptrk * ( y * y + z * z ) / ( R * R * R );
2426 ta[0][1] = -ptrk * x * y / ( R * R * R );
2427 ta[0][2] = -ptrk * x * z / ( R * R * R );
2428 ta[0][3] = x / R;
2429 ta[1][0] = -ptrk * y * x / ( R * R * R );
2430 ta[1][1] = ptrk * ( x * x + z * z ) / ( R * R * R );
2431 ta[1][2] = -ptrk * y * z / ( R * R * R );
2432 ta[1][3] = y / R;
2433 ta[2][0] = -ptrk * z * x / ( R * R * R );
2434 ta[2][1] = -ptrk * z * y / ( R * R * R );
2435 ta[2][2] = ptrk * ( x * x + y * y ) / ( R * R * R );
2436 ta[2][3] = z / R;
2437 ta[3][3] = 1;
2438 setA( m_nc, pa, ta );
2439 setAT( pa, m_nc, ta.T() );
2440
2441 HepMatrix tb( kc.nc(), 3, 0 );
2442 tb[0][0] = -ptrk * ( y * y + z * z ) / ( R * R * R );
2443 tb[0][1] = ptrk * x * y / ( R * R * R );
2444 tb[0][2] = ptrk * x * z / ( R * R * R );
2445 tb[1][0] = ptrk * y * x / ( R * R * R );
2446 tb[1][1] = -ptrk * ( x * x + z * z ) / ( R * R * R );
2447 tb[1][2] = ptrk * y * z / ( R * R * R );
2448 tb[2][0] = ptrk * z * x / ( R * R * R );
2449 tb[2][1] = ptrk * z * y / ( R * R * R );
2450 tb[2][2] = -ptrk * ( x * x + y * y ) / ( R * R * R );
2451 HepMatrix tbp = m_B.sub( 1, 4, 1, 3 );
2452 tb = tbp + tb;
2453 setB( m_nc, pb, tb );
2454 setBT( pb, m_nc, tb.T() );
2455 break;
2456 }
2457 case 7: {
2458 HepMatrix ta( kc.nc(), NTRKPAR, 0 );
2459 ta[0][0] = 1.0;
2460 ta[1][1] = 1.0;
2461 ta[2][2] = 1.0;
2462 ta[3][0] = p4Infit( n ).px() / p4Infit( n ).e();
2463 ta[3][1] = p4Infit( n ).py() / p4Infit( n ).e();
2464 ta[3][2] = p4Infit( n ).pz() / p4Infit( n ).e();
2465 ta[3][3] = p4Infit( n ).m() / p4Infit( n ).e();
2466 setA( m_nc, pa, ta );
2467 setAT( pa, m_nc, ta.T() );
2468 break;
2469 }
2470 }
2471 }
2472
2473 HepVector dc( kc.nc(), 0 );
2474 dc[0] = pmis.px() - p4.px();
2475 dc[1] = pmis.py() - p4.py();
2476 dc[2] = pmis.pz() - p4.pz();
2477 dc[3] = pmis.e() - p4.e();
2478 for ( int i = 0; i < kc.nc(); i++ ) { m_G[m_nc + i] = dc[i]; }
2479
2480 m_nc += 4;
2481
2482 break;
2483 }
2484 }
2485}
p2
double p2[4]
Definition BabayagaNLO.cxx:41
p1
double p1[4]
Definition BabayagaNLO.cxx:41
mass
double mass
Definition CosmicGenerator.cxx:128
HepPoint3D
HepGeom::Point3D< double > HepPoint3D
Definition CosmicGenerator.h:36
n
const Int_t n
Definition DataBase/tau_mode.c:57
etot
Double_t etot
Definition DataBase/tau_mode.c:8
x
Double_t x[10]
Definition DataBase/tau_mode.c:49
a0
character *LEPTONflag integer iresonances real zeta5 real a0
Definition Ekhara/src/ekhara/common.h:4
ptype
string::const_iterator ptype
Definition EvtMTree.hh:19
w
double w
Definition EvtPhokharaDef.hh:28
dc
int dc[18]
Definition EvtPycont.cc:63
sin
double sin(const BesAngle a)
Definition InstallArea/x86_64-el9-gcc13-dbg/include/MdcGeom/BesAngle.h:185
cos
double cos(const BesAngle a)
Definition InstallArea/x86_64-el9-gcc13-dbg/include/MdcGeom/BesAngle.h:187
I
const DifComplex I
m_p
NTuple::Item< double > m_p
Definition MdcHistItem.cxx:62
m_q
NTuple::Item< double > m_q
Definition MdcHistItem.cxx:71
num1
const int num1
Definition InstallArea/x86_64-el9-gcc13-dbg/include/TofSim/BesTofDigitizerEcV3.hh:34
n2
int n2
Definition SD0Tag.cxx:59
n1
int n1
Definition SD0Tag.cxx:58
KalmanKinematicFit::AddTotalEnergy
void AddTotalEnergy(int number, double etot, std::vector< int > lis)
Definition KalmanKinematicFit.cxx:309
KalmanKinematicFit::AddFourMomentum
void AddFourMomentum(int number, HepLorentzVector p4)
Definition KalmanKinematicFit.cxx:351
KalmanKinematicFit::AddThreeMomentum
void AddThreeMomentum(int number, Hep3Vector p3)
Definition KalmanKinematicFit.cxx:334
KalmanKinematicFit::BuildVirtualParticle
void BuildVirtualParticle(int number)
Definition KalmanKinematicFit.cxx:900
KalmanKinematicFit::getCInfit
HepSymMatrix getCInfit(int i) const
Definition KalmanKinematicFit.cxx:1390
KalmanKinematicFit::AddResonance
void AddResonance(int number, double mres, std::vector< int > tlis)
Definition KalmanKinematicFit.cxx:142
KalmanKinematicFit::AddTotalMomentum
void AddTotalMomentum(int number, double ptot, std::vector< int > lis)
Definition KalmanKinematicFit.cxx:226
KalmanKinematicFit::getCOrigin
HepSymMatrix getCOrigin(int i) const
Definition KalmanKinematicFit.cxx:1354
KalmanKinematicFit::instance
static KalmanKinematicFit * instance()
Definition KalmanKinematicFit.cxx:21
KalmanKinematicFit::AddEqualMass
void AddEqualMass(int number, std::vector< int > tlis1, std::vector< int > tlis2)
Definition KalmanKinematicFit.cxx:320
KinematicConstraints::FourMomentumConstraints
void FourMomentumConstraints(const HepLorentzVector p4, std::vector< int > tlis, HepSymMatrix Vme)
Definition KinematicConstraints.cxx:73
KinematicConstraints::ResonanceConstraints
void ResonanceConstraints(const double mres, std::vector< int > tlis, HepSymMatrix Vre)
Definition KinematicConstraints.cxx:34
KinematicConstraints::ThreeMomentumConstraints
void ThreeMomentumConstraints(const Hep3Vector p3, std::vector< int > tlis)
Definition KinematicConstraints.cxx:63
KinematicConstraints::TotalEnergyConstraints
void TotalEnergyConstraints(const double etot, std::vector< int > tlis)
Definition KinematicConstraints.cxx:44
KinematicConstraints::TotalMomentumConstraints
void TotalMomentumConstraints(const double ptot, std::vector< int > tlis)
Definition KinematicConstraints.cxx:53
KinematicConstraints::EqualMassConstraints
void EqualMassConstraints(std::vector< int > tlis1, std::vector< int > tlis2, HepSymMatrix Vne)
Definition KinematicConstraints.cxx:84
TrackPool::AddList
std::vector< int > AddList(int n1)
Definition TrackPool.cxx:441
TrackPool::wTrackInfit
std::vector< WTrackParameter > wTrackInfit() const
Definition Analysis/VertexFit/include/VertexFit/TrackPool.h:72
TrackPool::setVBeamPosition
void setVBeamPosition(const HepSymMatrix VBeamPosition)
Definition Analysis/VertexFit/include/VertexFit/TrackPool.h:145
TrackPool::GammaShapeValue
std::vector< GammaShape > GammaShapeValue() const
Definition Analysis/VertexFit/include/VertexFit/TrackPool.h:94
TrackPool::wTrackOrigin
std::vector< WTrackParameter > wTrackOrigin() const
Definition Analysis/VertexFit/include/VertexFit/TrackPool.h:71
TrackPool::GammaShapeList
std::vector< int > GammaShapeList() const
Definition Analysis/VertexFit/include/VertexFit/TrackPool.h:95
TrackPool::setBeamPosition
void setBeamPosition(const HepPoint3D BeamPosition)
Definition Analysis/VertexFit/include/VertexFit/TrackPool.h:144
TrackPool::setWTrackInfit
void setWTrackInfit(const int n, const WTrackParameter wtrk)
Definition Analysis/VertexFit/include/VertexFit/TrackPool.h:105
rb::R
complex_t R(double Q2, double M2, double G, double Mp2, double Mm2)
Definition TUtil.h:22