00001 #include <cmath>
00002 #include "VertexFit/KalmanKinematicFit.h"
00003 #include "VertexFit/KinematicConstraints.h"
00004 #include "VertexFit/HTrackParameter.h"
00005 #include "TStopwatch.h"
00006 #include "TGraph2D.h"
00007
00008 const int KalmanKinematicFit::NTRKPAR = 4;
00009 const int KalmanKinematicFit::Resonance = 1;
00010 const int KalmanKinematicFit::TotalEnergy = 2;
00011 const int KalmanKinematicFit::TotalMomentum = 4;
00012 const int KalmanKinematicFit::Position = 8;
00013 const int KalmanKinematicFit::ThreeMomentum = 16;
00014 const int KalmanKinematicFit::FourMomentum = 32;
00015 const int KalmanKinematicFit::EqualMass = 64;
00016
00017
00018 KalmanKinematicFit * KalmanKinematicFit::m_pointer = 0;
00019
00020 KalmanKinematicFit * KalmanKinematicFit::instance() {
00021 if(m_pointer) return m_pointer;
00022 m_pointer = new KalmanKinematicFit();
00023 return m_pointer;
00024 }
00025
00026 KalmanKinematicFit::KalmanKinematicFit(){;}
00027
00028 KalmanKinematicFit::~KalmanKinematicFit() {
00029
00030 delete m_pointer;
00031 }
00032
00033
00034 void KalmanKinematicFit::init() {
00035 clearWTrackOrigin();
00036 clearWTrackInfit();
00037 clearWTrackList();
00038
00039 clearGammaShape();
00040 clearGammaShapeList();
00041 clearMapkinematic();
00042 clearMappositionA();
00043 clearMappositionB();
00044 clearone();
00045 cleartwo();
00046 setBeamPosition(HepPoint3D(0.0,0.0,0.0));
00047 setVBeamPosition(HepSymMatrix(3,0));
00048
00049 m_kc.clear();
00050 m_chisq.clear();
00051 m_chi = 9999.;
00052 m_niter = 10;
00053 m_chicut = 200.;
00054 m_chiter = 0.005;
00055 m_espread = 0.0;
00056 m_collideangle = 11e-3;
00057 m_flag = 0;
00058 m_dynamicerror = 0;
00059 m_nc = 0;
00060 m_cpu = HepVector(10, 0);
00061 m_virtual_wtrk.clear();
00062 m_graph2d = 0;
00063
00064 }
00065
00066
00067
00068
00069
00070
00071 void KalmanKinematicFit::AddResonance(int number, double mres, int n1) {
00072 std::vector<int> tlis = AddList(n1);
00073 AddResonance(number, mres, tlis);
00074 }
00075
00076 void KalmanKinematicFit::AddResonance(int number, double mres, int n1, int n2) {
00077 std::vector<int> tlis = AddList(n1, n2);
00078 AddResonance(number, mres, tlis);
00079 }
00080
00081 void KalmanKinematicFit::AddResonance(int number, double mres, int n1, int n2, int n3) {
00082 std::vector<int> tlis = AddList(n1, n2, n3);
00083 AddResonance(number, mres, tlis);
00084 }
00085
00086 void KalmanKinematicFit::AddResonance(int number, double mres, int n1, int n2, int n3, int n4) {
00087 std::vector<int> tlis = AddList(n1, n2, n3, n4);
00088 AddResonance(number, mres, tlis);
00089 }
00090
00091 void KalmanKinematicFit::AddResonance(int number, double mres, int n1, int n2, int n3, int n4,
00092 int n5) {
00093 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5);
00094 AddResonance(number, mres, tlis);
00095 }
00096
00097 void KalmanKinematicFit::AddResonance(int number, double mres, int n1, int n2, int n3, int n4,
00098 int n5, int n6) {
00099 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6);
00100 AddResonance(number, mres, tlis);
00101 }
00102
00103 void KalmanKinematicFit::AddResonance(int number, double mres, int n1, int n2, int n3, int n4,
00104 int n5, int n6, int n7) {
00105 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7);
00106 AddResonance(number, mres, tlis);
00107 }
00108
00109 void KalmanKinematicFit::AddResonance(int number, double mres, int n1, int n2, int n3, int n4,
00110 int n5, int n6, int n7, int n8) {
00111 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8);
00112 AddResonance(number, mres, tlis);
00113 }
00114
00115 void KalmanKinematicFit::AddResonance(int number, double mres, int n1, int n2, int n3, int n4,
00116 int n5, int n6, int n7, int n8, int n9) {
00117 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9);
00118 AddResonance(number, mres, tlis);
00119 }
00120
00121 void KalmanKinematicFit::AddResonance(int number, double mres, int n1, int n2, int n3, int n4,
00122 int n5, int n6, int n7, int n8, int n9, int n10) {
00123 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10);
00124 AddResonance(number, mres, tlis);
00125 }
00126
00127
00128 void KalmanKinematicFit::AddResonance(int number, double mres, int n1, int n2, int n3, int n4,
00129 int n5, int n6, int n7, int n8, int n9,
00130 int n10, int n11) {
00131 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10, n11);
00132 AddResonance(number, mres, tlis);
00133 }
00134
00135 void KalmanKinematicFit::AddResonance(int number, double mres, int n1, int n2, int n3, int n4,
00136 int n5, int n6, int n7, int n8, int n9,
00137 int n10, int n11, int n12) {
00138 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10, n11, n12);
00139 AddResonance(number, mres, tlis);
00140 }
00141
00142 void KalmanKinematicFit::AddResonance(int number, double mres, std::vector<int> tlis) {
00143 KinematicConstraints kc;
00144 HepSymMatrix Vre = HepSymMatrix(1,0);
00145 kc.ResonanceConstraints(mres, tlis, Vre);
00146 m_kc.push_back(kc);
00147 if((unsigned int) number != m_kc.size()-1)
00148 std::cout << "wrong kinematic constraints index" << std::endl;
00149 }
00150
00151
00152
00153
00154
00155
00156 void KalmanKinematicFit::AddTotalMomentum(int number, double ptot, int n1) {
00157 std::vector<int> tlis = AddList(n1);
00158 AddTotalMomentum(number, ptot, tlis);
00159 }
00160 void KalmanKinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2) {
00161 std::vector<int> tlis = AddList(n1, n2);
00162 AddTotalMomentum(number, ptot, tlis);
00163 }
00164
00165 void KalmanKinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2, int n3) {
00166 std::vector<int> tlis = AddList(n1, n2, n3);
00167 AddTotalMomentum(number, ptot, tlis);
00168 }
00169
00170 void KalmanKinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2, int n3, int n4) {
00171 std::vector<int> tlis = AddList(n1, n2, n3, n4);
00172 AddTotalMomentum(number, ptot, tlis);
00173 }
00174
00175 void KalmanKinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2, int n3, int n4,
00176 int n5) {
00177 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5);
00178 AddTotalMomentum(number, ptot, tlis);
00179 }
00180
00181 void KalmanKinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2, int n3, int n4,
00182 int n5, int n6) {
00183 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6);
00184 AddTotalMomentum(number, ptot, tlis);
00185 }
00186
00187 void KalmanKinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2, int n3, int n4,
00188 int n5, int n6, int n7) {
00189 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7);
00190 AddTotalMomentum(number, ptot, tlis);
00191 }
00192
00193 void KalmanKinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2, int n3, int n4,
00194 int n5, int n6, int n7, int n8) {
00195 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8);
00196 AddTotalMomentum(number, ptot, tlis);
00197 }
00198
00199 void KalmanKinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2, int n3, int n4,
00200 int n5, int n6, int n7, int n8, int n9) {
00201 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9);
00202 AddTotalMomentum(number, ptot, tlis);
00203 }
00204
00205 void KalmanKinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2, int n3, int n4,
00206 int n5, int n6, int n7, int n8, int n9,
00207 int n10) {
00208 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10);
00209 AddTotalMomentum(number, ptot, tlis);
00210 }
00211
00212
00213 void KalmanKinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2, int n3, int n4,
00214 int n5, int n6, int n7, int n8, int n9,
00215 int n10, int n11) {
00216 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10, n11);
00217 AddTotalMomentum(number, ptot, tlis);
00218 }
00219
00220 void KalmanKinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2, int n3, int n4,
00221 int n5, int n6, int n7, int n8, int n9,
00222 int n10, int n11, int n12) {
00223 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10, n11, n12);
00224 AddTotalMomentum(number, ptot, tlis);
00225 }
00226
00227 void KalmanKinematicFit::AddTotalMomentum(int number, double ptot, std::vector<int> tlis) {
00228 KinematicConstraints kc;
00229 kc.TotalMomentumConstraints(ptot, tlis);
00230 m_kc.push_back(kc);
00231 if((unsigned int) number != m_kc.size()-1)
00232 std::cout << "wrong kinematic constraints index" << std::endl;
00233 }
00234
00235
00236
00237
00238
00239 void KalmanKinematicFit::AddTotalEnergy(int number, double etot, int n1) {
00240 std::vector<int> tlis = AddList(n1);
00241 AddTotalEnergy(number, etot, tlis);
00242 }
00243 void KalmanKinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2) {
00244 std::vector<int> tlis = AddList(n1, n2);
00245 AddTotalEnergy(number, etot, tlis);
00246 }
00247
00248 void KalmanKinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2, int n3) {
00249 std::vector<int> tlis = AddList(n1, n2, n3);
00250 AddTotalEnergy(number, etot, tlis);
00251 }
00252
00253 void KalmanKinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2, int n3, int n4) {
00254 std::vector<int> tlis = AddList(n1, n2, n3, n4);
00255 AddTotalEnergy(number, etot, tlis);
00256 }
00257
00258 void KalmanKinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2, int n3, int n4,
00259 int n5) {
00260 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5);
00261 AddTotalEnergy(number, etot, tlis);
00262 }
00263
00264 void KalmanKinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2, int n3, int n4,
00265 int n5, int n6) {
00266 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6);
00267 AddTotalEnergy(number, etot, tlis);
00268 }
00269
00270 void KalmanKinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2, int n3, int n4,
00271 int n5, int n6, int n7) {
00272 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7);
00273 AddTotalEnergy(number, etot, tlis);
00274 }
00275
00276 void KalmanKinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2, int n3, int n4,
00277 int n5, int n6, int n7, int n8) {
00278 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8);
00279 AddTotalEnergy(number, etot, tlis);
00280 }
00281
00282 void KalmanKinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2, int n3, int n4,
00283 int n5, int n6, int n7, int n8, int n9) {
00284 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9);
00285 AddTotalEnergy(number, etot, tlis);
00286 }
00287
00288 void KalmanKinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2, int n3, int n4,
00289 int n5, int n6, int n7, int n8, int n9,
00290 int n10) {
00291 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10);
00292 AddTotalEnergy(number, etot, tlis);
00293 }
00294
00295
00296 void KalmanKinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2, int n3, int n4,
00297 int n5, int n6, int n7, int n8, int n9,
00298 int n10, int n11) {
00299 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10, n11);
00300 AddTotalEnergy(number, etot, tlis);
00301 }
00302
00303 void KalmanKinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2, int n3, int n4,
00304 int n5, int n6, int n7, int n8, int n9,
00305 int n10, int n11, int n12) {
00306 std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10, n11, n12);
00307 AddTotalEnergy(number, etot, tlis);
00308 }
00309
00310 void KalmanKinematicFit::AddTotalEnergy(int number, double etot, std::vector<int> tlis) {
00311 KinematicConstraints kc;
00312 kc.TotalEnergyConstraints(etot, tlis);
00313 m_kc.push_back(kc);
00314 if((unsigned int) number != m_kc.size()-1)
00315 std::cout << "wrong kinematic constraints index" << std::endl;
00316 }
00317
00318
00319
00320
00321 void KalmanKinematicFit::AddEqualMass(int number, std::vector<int> tlis1, std::vector<int> tlis2) {
00322 KinematicConstraints kc;
00323 HepSymMatrix Vne = HepSymMatrix(1,0);
00324 kc.EqualMassConstraints(tlis1, tlis2, Vne);
00325 m_kc.push_back(kc);
00326 if((unsigned int) number != m_kc.size()-1)
00327 std::cout << "wrong kinematic constraints index" << std::endl;
00328 }
00329
00330
00331
00332
00333
00334 void KalmanKinematicFit::AddThreeMomentum(int number, Hep3Vector p3) {
00335 std::vector<int> tlis;
00336 tlis.clear();
00337 WTrackParameter wtrk;
00338 KinematicConstraints kc;
00339
00340 for(int i = 0; i < numberWTrack(); i++) {
00341 tlis.push_back(i);
00342 }
00343 kc.ThreeMomentumConstraints(p3, tlis);
00344 m_kc.push_back(kc);
00345 if((unsigned int) number != m_kc.size()-1)
00346 std::cout << "wrong kinematic constraints index" << std::endl;
00347 }
00348
00349
00350
00351
00352
00353 void KalmanKinematicFit::AddFourMomentum(int number, HepLorentzVector p4) {
00354
00355 std::vector<int> tlis;
00356 tlis.clear();
00357 KinematicConstraints kc;
00358
00359 for(int i = 0; i < numberWTrack(); i++) {
00360 tlis.push_back(i);
00361 }
00362
00363 HepSymMatrix Vme = HepSymMatrix(4,0);
00364 Vme[0][0] = 2*m_espread*m_espread*sin(m_collideangle)*sin(m_collideangle);
00365 Vme[0][3] = 2*m_espread*m_espread*sin(m_collideangle);
00366 Vme[3][3] = 2*m_espread*m_espread;
00367
00368 kc.FourMomentumConstraints(p4, tlis, Vme);
00369 m_kc.push_back(kc);
00370 if((unsigned int) number != m_kc.size()-1)
00371 std::cout << "wrong kinematic constraints index" << std::endl;
00372 }
00373
00374 void KalmanKinematicFit::AddFourMomentum(int number, double etot ){
00375
00376 HepLorentzVector p4(0.0, 0.0, 0.0, etot);
00377 std::vector<int> tlis;
00378 tlis.clear();
00379 KinematicConstraints kc;
00380
00381 for(int i = 0; i < numberWTrack(); i++) {
00382 tlis.push_back(i);
00383 }
00384 HepSymMatrix Vme = HepSymMatrix (4,0);
00385 Vme[3][3] = 2*m_espread*m_espread;
00386 kc.FourMomentumConstraints(p4, tlis, Vme);
00387 m_kc.push_back(kc);
00388 if((unsigned int) number != m_kc.size()-1)
00389 std::cout << "wrong kinematic constraints index" << std::endl;
00390 }
00391
00392
00393 void KalmanKinematicFit::fit() {
00394
00395
00396 KinematicConstraints kc;
00397 int nc = m_nc;
00398 int ntrk = numberWTrack();
00399
00400 TStopwatch timer;
00401
00402
00403
00404 timer.Start();
00405
00406 HepSymMatrix AC(m_nc, 0);
00407 AC = m_C0.similarity(m_A);
00408 timer.Stop();
00409 m_cpu[1] += timer.CpuTime();
00410
00411 timer.Start();
00412
00413 int ifail;
00414 m_W = HepSymMatrix(m_nc, 0);
00415 m_W = (m_Vm + m_C0.similarity(m_A)).inverse(ifail);
00416
00417 int ifailD;
00418 m_Dinv = m_D0inv + m_W.similarity(m_B.T());
00419 m_D = m_Dinv.inverse(ifailD);
00420 timer.Stop();
00421 m_cpu[2] += timer.CpuTime();
00422
00423 timer.Start();
00424
00425 HepVector G(m_nc, 0);
00426 G = m_A * (m_p0 - m_p) + m_B * (m_q0 - m_q)+ m_G;
00427
00428 m_KQ = HepMatrix(numbertwo(), m_nc, 0);
00429 m_KQ = m_D * m_BT * m_W;
00430
00431 m_KP = HepMatrix(numberone(), m_nc, 0);
00432 m_KP = m_C0 * m_AT * m_W - m_C0 * m_AT * m_W * m_B * m_KQ;
00433
00434 m_p = m_p0 - m_KP * G;
00435 m_q = m_q0 - m_KQ * G;
00436 timer.Stop();
00437 m_cpu[4] += timer.CpuTime();
00438
00439 timer.Start();
00440
00441 m_chi = (m_W.similarity(G.T()) - m_Dinv.similarity((m_q - m_q0).T()))[0][0];
00442 timer.Stop();
00443 m_cpu[3] += timer.CpuTime();
00444 timer.Start();
00445 if(m_dynamicerror ==1)
00446 gda();
00447
00448 timer.Stop();
00449 m_cpu[6] += timer.CpuTime();
00450
00451 }
00452
00453
00454
00455
00456 void KalmanKinematicFit::upCovmtx() {
00457 HepMatrix E(numberone(), numbertwo(), 0);
00458 E = -m_C0 * m_A.T() * m_KQ.T();
00459 m_C = m_C0 - m_W.similarity((m_A*m_C0).T()) + m_Dinv.similarity(E);
00460 }
00461
00462
00463
00464
00465
00466 void KalmanKinematicFit::fit(int n) {
00467 if(m_chisq.size() == 0) {
00468 for(unsigned int i = 0; i < m_kc.size(); i++)
00469 m_chisq.push_back(9999.0);
00470 }
00471
00472 KinematicConstraints kc;
00473 int nc = m_nc;
00474 int ntrk = numberWTrack();
00475
00476
00477 HepSymMatrix AC(m_nc, 0);
00478 AC = m_C0.similarity(m_A);
00479
00480
00481 int ifail;
00482 m_W = HepSymMatrix(m_nc, 0);
00483 m_W = (m_Vm + m_C0.similarity(m_A)).inverse(ifail);
00484
00485 int ifailD;
00486 m_Dinv = m_D0inv + m_W.similarity(m_B.T());
00487 m_D = m_Dinv.inverse(ifailD);
00488
00489
00490 HepVector G(m_nc, 0);
00491 G = m_A * (m_p0 - m_p) + m_B * (m_q0 - m_q)+ m_G;
00492
00493 m_KQ = HepMatrix(numbertwo(), m_nc, 0);
00494 m_KQ = m_D * m_BT * m_W;
00495
00496 m_KP = HepMatrix(numberone(), m_nc, 0);
00497 m_KP = m_C0 * m_AT * m_W - m_C0 * m_AT * m_W * m_B * m_KQ;
00498
00499 m_p = m_p0 - m_KP * G;
00500 m_q = m_q0 - m_KQ * G;
00501
00502
00503 m_chisq[n] = (m_W.similarity(G.T()) - m_Dinv.similarity((m_q - m_q0).T()))[0][0];
00504 if(m_dynamicerror ==1)
00505 gda();
00506 }
00507
00508
00509
00510
00511
00512
00513
00514
00515 bool KalmanKinematicFit::Fit() {
00516 bool okfit = false;
00517 TStopwatch timer;
00518 m_nktrk = numberWTrack();
00519 m_p0 = HepVector(numberone(), 0);
00520 m_p = HepVector(numberone(), 0);
00521 m_q0 = HepVector(numbertwo(), 0);
00522 m_q = HepVector(numbertwo(), 0);
00523 m_C0 = HepSymMatrix(numberone(), 0);
00524 m_C = HepSymMatrix(numberone(), 0);
00525 m_D0inv = HepSymMatrix(numbertwo(), 0);
00526 m_D = HepSymMatrix(numbertwo(), 0);
00527
00528 HepVector m_p_tmp = HepVector(numberone(), 0);
00529 HepVector m_q_tmp = HepVector(numbertwo(), 0);
00530 HepMatrix m_A_tmp;
00531 HepSymMatrix m_W_tmp;
00532 HepMatrix m_KQ_tmp;
00533 HepSymMatrix m_Dinv_tmp;
00534 for(int i = 0; i < numberWTrack(); i++) {
00535 setWTrackInfit(i, wTrackOrigin(i));
00536 int pa = mappositionA()[i] + 1;
00537 int pb = mappositionB()[i] + 1;
00538 int ptype = mapkinematic()[i];
00539 switch(ptype) {
00540 case 0 : {
00541 HepVector w1(4, 0);
00542 HepSymMatrix Ew1(4, 0);
00543 for(int j = 0; j < 3; j++) {
00544 w1[j] = wTrackOrigin(i).w()[j];
00545 }
00546 w1[3] = wTrackOrigin(i).mass();
00547
00548 for(int m = 0; m < 3; m++) {
00549 for(int n = 0; n < 3; n++) {
00550 Ew1[m][n] = wTrackOrigin(i).Ew()[m][n];
00551 }
00552 }
00553 setPOrigin(pa, w1);
00554 setPInfit(pa, w1);
00555 setCOrigin(pa, Ew1);
00556 break;
00557 }
00558 case 1 : {
00559 setPOrigin(pa, (wTrackOrigin(i).plmp()).sub(1, 4));
00560 setPInfit(pa, (wTrackOrigin(i).plmp()).sub(1, 4));
00561 setCOrigin(pa, (wTrackOrigin(i).Vplm()).sub(1, 4));
00562 break;
00563 }
00564 case 2 : {
00565 setQOrigin(pb, (wTrackOrigin(i).w()).sub(1, NTRKPAR));
00566 setQInfit(pb, (wTrackOrigin(i).w()).sub(1, NTRKPAR));
00567 HepSymMatrix Dinv(4,0);
00568 setDOriginInv(pb, Dinv);
00569 break;
00570 }
00571 case 3 : {
00572 setPOrigin(pa, (wTrackOrigin(i).plmp()).sub(3, 3));
00573 setPInfit(pa, (wTrackOrigin(i).plmp()).sub(3, 3));
00574 setCOrigin(pa, (wTrackOrigin(i).Vplm()).sub(3, 3));
00575 setQOrigin(pb, (wTrackOrigin(i).plmp()).sub(1, 2));
00576 setQInfit(pb, (wTrackOrigin(i).plmp()).sub(1, 2));
00577 setQOrigin(pb+2, (wTrackOrigin(i).plmp()).sub(4, 4));
00578 setQInfit(pb+2, (wTrackOrigin(i).plmp()).sub(4, 4));
00579 HepSymMatrix Dinv(3,0);
00580 setDOriginInv(pb, Dinv);
00581 break;
00582 }
00583 case 4 : {
00584 setPOrigin(pa, (wTrackOrigin(i).plmp()).sub(1, 2));
00585 setPInfit(pa, (wTrackOrigin(i).plmp()).sub(1, 2));
00586 setCOrigin(pa, (wTrackOrigin(i).Vplm()).sub(1, 2));
00587 setQOrigin(pb, (wTrackOrigin(i).plmp()).sub(3, 4));
00588 setQInfit(pb, (wTrackOrigin(i).plmp()).sub(3, 4));
00589 HepSymMatrix Dinv(2,0);
00590 setDOriginInv(pb, Dinv);
00591 break;
00592 }
00593 case 5 : {
00594 setPOrigin(pa, (wTrackOrigin(i).plmp()).sub(1, 3));
00595 setPInfit(pa, (wTrackOrigin(i).plmp()).sub(1, 3));
00596 setCOrigin(pa, (wTrackOrigin(i).Vplm()).sub(1, 3));
00597 setQOrigin(pb, (wTrackOrigin(i).plmp()).sub(4, 4));
00598 setQInfit(pb, (wTrackOrigin(i).plmp()).sub(4, 4));
00599 HepSymMatrix Dinv(1,0);
00600 setDOriginInv(pb, Dinv);
00601 break;
00602 }
00603 case 6 : {
00604 setPOrigin(pa, (wTrackOrigin(i).w()).sub(5, 7));
00605 setPOrigin(pa+3, (wTrackOrigin(i).w()).sub(4, 4));
00606 setPInfit(pa, (wTrackOrigin(i).w()).sub(5, 7));
00607 setPInfit(pa+3, (wTrackOrigin(i).w()).sub(4, 4));
00608 setCOrigin(pa, (wTrackOrigin(i).Ew()).sub(5,7));
00609 setCOrigin(pa+3, (wTrackOrigin(i).Ew()).sub(4,4));
00610 HepVector beam(3,0);
00611 beam[0] = getBeamPosition().x();
00612 beam[1] = getBeamPosition().y();
00613 beam[2] = getBeamPosition().z();
00614 setQOrigin(pb, beam);
00615 setQInfit(pb, beam);
00616 HepSymMatrix Dinv(3, 0);
00617 int ifail;
00618 Dinv = getVBeamPosition().inverse(ifail);
00619 setDOriginInv(pb, Dinv);
00620 break;
00621 }
00622 case 7 : {
00623 HepVector w1(4, 0);
00624 HepSymMatrix Ew1(4, 0);
00625 for(int j = 0; j < 3; j++) {
00626 w1[j] = wTrackOrigin(i).w()[j];
00627 }
00628 w1[3] = wTrackOrigin(i).mass();
00629
00630 for(int m = 0; m < 3; m++) {
00631 for(int n = 0; n < 3; n++) {
00632 Ew1[m][n] = wTrackOrigin(i).Ew()[m][n];
00633 }
00634 }
00635 setPOrigin(pa, w1);
00636 setPInfit(pa, w1);
00637 setCOrigin(pa, Ew1);
00638 break;
00639 }
00640
00641 }
00642 }
00643
00644
00645
00646
00647
00648
00649 std::vector<double> chisq;
00650 chisq.clear();
00651 int nc = 0;
00652 for(int i = 0; i < m_kc.size(); i++)
00653 nc += m_kc[i].nc();
00654
00655 m_A = HepMatrix(nc, numberone(), 0);
00656 m_AT = HepMatrix(numberone(), nc, 0);
00657 m_B = HepMatrix(nc, numbertwo(), 0);
00658 m_BT = HepMatrix(numbertwo(), nc, 0);
00659 m_G = HepVector(nc, 0);
00660 m_Vm = HepSymMatrix(nc, 0);
00661 int num1 = 0;
00662 for(unsigned int i = 0; i < m_kc.size(); i++) {
00663 KinematicConstraints kc = m_kc[i];
00664 m_Vm.sub(num1+1,kc.Vmeasure());
00665 num1 = num1 + kc.nc();
00666 }
00667
00668 double tmp_chisq = 999;
00669 bool flag_break = 0;
00670 for(int it = 0; it < m_niter; it++) {
00671 timer.Start();
00672 m_nc = 0;
00673 for(unsigned int i = 0; i < m_kc.size(); i++) {
00674 KinematicConstraints kc = m_kc[i];
00675 updateConstraints(kc);
00676 }
00677 timer.Stop();
00678 m_cpu[0] += timer.CpuTime();
00679 fit();
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
00702
00703
00704
00705
00706
00707
00708
00709
00710
00711
00712
00713
00714
00715
00716
00717
00718
00719
00720
00721
00722
00723
00724
00725
00726
00727
00728
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742
00743
00744
00745 chisq.push_back(m_chi);
00746 if(it > 0) {
00747 double delchi = chisq[it]- chisq[it-1];
00748 if(fabs(delchi) < m_chiter){
00749 flag_break =1;
00750 break;
00751 }
00752 }
00753 }
00754 if(!flag_break) {
00755 return okfit;
00756 }
00757 if(m_chi > m_chicut) return okfit;
00758 timer.Start();
00759 timer.Stop();
00760 m_cpu[5] += timer.CpuTime();
00761 okfit = true;
00762 return okfit;
00763
00764 }
00765
00766
00767 bool KalmanKinematicFit::Fit(int n) {
00768 bool okfit = false;
00769 if(n < 0 || (unsigned int)n >= m_kc.size()) return okfit;
00770 m_nktrk = numberWTrack();
00771 m_p0 = HepVector(numberone(), 0);
00772 m_p = HepVector(numberone(), 0);
00773 m_q0 = HepVector(numbertwo(), 0);
00774 m_q = HepVector(numbertwo(), 0);
00775 m_C0 = HepSymMatrix(numberone(), 0);
00776 m_C = HepSymMatrix(numberone(), 0);
00777 m_D0inv = HepSymMatrix(numbertwo(), 0);
00778 m_D = HepSymMatrix(numbertwo(), 0);
00779
00780 for(int i = 0; i < numberWTrack(); i++) {
00781 setWTrackInfit(i, wTrackOrigin(i));
00782 int pa = mappositionA()[i] + 1;
00783 int pb = mappositionB()[i] + 1;
00784 int ptype = mapkinematic()[i];
00785 switch(ptype) {
00786 case 0 : {
00787 HepVector w1(4, 0);
00788 HepSymMatrix Ew1(4, 0);
00789 for(int j = 0; j < 3; j++) {
00790 w1[j] = wTrackOrigin(i).w()[j];
00791 }
00792 w1[3] = wTrackOrigin(i).mass();
00793
00794 for(int m = 0; m < 3; m++) {
00795 for(int n = 0; n < 3; n++) {
00796 Ew1[m][n] = wTrackOrigin(i).Ew()[m][n];
00797 }
00798 }
00799 setPOrigin(pa, w1);
00800 setPInfit(pa, w1);
00801 setCOrigin(pa, Ew1);
00802 break;
00803 }
00804 case 1 : {
00805 setPOrigin(pa, (wTrackOrigin(i).plmp()).sub(1, 4));
00806 setPInfit(pa, (wTrackOrigin(i).plmp()).sub(1, 4));
00807 setCOrigin(pa, (wTrackOrigin(i).Vplm()).sub(1, 4));
00808 break;
00809 }
00810 case 2 : {
00811 setQOrigin(pb, (wTrackOrigin(i).w()).sub(1, NTRKPAR));
00812 setQInfit(pb, (wTrackOrigin(i).w()).sub(1, NTRKPAR));
00813 HepSymMatrix Dinv(4,0);
00814 setDOriginInv(pb, Dinv);
00815 break;
00816 }
00817 case 3 : {
00818 setPOrigin(pa, (wTrackOrigin(i).plmp()).sub(3, 3));
00819 setPInfit(pa, (wTrackOrigin(i).plmp()).sub(3, 3));
00820 setCOrigin(pa, (wTrackOrigin(i).Vplm()).sub(3, 3));
00821 setQOrigin(pb, (wTrackOrigin(i).w()).sub(1, 3));
00822 setQInfit(pb, (wTrackOrigin(i).w()).sub(1, 3));
00823 HepSymMatrix Dinv(3,0);
00824 setDOriginInv(pb, Dinv);
00825 break;
00826 }
00827 case 4 : {
00828 setPOrigin(pa, (wTrackOrigin(i).plmp()).sub(1, 2));
00829 setPInfit(pa, (wTrackOrigin(i).plmp()).sub(1, 2));
00830 setCOrigin(pa, (wTrackOrigin(i).Vplm()).sub(1, 2));
00831 setQOrigin(pb, (wTrackOrigin(i).plmp()).sub(3, 4));
00832 setQInfit(pb, (wTrackOrigin(i).plmp()).sub(3, 4));
00833 HepSymMatrix Dinv(2,0);
00834 setDOriginInv(pb, Dinv);
00835 break;
00836 }
00837 case 5 : {
00838 setPOrigin(pa, (wTrackOrigin(i).plmp()).sub(1, 3));
00839 setPInfit(pa, (wTrackOrigin(i).plmp()).sub(1, 3));
00840 setCOrigin(pa, (wTrackOrigin(i).Vplm()).sub(1, 3));
00841 setQOrigin(pb, (wTrackOrigin(i).plmp()).sub(4, 4));
00842 setQInfit(pb, (wTrackOrigin(i).plmp()).sub(4, 4));
00843 HepSymMatrix Dinv(1,0);
00844 setDOriginInv(pb, Dinv);
00845 break;
00846 }
00847 case 6 : {
00848 setPOrigin(pa, (wTrackOrigin(i).w()).sub(5, 7));
00849 setPOrigin(pa+3, (wTrackOrigin(i).w()).sub(4, 4));
00850 setPInfit(pa, (wTrackOrigin(i).w()).sub(5, 7));
00851 setPInfit(pa+3, (wTrackOrigin(i).w()).sub(4, 4));
00852 setCOrigin(pa, (wTrackOrigin(i).Ew()).sub(5,7));
00853 setCOrigin(pa+3, (wTrackOrigin(i).Ew()).sub(4,4));
00854 HepVector beam(3,0);
00855 beam[0] = getBeamPosition().x();
00856 beam[1] = getBeamPosition().y();
00857 beam[2] = getBeamPosition().z();
00858 setQOrigin(pb, beam);
00859 setQInfit(pb, beam);
00860 HepSymMatrix Dinv(3, 0);
00861 int ifail;
00862 Dinv = getVBeamPosition().inverse(ifail);
00863
00864 setDOriginInv(pb, Dinv);
00865 break;
00866 }
00867
00868 case 7 : {
00869 HepVector w1(4, 0);
00870 HepSymMatrix Ew1(4, 0);
00871 for(int j = 0; j < 3; j++) {
00872 w1[j] = wTrackOrigin(i).w()[j];
00873 }
00874 w1[3] = wTrackOrigin(i).mass();
00875
00876 for(int m = 0; m < 3; m++) {
00877 for(int n = 0; n < 3; n++) {
00878 Ew1[m][n] = wTrackOrigin(i).Ew()[m][n];
00879 }
00880 }
00881 setPOrigin(pa, w1);
00882 setPInfit(pa, w1);
00883 setCOrigin(pa, Ew1);
00884 break;
00885 }
00886 }
00887 }
00888
00889
00890
00891
00892
00893
00894
00895 std::vector<double> chisq;
00896 chisq.clear();
00897 int nc = 0;
00898
00899 nc += m_kc[n].nc();
00900
00901 m_A = HepMatrix(nc, numberone(), 0);
00902 m_AT = HepMatrix(numberone(), nc, 0);
00903 m_B = HepMatrix(nc, numbertwo(), 0);
00904 m_BT = HepMatrix(numbertwo(), nc, 0);
00905 m_G = HepVector(nc, 0);
00906 m_Vm = HepSymMatrix(nc, 0);
00907 int num1 = 0;
00908 KinematicConstraints kc = m_kc[n];
00909 m_Vm.sub(num1+1,kc.Vmeasure());
00910 num1 = kc.nc();
00911 for(int it = 0; it < m_niter; it++) {
00912 m_nc = 0;
00913 KinematicConstraints kc = m_kc[n];
00914 updateConstraints(kc);
00915 fit(n);
00916
00917 chisq.push_back(m_chisq[n]);
00918
00919 if(it > 0) {
00920
00921 double delchi = chisq[it]- chisq[it-1];
00922 if(fabs(delchi) < m_chiter)
00923 break;
00924 }
00925 }
00926 if(m_chisq[n] >= m_chicut) {
00927 return okfit;
00928 }
00929
00930
00931
00932 okfit = true;
00933
00934
00935 return okfit;
00936
00937 }
00938
00939
00940
00941
00942
00943 void KalmanKinematicFit::BuildVirtualParticle( int n) {
00944 upCovmtx();
00945 KinematicConstraints kc = m_kc[n];
00946 int ntrk = (kc.Ltrk()).size();
00947 int charge = 0;
00948 HepVector w(7, 0);
00949 HepSymMatrix ew1(7, 0);
00950 HepSymMatrix ew2(7, 0);
00951 HepVector w4(4, 0);
00952 HepSymMatrix ew4(4, 0);
00953 HepMatrix dwdp(4, 4, 0);
00954 dwdp[0][0] = 1;
00955 dwdp[1][1] = 1;
00956 dwdp[2][2] = 1;
00957 dwdp[3][3] = 1;
00958 for (int i = 0; i < ntrk; i++) {
00959 int itk = (kc.Ltrk())[i];
00960 charge += wTrackInfit(itk).charge();
00961 w4 = w4 + dwdp * pInfit(itk);
00962
00963 }
00964 HepMatrix I(4, numberone(), 0);
00965 for(int j2=0; j2<ntrk; j2++){
00966 I[0][0+j2*4] = 1;
00967 I[1][1+j2*4] = 1;
00968 I[2][2+j2*4] = 1;
00969 I[3][3+j2*4] = 1;
00970 }
00971 ew4 = m_C.similarity(I);
00972 HepMatrix J(7,4,0);
00973 double px = w4[0];
00974 double py = w4[1];
00975 double pz = w4[2];
00976 double e = w4[3];
00977 double pt = sqrt(px*px + py*py);
00978 double p0 = sqrt(px*px + py*py + pz*pz);
00979 double m = sqrt(e*e - p0*p0);
00980 J[0][0] = -py;
00981 J[0][1] = -(pz*px*pt)/(p0*p0);
00982 J[0][2] = -m*px/(p0*p0);
00983 J[0][3] = e*px/(p0*p0);
00984 J[1][0] = px;
00985 J[1][1] = -(pz*py*pt)/(p0*p0);
00986 J[1][2] = -m*py/(p0*p0);
00987 J[1][3] = e*py/(p0*p0);
00988 J[2][0] = 0;
00989 J[2][1] = pt*pt*pt/(p0*p0);
00990 J[2][2] = -m*pz/(p0*p0);
00991 J[2][3] = e*pz/(p0*p0);
00992 J[3][0] = 0;
00993 J[3][1] = 0;
00994 J[3][2] = 0;
00995 J[3][3] = 1;
00996 ew1 = ew4.similarity(J);
00997 ew2[0][0] = ew1[0][0];
00998 ew2[1][1] = ew1[1][1];
00999 ew2[2][2] = ew1[2][2];
01000 ew2[3][3] = ew1[3][3];
01001 w[0] = w4[0];
01002 w[1] = w4[1];
01003 w[2] = w4[2];
01004 w[3] = w4[3];
01005 WTrackParameter vwtrk;
01006 vwtrk.setCharge(charge);
01007 vwtrk.setW(w);
01008 vwtrk.setEw(ew2);
01009 vwtrk.setMass(m);
01010 m_virtual_wtrk.push_back(vwtrk);
01011 }
01012
01013
01014
01015 void KalmanKinematicFit::gda(){
01016
01017 for(int i = 0; i < numberWTrack(); i++) {
01018 if ((wTrackOrigin(i)).type() == 2 ){
01019 int n ;
01020 for(int j = 0; j < numberGammaShape(); j++) {
01021 if(i==GammaShapeList(j)) n = j;
01022 }
01023 int pa = mappositionA()[i] + 1;
01024 int pb = mappositionB()[i] + 1;
01025 int ptype = mapkinematic()[i];
01026 HepSymMatrix Ew(NTRKPAR, 0);
01027 HepLorentzVector p1 = p4Infit(i);
01028 HepLorentzVector p2 = p4Origin(i);
01029 double eorigin = pOrigin(i)[3];
01030 HepMatrix dwda1(NTRKPAR, 3, 0);
01031 dwda1[0][0] = 1;
01032 dwda1[1][1] = - (p2.e()*p2.e())/(p2.px()*p2.px() + p2.py()*p2.py());
01033
01034 dwda1[3][2] = 1;
01035 HepSymMatrix emcmea1(3, 0);
01036 double pk = p1[3];
01037
01038 emcmea1[0][0] = GammaShapeValue(n).getdphi() * GammaShapeValue(n).getdphi();
01039 emcmea1[1][1] = GammaShapeValue(n).getdthe() * GammaShapeValue(n).getdthe()*(1/(1 - p2.cosTheta()*p2.cosTheta())) *(1/(1 - p2.cosTheta()*p2.cosTheta()));
01040
01041 emcmea1[2][2] = GammaShapeValue(n).de(eorigin,pk)*GammaShapeValue(n).de(eorigin,pk);
01042
01043
01044
01045
01046
01047
01048
01049 Ew[0][0] = emcmea1[0][0];
01050 Ew[1][1] = emcmea1[1][1];
01051 Ew[3][3] = emcmea1[2][2];
01052
01053 if(ptype==6)
01054 setCOrigin(pa+3, Ew.sub(4,4));
01055 else setCOrigin(pa,Ew);
01056 }
01057 }
01058 }
01059
01060
01061 HepVector KalmanKinematicFit::pull(int n){
01062 upCovmtx();
01063 int pa = mappositionA()[n] + 1;
01064 int pb = mappositionB()[n] + 1 ;
01065 int ptype = mapkinematic()[n];
01066 switch(ptype) {
01067 case 0 :{
01068 HepVector W(7,0);
01069 HepSymMatrix Ew(7,0);
01070 HepVector W1(7,0);
01071 HepSymMatrix Ew1(7,0);
01072 WTrackParameter wtrk = wTrackOrigin(n);
01073 W = wTrackOrigin(n).w();
01074 Ew = wTrackOrigin(n).Ew();
01075 for(int i=0; i<4; i++) {
01076 W[i] = pInfit(n)[i];
01077 }
01078 for(int j=0; j<4; j++) {
01079 for(int k=0; k<4; k++) {
01080 Ew[j][k] = getCInfit(n)[j][k];
01081 }
01082 }
01083 W1 = W;
01084 double px = p4Infit(n).px();
01085 double py = p4Infit(n).py();
01086 double pz = p4Infit(n).pz();
01087 double m = p4Infit(n).m();
01088 double e = p4Infit(n).e();
01089 HepMatrix J(7, 7, 0);
01090 J[0][0] = 1;
01091 J[1][1] = 1;
01092 J[2][2] = 1;
01093 J[3][0] = px/e;
01094 J[3][1] = py/e;
01095 J[3][2] = pz/e;
01096 J[3][3] = m/e;
01097 J[4][4] = 1;
01098 J[5][5] = 1;
01099 J[6][6] = 1;
01100 Ew1 = Ew.similarity(J);
01101
01102 wtrk.setW(W1);
01103 wtrk.setEw(Ew1);
01104 setWTrackInfit(n, wtrk);
01105
01106 HTrackParameter horigin = HTrackParameter(wTrackOrigin(n));
01107 HTrackParameter hinfit = HTrackParameter(wTrackInfit(n));
01108 HepVector a0 = horigin.hel();
01109 HepVector a1 = hinfit.hel();
01110 HepSymMatrix v0 = horigin.eHel();
01111 HepSymMatrix v1 = hinfit.eHel();
01112 HepVector pull(9,0);
01113 for (int k=0; k<5; k++) {
01114 pull[k] = (a0[k]-a1[k])/sqrt(abs(v0[k][k]-v1[k][k]));
01115 }
01116 for (int l=5; l<9; l++) {
01117 pull[l] = (wTrackOrigin(n).w()[l-5] - wTrackInfit(n).w()[l-5])/sqrt(abs(wTrackOrigin(n).Ew()[l-5][l-5] - wTrackInfit(n).Ew()[l-5][l-5]));
01118 }
01119 return pull;
01120 break;
01121 }
01122 case 1 : {
01123 HepVector a0(4,0);
01124 HepVector a1(4,0);
01125 a0 = m_p0.sub(pa, pa+3);
01126 a1 = m_p.sub(pa, pa+3);
01127 HepSymMatrix v1 = getCInfit(n);
01128 HepSymMatrix v0 = getCOrigin(n);
01129 HepVector pull(3,0);
01130 for (int k=0; k<2; k++) {
01131 pull[k] = (a0[k]-a1[k])/sqrt(v0[k][k]-v1[k][k]);
01132 }
01133 pull[2] = (a0[3]-a1[3])/sqrt(v0[3][3]-v1[3][3]);
01134 return pull;
01135 break;
01136 }
01137
01138
01139
01140
01141
01142
01143
01144
01145
01146
01147
01148
01149 case 5 : {
01150 HepLorentzVector p0 = p4Origin(n);
01151 HepLorentzVector p1 = p4Infit(n);
01152 HepVector a0(2,0);
01153 HepVector a1(2,0);
01154 a0[0] = p4Origin(n).phi();
01155 a1[0] = p4Infit(n).phi();
01156 a0[1] = p4Origin(n).pz()/p4Origin(n).perp();
01157 a1[1] = p4Infit(n).pz()/p4Infit(n).perp();
01158 HepMatrix Jacobi(2, 4, 0);
01159 Jacobi[0][0] = - p4Infit(n).py()/p4Infit(n).perp2();
01160 Jacobi[0][1] = p4Infit(n).px()/p4Infit(n).perp2();
01161 Jacobi[1][0] = - (p4Infit(n).px()/p4Infit(n).perp()) * (p4Infit(n).pz()/p4Infit(n).perp2());
01162 Jacobi[1][1] = - (p4Infit(n).py()/p4Infit(n).perp()) * (p4Infit(n).pz()/p4Infit(n).perp2());
01163 Jacobi[1][2] = 1/p4Infit(n).perp();
01164 HepSymMatrix v1 = getCInfit(n).similarity(Jacobi);
01165 HepSymMatrix v0 = wTrackOrigin(n).Vplm().sub(1,2);
01166 HepVector pull(2,0);
01167 for (int k=0; k<2; k++) {
01168 pull[k] = (a0[k]-a1[k])/sqrt(v0[k][k]-v1[k][k]);
01169 }
01170 return pull;
01171 break;
01172 }
01173 }
01174 }
01175
01176
01177
01178 HepVector KalmanKinematicFit::pInfit(int n) const {
01179 int pa = mappositionA()[n] + 1;
01180 int pb = mappositionB()[n] + 1;
01181 int ptype = mapkinematic()[n];
01182 switch(ptype) {
01183 case 0 : {
01184 double mass = m_p.sub(pa+3, pa+3)[0];
01185 HepVector p4(4,0);
01186 p4[0] = m_p.sub(pa,pa)[0];
01187 p4[1] = m_p.sub(pa+1, pa+1)[0];
01188 p4[2] = m_p.sub(pa+2, pa+2)[0];
01189 p4[3] = sqrt(p4[0]*p4[0] + p4[1]*p4[1] + p4[2]*p4[2] + mass * mass);
01190 return p4;
01191 break;
01192 }
01193 case 1 : {
01194 double phi = m_p.sub(pa, pa)[0];
01195 double lambda = m_p.sub(pa+1, pa+1)[0];
01196 double mass = m_p.sub(pa+2, pa+2)[0];
01197 double E = m_p.sub(pa+3, pa+3)[0];
01198 double p0 = sqrt(E*E - mass*mass);
01199 HepVector p4(4,0);
01200 p4[0] = p0*cos(phi)/sqrt(1 + lambda*lambda);
01201 p4[1] = p0*sin(phi)/sqrt(1 + lambda*lambda);
01202 p4[2] = p0*lambda/sqrt(1 + lambda*lambda);
01203 p4[3] = sqrt(p0*p0 + mass*mass);
01204 return p4;
01205 break;
01206 }
01207 case 2 : {
01208 return m_q.sub(pb, pb+3);
01209 break;
01210 }
01211 case 3 : {
01212 double mass = (m_p.sub(pa, pa))[0];
01213 double phi = m_q.sub(pb, pb)[0];
01214 double lambda = m_q.sub(pb+1, pb+1)[0];
01215 double E = m_q.sub(pb+2, pb+2)[0];
01216 double p0 = sqrt(E*E - mass*mass);
01217 HepVector p4(4,0);
01218 p4[0] = p0*cos(phi)/sqrt(1 + lambda*lambda);
01219 p4[1] = p0*sin(phi)/sqrt(1 + lambda*lambda);
01220 p4[2] = p0*lambda/sqrt(1 + lambda*lambda);
01221 p4[3] = sqrt(p0*p0 + mass*mass);
01222 return p4;
01223 break;
01224 }
01225 case 4 : {
01226 double phi = m_p.sub(pa, pa)[0];
01227 double lambda = m_p.sub(pa+1, pa+1)[0];
01228 double mass = m_q.sub(pb, pb)[0];
01229 double E = m_q.sub(pb+1, pb+1)[0];
01230 double p0 = sqrt(E*E - mass*mass);
01231 HepVector p4(4,0);
01232 p4[0] = p0*cos(phi)/sqrt(1 + lambda*lambda);
01233 p4[1] = p0*sin(phi)/sqrt(1 + lambda*lambda);
01234 p4[2] = p0*lambda/sqrt(1 + lambda*lambda);
01235 p4[3] = sqrt(p0*p0 + mass*mass);
01236 return p4;
01237 break;
01238 }
01239 case 5 : {
01240 double phi = m_p.sub(pa, pa)[0];
01241 double lambda = m_p.sub(pa+1, pa+1)[0];
01242 double mass = m_p.sub(pa+2, pa+2)[0];
01243 double p0 = m_q.sub(pb, pb)[0];
01244 HepVector p4(4,0);
01245 p4[0] = p0*cos(phi)/sqrt(1 + lambda*lambda);
01246 p4[1] = p0*sin(phi)/sqrt(1 + lambda*lambda);
01247 p4[2] = p0*lambda/sqrt(1 + lambda*lambda);
01248 p4[3] = sqrt(p0*p0 + mass*mass);
01249 return p4;
01250 break;
01251 }
01252 case 6 : {
01253 double x = m_p.sub(pa, pa)[0] - m_q.sub(pb, pb)[0];
01254 double y = m_p.sub(pa+1, pa+1)[0] - m_q.sub(pb+1, pb+1)[0];
01255 double z = m_p.sub(pa+2, pa+2)[0] - m_q.sub(pb+2, pb+2)[0];
01256 double p0 = m_p.sub(pa+3, pa+3)[0];
01257 double R = sqrt(x*x + y*y + z*z);
01258 HepVector p4(4,0);
01259 p4[0] = p0*x/R;
01260 p4[1] = p0*y/R;
01261 p4[2] = p0*z/R;
01262 p4[3] = p0;
01263 return p4;
01264 break;
01265 }
01266 case 7 : {
01267 double mass = m_p.sub(pa+3, pa+3)[0];
01268 HepVector p4(4,0);
01269 p4[0] = m_p.sub(pa,pa)[0];
01270 p4[1] = m_p.sub(pa+1, pa+1)[0];
01271 p4[2] = m_p.sub(pa+2, pa+2)[0];
01272 p4[3] = sqrt(p4[0]*p4[0] + p4[1]*p4[1] + p4[2]*p4[2] + mass * mass);
01273 return p4;
01274 break;
01275 }
01276 }
01277 }
01278
01279
01280 HepVector KalmanKinematicFit::pOrigin(int n) const {
01281 int pa = mappositionA()[n] + 1;
01282 int pb = mappositionB()[n] + 1;
01283 int ptype = mapkinematic()[n];
01284 switch(ptype) {
01285 case 0 : {
01286 double mass = m_p0.sub(pa+3, pa+3)[0];
01287 HepVector p4(4,0);
01288 p4[0] = m_p0.sub(pa,pa)[0];
01289 p4[1] = m_p0.sub(pa+1, pa+1)[0];
01290 p4[2] = m_p0.sub(pa+2, pa+2)[0];
01291 p4[3] = sqrt(p4[0]*p4[0] + p4[1]*p4[1] + p4[2]*p4[2] + mass * mass);
01292 return p4;
01293 break;
01294 }
01295 case 1 : {
01296 double phi = m_p0.sub(pa, pa)[0];
01297 double lambda = m_p0.sub(pa+1, pa+1)[0];
01298 double mass = m_p0.sub(pa+2, pa+2)[0];
01299 double E = m_p0.sub(pa+3, pa+3)[0];
01300 double p0 = sqrt(E*E - mass*mass);
01301 HepVector p4(4,0);
01302 p4[0] = p0*cos(phi)/sqrt(1 + lambda*lambda);
01303 p4[1] = p0*sin(phi)/sqrt(1 + lambda*lambda);
01304 p4[2] = p0*lambda/sqrt(1 + lambda*lambda);
01305 p4[3] = sqrt(p0*p0 + mass*mass);
01306 return p4;
01307 break;
01308 }
01309 case 2 : {
01310 return m_q0.sub(pb, pb+3);
01311 break;
01312 }
01313 case 3 : {
01314 double mass = (m_p0.sub(pa, pa))[0];
01315 double phi = m_q0.sub(pb, pb)[0];
01316 double lambda = m_q0.sub(pb+1, pb+1)[0];
01317 double E = m_q0.sub(pb+2, pb+2)[0];
01318 double p0 = sqrt(E*E - mass*mass);
01319 HepVector p4(4,0);
01320 p4[0] = p0*cos(phi)/sqrt(1 + lambda*lambda);
01321 p4[1] = p0*sin(phi)/sqrt(1 + lambda*lambda);
01322 p4[2] = p0*lambda/sqrt(1 + lambda*lambda);
01323 p4[3] = sqrt(p0*p0 + mass*mass);
01324 return p4;
01325 break;
01326 }
01327 case 4 : {
01328 double phi = m_p0.sub(pa, pa)[0];
01329 double lambda = m_p0.sub(pa+1, pa+1)[0];
01330 double mass = m_q0.sub(pb, pb)[0];
01331 double E = m_q0.sub(pb+1, pb+1)[0];
01332 double p0 = sqrt(E*E - mass*mass);
01333 HepVector p4(4,0);
01334 p4[0] = p0*cos(phi)/sqrt(1 + lambda*lambda);
01335 p4[1] = p0*sin(phi)/sqrt(1 + lambda*lambda);
01336 p4[2] = p0*lambda/sqrt(1 + lambda*lambda);
01337 p4[3] = sqrt(p0*p0 + mass*mass);
01338 return p4;
01339 break;
01340 }
01341 case 5 : {
01342 double phi = m_p0.sub(pa, pa)[0];
01343 double lambda = m_p0.sub(pa+1, pa+1)[0];
01344 double mass = m_p0.sub(pa+2, pa+2)[0];
01345 double p0 = m_q0.sub(pb, pb)[0];
01346 HepVector p4(4,0);
01347 p4[0] = p0*cos(phi)/sqrt(1 + lambda*lambda);
01348 p4[1] = p0*sin(phi)/sqrt(1 + lambda*lambda);
01349 p4[2] = p0*lambda/sqrt(1 + lambda*lambda);
01350 p4[3] = sqrt(p0*p0 + mass*mass);
01351 return p4;
01352 break;
01353 }
01354 case 6 : {
01355 double x = m_p0.sub(pa, pa)[0] - m_q0.sub(pb, pb)[0];
01356 double y = m_p0.sub(pa+1, pa+1)[0] - m_q0.sub(pb+1, pb+1)[0];
01357 double z = m_p0.sub(pa+2, pa+2)[0] - m_q0.sub(pb+2, pb+2)[0];
01358 double p0 = m_p0.sub(pa+3, pa+3)[0];
01359 double R = sqrt(x*x + y*y + z*z);
01360 HepVector p4(4,0);
01361 p4[0] = p0*x/R;
01362 p4[1] = p0*y/R;
01363 p4[2] = p0*z/R;
01364 p4[3] = p0;
01365 return p4;
01366 break;
01367 }
01368 case 7 : {
01369 double mass = m_p0.sub(pa+3, pa+3)[0];
01370 HepVector p4(4,0);
01371 p4[0] = m_p0.sub(pa,pa)[0];
01372 p4[1] = m_p0.sub(pa+1, pa+1)[0];
01373 p4[2] = m_p0.sub(pa+2, pa+2)[0];
01374 p4[3] = sqrt(p4[0]*p4[0] + p4[1]*p4[1] + p4[2]*p4[2] + mass * mass);
01375 return p4;
01376 break;
01377 }
01378 }
01379 }
01380
01381 HepSymMatrix KalmanKinematicFit::getCOrigin(int n) const {
01382 int pa = mappositionA()[n] + 1;
01383 int pb = mappositionB()[n] + 1;
01384 int ptype = mapkinematic()[n];
01385 switch(ptype) {
01386 case 0 : {
01387 return m_C0.sub(pa, pa+NTRKPAR-1);
01388 break;
01389 }
01390 case 1 : {
01391 return m_C0.sub(pa, pa+NTRKPAR-1);
01392 break;
01393 }
01394 case 3 : {
01395 return m_C0.sub(pa, pa);
01396 break;
01397 }
01398 case 4 : {
01399 return m_C0.sub(pa, pa+1);
01400 break;
01401 }
01402 case 5 : {
01403 return m_C0.sub(pa, pa+2);
01404 break;
01405 }
01406 case 7 : {
01407 return m_C0.sub(pa, pa+NTRKPAR-1);
01408 break;
01409 }
01410
01411 }
01412 }
01413
01414
01415 HepSymMatrix KalmanKinematicFit::getCInfit(int n) const {
01416 int pa = mappositionA()[n] + 1;
01417 int pb = mappositionB()[n] + 1;
01418 int ptype = mapkinematic()[n];
01419 switch(ptype) {
01420 case 0 : {
01421 return m_C.sub(pa, pa+NTRKPAR-1);
01422 break;
01423 }
01424 case 1 : {
01425 return m_C.sub(pa, pa+NTRKPAR-1);
01426 break;
01427 }
01428 case 3 : {
01429 return m_C.sub(pa, pa);
01430 break;
01431 }
01432 case 4 : {
01433 return m_C.sub(pa, pa+1);
01434 break;
01435 }
01436 case 5 : {
01437 return m_C.sub(pa, pa+2);
01438 break;
01439 }
01440 case 7 : {
01441 return m_C.sub(pa, pa+NTRKPAR-1);
01442 break;
01443 }
01444 }
01445 }
01446
01447
01448
01449
01450 void KalmanKinematicFit::updateConstraints(KinematicConstraints k ) {
01451 KinematicConstraints kc = k;
01452
01453
01454 int type = kc.Type();
01455 switch(type) {
01456 case Resonance: {
01457
01458
01459
01460 double mres = kc.mres();
01461 HepLorentzVector pmis;
01462 for(unsigned int j = 0; j< (kc.Ltrk()).size(); j++){
01463 int n = (kc.Ltrk())[j];
01464 pmis = pmis + p4Infit(n);
01465 }
01466 for(unsigned int j = 0; j< (kc.Ltrk()).size(); j++){
01467 int n = (kc.Ltrk())[j];
01468 double lambda = p4Infit(n).pz()/p4Infit(n).perp();
01469 double a1 = 1 + lambda*lambda;
01470 int pa = mappositionA()[n] + 1;
01471 int pb = mappositionB()[n] + 1;
01472 int ptype = mapkinematic()[n];
01473 switch(ptype) {
01474 case 0 :{
01475 HepMatrix ta(1, NTRKPAR, 0);
01476 ta[0][0] = -2 * pmis.px() + 2 * pmis.e() * p4Infit(n).px() / p4Infit(n).e();
01477 ta[0][1] = -2 * pmis.py() + 2 * pmis.e() * p4Infit(n).py() / p4Infit(n).e();
01478 ta[0][2] = -2 * pmis.pz() + 2 * pmis.e() * p4Infit(n).pz() / p4Infit(n).e();
01479 ta[0][3] = 2 * pmis.e() * p4Infit(n).m() / p4Infit(n).e();
01480 setA(m_nc,pa,ta);
01481 setAT(pa, m_nc, ta.T());
01482 break;
01483 }
01484 case 1 : {
01485 HepMatrix ta(1, NTRKPAR, 0);
01486 double a1 = lambda * lambda + 1;
01487 ta[0][0] = 2 * pmis.px() * p4Infit(n).py() - 2 * pmis.py() * p4Infit(n).px();
01488 ta[0][1] = 2 * pmis.px() * p4Infit(n).px() * lambda/sqrt(a1) + 2 * pmis.py() * (p4Infit(n)).py() * lambda/sqrt(a1) - 2 * pmis.pz() * (p4Infit(n)).pz() /(sqrt(a1) * lambda);
01489 ta[0][2] = 2 * pmis.px() * (p4Infit(n)).px() * p4Infit(n).m() /((p4Infit(n)).rho() * p4Infit(n).rho()) + 2 * pmis.py() * (p4Infit(n)).py() * p4Infit(n).m()/((p4Infit(n)).rho() * p4Infit(n).rho())+ 2 * pmis.pz() * (p4Infit(n)).pz() * p4Infit(n).m()/((p4Infit(n)).rho() * p4Infit(n).rho());
01490 ta[0][3] = 2 * pmis.e() - 2 * pmis.px() * (p4Infit(n)).px() * p4Infit(n).e() /((p4Infit(n)).rho() * p4Infit(n).rho())- 2 * pmis.py() * (p4Infit(n)).py() * p4Infit(n).e()/((p4Infit(n)).rho() * p4Infit(n).rho())- 2 * pmis.pz() * (p4Infit(n)).pz() * p4Infit(n).e()/((p4Infit(n)).rho() * p4Infit(n).rho());
01491 setA(m_nc,pa,ta);
01492 setAT(pa, m_nc, ta.T());
01493 break;
01494 }
01495 case 2 : {
01496 HepMatrix tb(1, 4, 0);
01497 tb[0][0] = -2 * pmis.px();
01498 tb[0][1] = -2 * pmis.py();
01499 tb[0][2] = -2 * pmis.pz();
01500 tb[0][3] = 2 * pmis.e();
01501 setB(m_nc,pb,tb);
01502 setBT(pb,m_nc,tb.T());
01503 break;
01504 }
01505 case 3 : {
01506 HepMatrix ta(1, 1, 0);
01507 double a1 = lambda * lambda + 1;
01508 ta[0][0] = 2 * pmis.px() * (p4Infit(n)).px() * p4Infit(n).m() /((p4Infit(n)).rho() * p4Infit(n).rho()) + 2 * pmis.py() * (p4Infit(n)).py() * p4Infit(n).m()/((p4Infit(n)).rho() * p4Infit(n).rho())+ 2 * pmis.pz() * (p4Infit(n)).pz() * p4Infit(n).m()/((p4Infit(n)).rho() * p4Infit(n).rho());
01509 setA(m_nc,pa,ta);
01510 setAT(pa, m_nc, ta.T());
01511 HepMatrix tb(1, 3, 0);
01512 tb[0][0] = 2 * pmis.px() * p4Infit(n).py() - 2 * pmis.py() * p4Infit(n).px();
01513 tb[0][1] = 2 * pmis.px() * p4Infit(n).px() * lambda/sqrt(a1) + 2 * pmis.py() * (p4Infit(n)).py() * lambda/sqrt(a1) - 2 * pmis.pz() * (p4Infit(n)).pz() /(sqrt(a1) * lambda);
01514 tb[0][2] = 2 * pmis.e() - 2 * pmis.px() * (p4Infit(n)).px() * p4Infit(n).e() /((p4Infit(n)).rho() * p4Infit(n).rho())- 2 * pmis.py() * (p4Infit(n)).py() * p4Infit(n).e()/((p4Infit(n)).rho() * p4Infit(n).rho())- 2 * pmis.pz() * (p4Infit(n)).pz() * p4Infit(n).e()/((p4Infit(n)).rho() * p4Infit(n).rho());
01515 setB(m_nc,pb,tb);
01516 setBT(pb,m_nc,tb.T());
01517 break;
01518 }
01519 case 4 : {
01520 HepMatrix ta(1, 2, 0);
01521 double a1 = lambda * lambda + 1;
01522 ta[0][0] = 2 * pmis.px() * p4Infit(n).py() - 2 * pmis.py() * p4Infit(n).px();
01523 ta[0][1] = 2 * pmis.px() * p4Infit(n).px() * lambda/sqrt(a1) + 2 * pmis.py() * (p4Infit(n)).py() * lambda/sqrt(a1) - 2 * pmis.pz() * (p4Infit(n)).pz() /(sqrt(a1) * lambda);
01524 setA(m_nc,pa,ta);
01525 setAT(pa, m_nc, ta.T());
01526
01527 HepMatrix tb(1, 2, 0);
01528 tb[0][0] = 2 * pmis.px() * (p4Infit(n)).px() * p4Infit(n).m() /((p4Infit(n)).rho() * p4Infit(n).rho()) + 2 * pmis.py() * (p4Infit(n)).py() * p4Infit(n).m()/((p4Infit(n)).rho() * p4Infit(n).rho())+ 2 * pmis.pz() * (p4Infit(n)).pz() * p4Infit(n).m()/((p4Infit(n)).rho() * p4Infit(n).rho());
01529 tb[0][1] = 2 * pmis.e() - 2 * pmis.px() * (p4Infit(n)).px() * p4Infit(n).e() /((p4Infit(n)).rho() * p4Infit(n).rho())- 2 * pmis.py() * (p4Infit(n)).py() * p4Infit(n).e()/((p4Infit(n)).rho() * p4Infit(n).rho())- 2 * pmis.pz() * (p4Infit(n)).pz() * p4Infit(n).e()/((p4Infit(n)).rho() * p4Infit(n).rho());
01530 setB(m_nc,pb,tb);
01531 setBT(pb, m_nc, tb.T());
01532 break;
01533 }
01534 case 5 : {
01535 HepMatrix ta(1, 3, 0);
01536 ta[0][0] = 2 * pmis.px() * p4Infit(n).py() - 2 * pmis.py() * p4Infit(n).px();
01537 ta[0][1] = 2 * pmis.px() * p4Infit(n).px() * lambda/sqrt(a1)+ 2 * pmis.py() * (p4Infit(n)).py() * lambda/sqrt(a1) - 2 * pmis.pz() * (p4Infit(n)).pz() /(sqrt(a1) * lambda);
01538 ta[0][2] = 2 * pmis.e() * (p4Infit(n)).m()/(p4Infit(n)).e();
01539 setA(m_nc,pa,ta);
01540 setAT(pa, m_nc, ta.T());
01541 HepMatrix tb(1, 1, 0);
01542 tb[0][0] = 2 * pmis.e() * (p4Infit(n)).rho()/(p4Infit(n)).e() - 2 * pmis.px() * (p4Infit(n)).px()/(p4Infit(n)).rho() - 2 * pmis.py() * (p4Infit(n)).py()/(p4Infit(n)).rho() - 2 * pmis.pz() * (p4Infit(n)).pz()/(p4Infit(n)).rho();
01543 setB(m_nc,pb,tb);
01544 setBT(pb, m_nc, tb.T());
01545 break;
01546 }
01547 case 7 :{
01548 HepMatrix ta(1, NTRKPAR, 0);
01549 ta[0][0] = -2 * pmis.px() + 2 * pmis.e() * p4Infit(n).px() / p4Infit(n).e();
01550 ta[0][1] = -2 * pmis.py() + 2 * pmis.e() * p4Infit(n).py() / p4Infit(n).e();
01551 ta[0][2] = -2 * pmis.pz() + 2 * pmis.e() * p4Infit(n).pz() / p4Infit(n).e();
01552 ta[0][3] = 2 * pmis.e() * p4Infit(n).m() / p4Infit(n).e();
01553 setA(m_nc,pa,ta);
01554 setAT(pa, m_nc, ta.T());
01555 break;
01556 }
01557 }
01558 }
01559
01560 HepVector dc(1, 0);
01561 dc[0] = pmis.m2() - mres * mres;
01562 m_G[m_nc] = dc[0];
01563 m_nc+=1;
01564
01565 break;
01566 }
01567 case TotalEnergy: {
01568
01569
01570
01571 double etot = kc.etot();
01572 HepLorentzVector pmis;
01573 for(unsigned int j = 0; j < (kc.Ltrk()).size(); j++){
01574 int n = (kc.Ltrk())[j];
01575 pmis = pmis + p4Infit(n);
01576 }
01577
01578 for(unsigned int j = 0; j < (kc.Ltrk()).size(); j++) {
01579 int n = (kc.Ltrk())[j];
01580 int pa = mappositionA()[n] + 1;
01581 int pb = mappositionB()[n] + 1;
01582 int ptype = mapkinematic()[n];
01583 switch(ptype) {
01584 case 0 :{
01585 HepMatrix ta(1, NTRKPAR, 0);
01586 ta[0][0] = p4Infit(n).px()/p4Infit(n).e();
01587 ta[0][1] = p4Infit(n).py()/p4Infit(n).e();
01588 ta[0][2] = p4Infit(n).pz()/p4Infit(n).e();
01589 ta[0][3] = p4Infit(n).m()/p4Infit(n).e();
01590 setA(m_nc,pa,ta);
01591 setAT(pa, m_nc, ta.T());
01592 break;
01593 }
01594 case 1: {
01595 HepMatrix ta(1, NTRKPAR, 0);
01596 ta[0][3] = 1.0;
01597 setA(m_nc,pa,ta);
01598 setAT(pa, m_nc, ta.T());
01599 break;
01600 }
01601 case 2: {
01602 HepMatrix tb(1, 4, 0);
01603 tb[0][3] = 1.0;
01604 setA(m_nc,pb,tb);
01605 setAT(pb, m_nc, tb.T());
01606 break;
01607 }
01608 case 3: {
01609 HepMatrix ta(1, 1, 0);
01610 ta[0][0] = p4Infit(n).m()/p4Infit(n).e();
01611 setA(m_nc,pa,ta);
01612 setAT(pa, m_nc, ta.T());
01613
01614 HepMatrix tb(1, 3, 0);
01615 setB(m_nc,pb,tb);
01616 setBT(pb, m_nc, tb.T());
01617 break;
01618 }
01619 case 4: {
01620 HepMatrix ta(1, 2, 0);
01621 setA(m_nc,pa,ta);
01622 setAT(pa, m_nc, ta.T());
01623
01624 HepMatrix tb(1, 2, 0);
01625 tb[0][0] = 0.0;
01626 tb[0][1] = 1.0;
01627
01628
01629 setB(m_nc,pb,tb);
01630 setBT(pb, m_nc, tb.T());
01631 break;
01632 }
01633 case 5: {
01634 HepMatrix ta(1, 3, 0);
01635 ta[0][2] = p4Infit(n).m()/p4Infit(n).e();
01636 setA(m_nc,pa,ta);
01637 setAT(pa, m_nc, ta.T());
01638
01639 HepMatrix tb(1, 1, 0);
01640 tb[0][0] = p4Infit(n).rho()/p4Infit(n).e();
01641 setB(m_nc,pb,tb);
01642 setBT(pb, m_nc, tb.T());
01643 break;
01644 }
01645 case 7 :{
01646 HepMatrix ta(1, NTRKPAR, 0);
01647 ta[0][0] = p4Infit(n).px()/p4Infit(n).e();
01648 ta[0][1] = p4Infit(n).py()/p4Infit(n).e();
01649 ta[0][2] = p4Infit(n).pz()/p4Infit(n).e();
01650 ta[0][3] = p4Infit(n).m()/p4Infit(n).e();
01651 setA(m_nc,pa,ta);
01652 setAT(pa, m_nc, ta.T());
01653 break;
01654 }
01655 }
01656
01657 }
01658
01659 HepVector dc(1, 0);
01660 dc[0] = pmis.e() - etot;
01661 m_G[m_nc] = dc[0];
01662 m_nc+=1;
01663 break;
01664 }
01665 case TotalMomentum: {
01666
01667
01668
01669 double ptot = kc.ptot();
01670 HepLorentzVector pmis;
01671 for(unsigned int j = 0; j< (kc.Ltrk()).size(); j++){
01672 int n = (kc.Ltrk())[j];
01673 pmis = pmis + p4Infit(n);
01674 }
01675
01676 for(unsigned int j = 0; j < (kc.Ltrk()).size(); j++) {
01677 int n = (kc.Ltrk())[j];
01678 int pa = mappositionA()[n] + 1;
01679 int pb = mappositionB()[n] + 1;
01680 int ptype = mapkinematic()[n];
01681 double lambda = p4Infit(n).pz()/p4Infit(n).perp();
01682 switch(ptype) {
01683 case 0 : {
01684 HepMatrix ta(1, NTRKPAR, 0);
01685 ta[0][0] = pmis.px()/pmis.rho();
01686 ta[0][1] = pmis.py()/pmis.rho();
01687 ta[0][2] = pmis.pz()/pmis.rho();
01688 setA(m_nc,pa,ta);
01689 setAT(pa, m_nc, ta.T());
01690 break;
01691 }
01692 case 1 : {
01693 HepMatrix ta(1, NTRKPAR, 0);
01694 ta[0][0] = - (pmis.px()/pmis.rho()) * p4Infit(n).py() + (pmis.px()/pmis.rho())*p4Infit(n).px();
01695 ta[0][1] = - (pmis.px()/pmis.rho()) * p4Infit(n).px() * (lambda/(1 + lambda*lambda)) - (pmis.py()/pmis.rho()) * p4Infit(n).py() * (lambda/(1 + lambda*lambda)) + (pmis.pz()/pmis.rho()) * p4Infit(n).pz() * (1/(lambda * (1 + lambda*lambda)));
01696 ta[0][2] = -((pmis.px()/pmis.rho()) * p4Infit(n).m()/(p4Infit(n).rho()*p4Infit(n).rho())) * (p4Infit(n).px() + p4Infit(n).py() +p4Infit(n).pz());
01697 ta[0][3] = ((pmis.px()/pmis.rho()) * p4Infit(n).e()/(p4Infit(n).rho()*p4Infit(n).rho())) * (p4Infit(n).px() + p4Infit(n).py() +
01698 p4Infit(n).pz());
01699
01700 setA(m_nc,pa,ta);
01701 setAT(pa, m_nc, ta.T());
01702 break;
01703 }
01704 case 2 : {
01705 HepMatrix tb(1, 4, 0);
01706 tb[0][0] = pmis.px()/pmis.rho();
01707 tb[0][1] = pmis.py()/pmis.rho();
01708 tb[0][2] = pmis.pz()/pmis.rho();
01709 setB(m_nc,pb,tb);
01710 setBT(pb, m_nc, tb.T());
01711 break;
01712 }
01713 case 3 : {
01714 HepMatrix ta(1, 1, 0);
01715 setA(m_nc,pa,ta);
01716 setAT(pa, m_nc, ta.T());
01717 HepMatrix tb(1, 3, 0);
01718 tb[0][0] = pmis.px()/pmis.rho();
01719 tb[0][1] = pmis.py()/pmis.rho();
01720 tb[0][2] = pmis.pz()/pmis.rho();
01721 setB(m_nc,pb,tb);
01722 setBT(pb, m_nc, tb.T());
01723 break;
01724 }
01725 case 4 : {
01726 HepMatrix ta(1, 2, 0);
01727 ta[0][0] = - (pmis.px()/pmis.rho()) * p4Infit(n).py() + (pmis.px()/pmis.rho())*p4Infit(n).px();
01728 ta[0][1] = - (pmis.px()/pmis.rho()) * p4Infit(n).px() * (lambda/(1 + lambda*lambda)) - (pmis.py()/pmis.rho()) * p4Infit(n).py() * (lambda/(1 + lambda*lambda)) + (pmis.pz()/pmis.rho()) * p4Infit(n).pz() * (1/(lambda * (1 + lambda*lambda)));
01729 setA(m_nc,pa,ta);
01730 setAT(pa, m_nc, ta.T());
01731
01732 HepMatrix tb(1, 2, 0);
01733 tb[0][0] = -((pmis.px()/pmis.rho()) * p4Infit(n).m()/(p4Infit(n).rho()*p4Infit(n).rho())) * (p4Infit(n).px() + p4Infit(n).py() +p4Infit(n).pz());
01734 tb[0][1] = ((pmis.px()/pmis.rho()) * p4Infit(n).e()/(p4Infit(n).rho()*p4Infit(n).rho())) * (p4Infit(n).px() + p4Infit(n).py() +
01735 p4Infit(n).pz());
01736
01737 setB(m_nc,pb,tb);
01738 setBT(pb, m_nc, tb.T());
01739 break;
01740 }
01741 case 5 : {
01742 HepMatrix ta(1, 3, 0);
01743 ta[0][0] = - (pmis.px()/pmis.rho()) * p4Infit(n).py() + (pmis.px()/pmis.rho())*p4Infit(n).px();
01744 ta[0][1] = - (pmis.px()/pmis.rho()) * p4Infit(n).px() * (lambda/(1 + lambda*lambda)) - (pmis.py()/pmis.rho()) * p4Infit(n).py() * (lambda/(1 + lambda*lambda)) + (pmis.pz()/pmis.rho()) * p4Infit(n).pz() * (1/(lambda * (1 + lambda*lambda)));
01745 setA(m_nc,pa,ta);
01746 setAT(pa, m_nc, ta.T());
01747
01748 HepMatrix tb(1, 1, 0);
01749 tb[0][0] = (pmis.px()/pmis.rho()) * (p4Infit(n).px()/p4Infit(n).rho()) + (pmis.py()/pmis.rho()) * (p4Infit(n).py()/p4Infit(n).rho()) + (pmis.pz()/pmis.rho()) * (p4Infit(n).pz()/p4Infit(n).rho());
01750 setB(m_nc,pb,tb);
01751 setBT(pb, m_nc, tb.T());
01752 break;
01753 }
01754 case 7 : {
01755 HepMatrix ta(1, NTRKPAR, 0);
01756 ta[0][0] = pmis.px()/pmis.rho();
01757 ta[0][1] = pmis.py()/pmis.rho();
01758 ta[0][2] = pmis.pz()/pmis.rho();
01759 setA(m_nc,pa,ta);
01760 setAT(pa, m_nc, ta.T());
01761 break;
01762 }
01763 }
01764 }
01765
01766
01767 HepVector dc(1, 0);
01768 dc[0] = pmis.rho() - ptot;
01769 m_G[m_nc] = dc[0];
01770 m_nc+=1;
01771 break;
01772 }
01773
01774
01775 case ThreeMomentum: {
01776
01777
01778
01779
01780
01781 Hep3Vector p3 = kc.p3();
01782 HepLorentzVector pmis;
01783 for(unsigned int j = 0; j< (kc.Ltrk()).size(); j++){
01784 int n = (kc.Ltrk())[j];
01785 pmis = pmis + p4Infit(n);
01786 }
01787 for(unsigned int j = 0; j < (kc.Ltrk()).size(); j++) {
01788 int n = (kc.Ltrk())[j];
01789 int pa = mappositionA()[n] + 1;
01790 int pb = mappositionB()[n] + 1;
01791 int ptype = mapkinematic()[n];
01792 double lambda = p4Infit(n).pz()/p4Infit(n).perp();
01793 switch(ptype) {
01794 case 0 : {
01795 HepMatrix ta(kc.nc(), NTRKPAR, 0);
01796 ta[0][0] = 1.0;
01797 ta[1][1] = 1.0;
01798 ta[2][2] = 1.0;
01799 setA(m_nc, pa, ta);
01800 setAT(pa, m_nc, ta.T());
01801 break;
01802 }
01803 case 1 : {
01804 HepMatrix ta(kc.nc(), NTRKPAR, 0);
01805 ta[0][0] = -p4Infit(n).py();
01806 ta[0][1] = -p4Infit(n).px()*(lambda/sqrt(1 + lambda*lambda));
01807 ta[0][2] = -p4Infit(n).m()*p4Infit(n).px()/(p4Infit(n).rho()*p4Infit(n).rho());
01808 ta[0][3] = p4Infit(n).e()*p4Infit(n).px()/(p4Infit(n).rho()*p4Infit(n).rho());
01809 ta[1][0] = p4Infit(n).px();
01810 ta[1][1] = -p4Infit(n).py()*(lambda/sqrt(1 + lambda*lambda));
01811 ta[1][2] = -p4Infit(n).m()*p4Infit(n).py()/(p4Infit(n).rho()*p4Infit(n).rho());
01812 ta[1][3] = p4Infit(n).e()*p4Infit(n).py()/(p4Infit(n).rho()*p4Infit(n).rho());
01813 ta[2][0] = 0;
01814 ta[2][1] = p4Infit(n).pz()*(1/(lambda * (1 + lambda*lambda)));
01815 ta[2][2] = -p4Infit(n).m()*p4Infit(n).pz()/(p4Infit(n).rho()*p4Infit(n).rho());
01816 ta[2][3] = p4Infit(n).e()*p4Infit(n).pz()/(p4Infit(n).rho()*p4Infit(n).rho());
01817
01818
01819
01820 setA(m_nc, pa, ta);
01821 setAT(pa, m_nc, ta.T());
01822 break;
01823 }
01824 case 2 : {
01825 HepMatrix tb(kc.nc(), 4, 0);
01826 tb[0][0] = 1.0;
01827 tb[1][1] = 1.0;
01828 tb[2][2] = 1.0;
01829 setB(m_nc, pb, tb);
01830 setBT(pb, m_nc, tb.T());
01831 break;
01832 }
01833 case 3 : {
01834 HepMatrix ta(kc.nc(), 1, 0);
01835 setA(m_nc, pa, ta);
01836 setAT(pa, m_nc, ta.T());
01837
01838 HepMatrix tb(kc.nc(), 3, 0);
01839 tb[0][0] = 1.0;
01840 tb[1][1] = 1.0;
01841 tb[2][2] = 1.0;
01842 setB(m_nc, pb, tb);
01843 setBT(pb, m_nc, tb.T());
01844
01845 break;
01846 }
01847 case 4 : {
01848 HepMatrix ta(kc.nc(), 2, 0);
01849 ta[0][0] = -p4Infit(n).py();
01850 ta[0][1] = -p4Infit(n).px()*(lambda/sqrt(1 + lambda*lambda));
01851 ta[1][0] = p4Infit(n).px();
01852 ta[1][1] = -p4Infit(n).py()*(lambda/sqrt(1 + lambda*lambda));
01853 ta[2][0] = 0;
01854 ta[2][1] = p4Infit(n).pz()*(1/(lambda * (1 + lambda*lambda)));
01855 setA(m_nc, pa, ta);
01856 setAT(pa, m_nc, ta.T());
01857
01858 HepMatrix tb(kc.nc(), 2, 0);
01859 tb[0][1] = -p4Infit(n).m()*p4Infit(n).px()/(p4Infit(n).rho()*p4Infit(n).rho());
01860 tb[1][1] = -p4Infit(n).m()*p4Infit(n).py()/(p4Infit(n).rho()*p4Infit(n).rho());
01861 tb[2][1] = -p4Infit(n).m()*p4Infit(n).pz()/(p4Infit(n).rho()*p4Infit(n).rho());
01862 setB(m_nc, pb, tb);
01863 setBT(pb, m_nc, tb.T());
01864 break;
01865 }
01866 case 5 : {
01867 HepMatrix ta(kc.nc(), 3, 0);
01868 ta[0][0] = -p4Infit(n).py();
01869 ta[0][1] = -p4Infit(n).px()*(lambda/sqrt(1 + lambda*lambda));
01870 ta[1][0] = p4Infit(n).px();
01871 ta[1][1] = -p4Infit(n).py()*(lambda/sqrt(1 + lambda*lambda));
01872 ta[2][0] = 0;
01873 ta[2][1] = p4Infit(n).pz()*(1/(lambda * (1 + lambda*lambda)));
01874 setA(m_nc, pa, ta);
01875 setAT(pa, m_nc, ta.T());
01876
01877 HepMatrix tb(kc.nc(), 1, 0);
01878 tb[0][0] = p4Infit(n).px()/p4Infit(n).rho();
01879 tb[1][0] = p4Infit(n).py()/p4Infit(n).rho();
01880 tb[2][0] = p4Infit(n).pz()/p4Infit(n).rho();
01881 setB(m_nc, pb, tb);
01882 setBT(pb, m_nc, tb.T());
01883 break;
01884 }
01885 case 7 : {
01886 HepMatrix ta(kc.nc(), NTRKPAR, 0);
01887 ta[0][0] = 1.0;
01888 ta[1][1] = 1.0;
01889 ta[2][2] = 1.0;
01890 setA(m_nc, pa, ta);
01891 setAT(pa, m_nc, ta.T());
01892 break;
01893 }
01894 }
01895 }
01896 HepVector dc(kc.nc(), 0);
01897 dc[0] = pmis.px() - p3.x();
01898 dc[1] = pmis.py() - p3.y();
01899 dc[2] = pmis.pz() - p3.z();
01900 for(int i = 0; i < kc.nc(); i++) {
01901 m_G[m_nc+i] = dc[i];
01902 }
01903
01904 m_nc += 3;
01905
01906 break;
01907 }
01908 case EqualMass: {
01909
01910
01911
01912
01913 int isiz = (kc.numEqual())[0];
01914 HepLorentzVector pmis1, pmis2;
01915 for(int n = 0; n < isiz; n++) {
01916 int n1 = (kc.Ltrk())[n];
01917 pmis1 = pmis1 + p4Infit(n1);
01918 }
01919
01920 int jsiz = (kc.numEqual())[1];
01921 for(int n = 0; n < jsiz; n++){
01922 int n2 = (kc.Ltrk())[n+isiz];
01923 pmis2 = pmis2 + p4Infit(n2);
01924 }
01925
01926 for(int i = 0; i < isiz; i++) {
01927 int n1 = (kc.Ltrk())[i];
01928 double lambda = p4Infit(n1).pz()/p4Infit(n1).perp();
01929 int pa = mappositionA()[n1] + 1;
01930 int pb = mappositionB()[n1] + 1;
01931 int ptype = mapkinematic()[n1];
01932 switch(ptype) {
01933 case 0: {
01934 HepMatrix ta(1, NTRKPAR, 0);
01935 ta[0][0] = -2 * pmis1.px() + 2 * pmis1.e() * p4Infit(n1).px() / p4Infit(n1).e();
01936 ta[0][1] = -2 * pmis1.py() + 2 * pmis1.e() * p4Infit(n1).py() / p4Infit(n1).e();
01937 ta[0][2] = -2 * pmis1.pz() + 2 * pmis1.e() * p4Infit(n1).pz() / p4Infit(n1).e();
01938 ta[0][3] = 2 * pmis1.e() * p4Infit(n1).m() / p4Infit(n1).e();
01939 setA(m_nc,pa,ta);
01940 setAT(pa, m_nc, ta.T());
01941 break;
01942 }
01943 case 1 : {
01944 HepMatrix ta(1, NTRKPAR, 0);
01945 double a1 = lambda * lambda + 1;
01946 ta[0][0] = 2 * pmis1.px() * p4Infit(n1).py() - 2 * pmis1.py() * p4Infit(n1).px();
01947 ta[0][1] = 2 * pmis1.px() * p4Infit(n1).px() * lambda/sqrt(a1) + 2 * pmis1.py() * (p4Infit(n1)).py() * lambda/sqrt(a1) - 2 * pmis1.pz() * (p4Infit(n1)).pz() /(sqrt(a1) * lambda);
01948 ta[0][2] = 2 * pmis1.px() * (p4Infit(n1)).px() * p4Infit(n1).m() /((p4Infit(n1)).rho() * p4Infit(n1).rho()) + 2 * pmis1.py() * (p4Infit(n1)).py() * p4Infit(n1).m()/((p4Infit(n1)).rho() * p4Infit(n1).rho())+ 2 * pmis1.pz() * (p4Infit(n1)).pz() * p4Infit(n1).m()/((p4Infit(n1)).rho() * p4Infit(n1).rho());
01949 ta[0][3] = 2 * pmis1.e() - 2 * pmis1.px() * (p4Infit(n1)).px() * p4Infit(n1).e() /((p4Infit(n1)).rho() * p4Infit(n1).rho())- 2 * pmis1.py() * (p4Infit(n1)).py() * p4Infit(n1).e()/((p4Infit(n1)).rho() * p4Infit(n1).rho())- 2 * pmis1.pz() * (p4Infit(n1)).pz() * p4Infit(n1).e()/((p4Infit(n1)).rho() * p4Infit(n1).rho());
01950 setA(m_nc,pa,ta);
01951 setAT(pa, m_nc, ta.T());
01952 break;
01953 }
01954 case 2 : {
01955 HepMatrix tb(1, 4, 0);
01956 tb[0][0] = -2 * pmis1.px();
01957 tb[0][1] = -2 * pmis1.py();
01958 tb[0][2] = -2 * pmis1.pz();
01959 tb[0][3] = 2 * pmis1.e();
01960 setB(m_nc,pb,tb);
01961 setBT(pb,m_nc,tb.T());
01962 break;
01963 }
01964 case 3 : {
01965 HepMatrix ta(1, 1, 0);
01966 ta[0][0] = 2 * pmis1.e() * (p4Infit(n1).m()/p4Infit(n1).e());
01967 setA(m_nc,pa,ta);
01968 setAT(pa, m_nc, ta.T());
01969 HepMatrix tb(1, 3, 0);
01970 tb[0][0] = -2 * pmis1.px();
01971 tb[0][1] = -2 * pmis1.py();
01972 tb[0][2] = -2 * pmis1.pz();
01973 setB(m_nc,pb,tb);
01974 setBT(pb,m_nc,tb.T());
01975 break;
01976 }
01977 case 4 : {
01978 HepMatrix ta(1, 2, 0);
01979 double a1 = lambda * lambda + 1;
01980 ta[0][0] = 2 * pmis1.px() * p4Infit(n1).py() - 2 * pmis1.py() * p4Infit(n1).px();
01981 ta[0][1] = 2 * pmis1.px() * p4Infit(n1).px() * lambda/sqrt(a1) + 2 * pmis1.py() * (p4Infit(n1)).py() * lambda/sqrt(a1) - 2 * pmis1.pz() * (p4Infit(n1)).pz() /(sqrt(a1) * lambda);
01982 setA(m_nc,pa,ta);
01983 setAT(pa, m_nc, ta.T());
01984
01985 HepMatrix tb(1, 2, 0);
01986 tb[0][0] = 2 * pmis1.px() * (p4Infit(n1)).px() * p4Infit(n1).m() /((p4Infit(n1)).rho() * p4Infit(n1).rho()) + 2 * pmis1.py() * (p4Infit(n1)).py() * p4Infit(n1).m()/((p4Infit(n1)).rho() * p4Infit(n1).rho())+ 2 * pmis1.pz() * (p4Infit(n1)).pz() * p4Infit(n1).m()/((p4Infit(n1)).rho() * p4Infit(n1).rho());
01987 tb[0][1] = 2 * pmis1.e() - 2 * pmis1.px() * (p4Infit(n1)).px() * p4Infit(n1).e() /((p4Infit(n1)).rho() * p4Infit(n1).rho())- 2 * pmis1.py() * (p4Infit(n1)).py() * p4Infit(n1).e()/((p4Infit(n1)).rho() * p4Infit(n1).rho())- 2 * pmis1.pz() * (p4Infit(n1)).pz() * p4Infit(n1).e()/((p4Infit(n1)).rho() * p4Infit(n1).rho());
01988 setB(m_nc,pb,tb);
01989 setBT(pb, m_nc, tb.T());
01990 break;
01991 }
01992 case 5 : {
01993 HepMatrix ta(1, 3, 0);
01994 double a1 = lambda * lambda + 1;
01995 ta[0][0] = 2 * pmis1.px() * p4Infit(n1).py() - 2 * pmis1.py() * p4Infit(n1).px();
01996 ta[0][1] = 2 * pmis1.px() * p4Infit(n1).px() * lambda/sqrt(a1)+ 2 * pmis1.py() * (p4Infit(n1)).py() * lambda/sqrt(a1) - 2 * pmis1.pz() * (p4Infit(n1)).pz() /(sqrt(a1) * lambda);
01997 ta[0][2] = 2 * pmis1.e() * (p4Infit(n1)).m()/(p4Infit(n1)).e();
01998 setA(m_nc,pa,ta);
01999 setAT(pa, m_nc, ta.T());
02000 HepMatrix tb(1, 1, 0);
02001 tb[0][0] = 2 * pmis1.e() * (p4Infit(n1)).rho()/(p4Infit(n1)).e() - 2 * pmis1.px() * (p4Infit(n1)).px()/(p4Infit(n1)).rho() - 2 * pmis1.py() * (p4Infit(n1)).py()/(p4Infit(n1)).rho() - 2 * pmis1.pz() * (p4Infit(n1)).pz()/(p4Infit(n1)).rho();
02002 setB(m_nc,pb,tb);
02003 setBT(pb, m_nc, tb.T());
02004 break;
02005 }
02006 case 7: {
02007 HepMatrix ta(1, NTRKPAR, 0);
02008 ta[0][0] = -2 * pmis1.px() + 2 * pmis1.e() * p4Infit(n1).px() / p4Infit(n1).e();
02009 ta[0][1] = -2 * pmis1.py() + 2 * pmis1.e() * p4Infit(n1).py() / p4Infit(n1).e();
02010 ta[0][2] = -2 * pmis1.pz() + 2 * pmis1.e() * p4Infit(n1).pz() / p4Infit(n1).e();
02011 ta[0][3] = 2 * pmis1.e() * p4Infit(n1).m() / p4Infit(n1).e();
02012 setA(m_nc,pa,ta);
02013 setAT(pa, m_nc, ta.T());
02014 break;
02015 }
02016 }
02017 }
02018
02019 for(int i = isiz; i < isiz+jsiz; i++) {
02020 int n2 = (kc.Ltrk())[i];
02021 double lambda = p4Infit(n2).pz()/p4Infit(n2).perp();
02022 int pa = mappositionA()[n2] + 1;
02023 int pb = mappositionB()[n2] + 1;
02024 int ptype = mapkinematic()[n2];
02025 switch(ptype) {
02026 case 0 : {
02027 HepMatrix ta(1, NTRKPAR, 0);
02028 ta[0][0] = -2 * pmis2.px() + 2 * pmis2.e() * p4Infit(n2).px() / p4Infit(n2).e();
02029 ta[0][1] = -2 * pmis2.py() + 2 * pmis2.e() * p4Infit(n2).py() / p4Infit(n2).e();
02030 ta[0][2] = -2 * pmis2.pz() + 2 * pmis2.e() * p4Infit(n2).pz() / p4Infit(n2).e();
02031 ta[0][3] = 2 * pmis2.e() * p4Infit(n2).m() / p4Infit(n2).e();
02032 setA(m_nc,pa,-ta);
02033 setAT(pa, m_nc, -ta.T());
02034 break;
02035 }
02036 case 1 : {
02037 HepMatrix ta(1, NTRKPAR, 0);
02038 double a1 = lambda * lambda + 1;
02039 ta[0][0] = 2 * pmis2.px() * p4Infit(n2).py() - 2 * pmis2.py() * p4Infit(n2).px();
02040 ta[0][1] = 2 * pmis2.px() * p4Infit(n2).px() * lambda/sqrt(a1) + 2 * pmis2.py() * (p4Infit(n2)).py() * lambda/sqrt(a1) - 2 * pmis2.pz() * (p4Infit(n2)).pz() /(sqrt(a1) * lambda);
02041 ta[0][2] = 2 * pmis2.px() * (p4Infit(n2)).px() * p4Infit(n2).m() /((p4Infit(n2)).rho() * p4Infit(n2).rho()) + 2 * pmis2.py() * (p4Infit(n2)).py() * p4Infit(n2).m()/((p4Infit(n2)).rho() * p4Infit(n2).rho())+ 2 * pmis2.pz() * (p4Infit(n2)).pz() * p4Infit(n2).m()/((p4Infit(n2)).rho() * p4Infit(n2).rho());
02042 ta[0][3] = 2 * pmis2.e() - 2 * pmis2.px() * (p4Infit(n2)).px() * p4Infit(n2).e() /((p4Infit(n2)).rho() * p4Infit(n2).rho())- 2 * pmis2.py() * (p4Infit(n2)).py() * p4Infit(n2).e()/((p4Infit(n2)).rho() * p4Infit(n2).rho())- 2 * pmis2.pz() * (p4Infit(n2)).pz() * p4Infit(n2).e()/((p4Infit(n2)).rho() * p4Infit(n2).rho());
02043 setA(m_nc,pa,-ta);
02044 setAT(pa, m_nc, -ta.T());
02045 break;
02046 }
02047 case 2 : {
02048 HepMatrix tb(1, 4, 0);
02049 tb[0][0] = -2 * pmis2.px();
02050 tb[0][1] = -2 * pmis2.py();
02051 tb[0][2] = -2 * pmis2.pz();
02052 tb[0][3] = 2 * pmis2.e();
02053 setB(m_nc,pb,-tb);
02054 setBT(pb,m_nc,-tb.T());
02055 break;
02056 }
02057 case 3 : {
02058 HepMatrix ta(1, 1, 0);
02059 ta[0][0] = 2 * pmis2.e() * (p4Infit(n2).m()/p4Infit(n2).e());
02060 setA(m_nc,pa,-ta);
02061 setAT(pa, m_nc, -ta.T());
02062 HepMatrix tb(1, 3, 0);
02063 tb[0][0] = -2 * pmis2.px();
02064 tb[0][1] = -2 * pmis2.py();
02065 tb[0][2] = -2 * pmis2.pz();
02066 setB(m_nc,pb,-tb);
02067 setBT(pb,m_nc,-tb.T());
02068 break;
02069 }
02070 case 4 : {
02071 HepMatrix ta(1, 2, 0);
02072 double a1 = lambda * lambda + 1;
02073 ta[0][0] = 2 * pmis2.px() * p4Infit(n2).py() - 2 * pmis2.py() * p4Infit(n2).px();
02074 ta[0][1] = 2 * pmis2.px() * p4Infit(n2).px() * lambda/sqrt(a1) + 2 * pmis2.py() * (p4Infit(n2)).py() * lambda/sqrt(a1) - 2 * pmis2.pz() * (p4Infit(n2)).pz() /(sqrt(a1) * lambda);
02075 setA(m_nc,pa,-ta);
02076 setAT(pa, m_nc, -ta.T());
02077
02078 HepMatrix tb(1, 2, 0);
02079 tb[0][0] = 2 * pmis2.px() * (p4Infit(n2)).px() * p4Infit(n2).m() /((p4Infit(n2)).rho() * p4Infit(n2).rho()) + 2 * pmis2.py() * (p4Infit(n2)).py() * p4Infit(n2).m()/((p4Infit(n2)).rho() * p4Infit(n2).rho())+ 2 * pmis2.pz() * (p4Infit(n2)).pz() * p4Infit(n2).m()/((p4Infit(n2)).rho() * p4Infit(n2).rho());
02080 tb[0][1] = 2 * pmis2.e() - 2 * pmis2.px() * (p4Infit(n2)).px() * p4Infit(n2).e() /((p4Infit(n2)).rho() * p4Infit(n2).rho())- 2 * pmis2.py() * (p4Infit(n2)).py() * p4Infit(n2).e()/((p4Infit(n2)).rho() * p4Infit(n2).rho())- 2 * pmis2.pz() * (p4Infit(n2)).pz() * p4Infit(n2).e()/((p4Infit(n2)).rho() * p4Infit(n2).rho());
02081 setB(m_nc,pb,-tb);
02082 setBT(pb, m_nc, -tb.T());
02083 break;
02084 }
02085 case 5 : {
02086 HepMatrix ta(1, 3, 0);
02087 double a1 = lambda * lambda + 1;
02088 ta[0][0] = 2 * pmis2.px() * p4Infit(n2).py() - 2 * pmis2.py() * p4Infit(n2).px();
02089 ta[0][1] = 2 * pmis2.px() * p4Infit(n2).px() * lambda/sqrt(a1) + 2 * pmis2.py() * (p4Infit(n2)).py() * lambda/sqrt(a1) - 2 * pmis2.pz() * (p4Infit(n2)).pz() /(sqrt(a1) * lambda);
02090 ta[0][2] = 2 * pmis2.e() * (p4Infit(n2)).m()/(p4Infit(n2)).e();
02091 setA(m_nc,pa,-ta);
02092 setAT(pa, m_nc, -ta.T());
02093 HepMatrix tb(1, 1, 0);
02094 tb[0][0] = 2 * pmis2.e() * (p4Infit(n2)).rho()/(p4Infit(n2)).e() - 2 * pmis2.px() * (p4Infit(n2)).px()/(p4Infit(n2)).rho() - 2 * pmis2.py() * (p4Infit(n2)).py()/(p4Infit(n2)).rho() - 2 * pmis2.pz() * (p4Infit(n2)).pz()/(p4Infit(n2)).rho();
02095 setB(m_nc,pb,-tb);
02096 setBT(pb, m_nc, -tb.T());
02097 break;
02098 }
02099 case 7 : {
02100 HepMatrix ta(1, NTRKPAR, 0);
02101 ta[0][0] = -2 * pmis2.px() + 2 * pmis2.e() * p4Infit(n2).px() / p4Infit(n2).e();
02102 ta[0][1] = -2 * pmis2.py() + 2 * pmis2.e() * p4Infit(n2).py() / p4Infit(n2).e();
02103 ta[0][2] = -2 * pmis2.pz() + 2 * pmis2.e() * p4Infit(n2).pz() / p4Infit(n2).e();
02104 ta[0][3] = 2 * pmis2.e() * p4Infit(n2).m() / p4Infit(n2).e();
02105 setA(m_nc,pa,-ta);
02106 setAT(pa, m_nc, -ta.T());
02107 break;
02108 }
02109 }
02110 }
02111
02112
02113
02114
02115 HepVector dc(1, 0);
02116 dc[0] = pmis1.m2() - pmis2.m2();
02117 m_G[m_nc] = dc[0];
02118
02119 m_nc+=1;
02120
02121 break;
02122 }
02123
02124
02125 case FourMomentum:
02126 default: {
02127
02128
02129
02130
02131
02132
02133 HepLorentzVector p4 = kc.p4();
02134 HepLorentzVector pmis;
02135 for(unsigned int j = 0; j< (kc.Ltrk()).size(); j++){
02136 int n = (kc.Ltrk())[j];
02137 pmis = pmis + p4Infit(n);
02138 }
02139 for(unsigned int j = 0; j < (kc.Ltrk()).size(); j++) {
02140 int n = (kc.Ltrk())[j];
02141 double lambda = p4Infit(n).pz()/p4Infit(n).perp();
02142 int pa = mappositionA()[n] + 1;
02143 int pb = mappositionB()[n] + 1;
02144 int ptype = mapkinematic()[n];
02145 switch(ptype) {
02146 case 0 : {
02147 HepMatrix ta(kc.nc(), NTRKPAR, 0);
02148 ta[0][0] = 1.0;
02149 ta[1][1] = 1.0;
02150 ta[2][2] = 1.0;
02151 ta[3][0] = p4Infit(n).px() / p4Infit(n).e();
02152 ta[3][1] = p4Infit(n).py() / p4Infit(n).e();
02153 ta[3][2] = p4Infit(n).pz() / p4Infit(n).e();
02154 ta[3][3] = p4Infit(n).m() / p4Infit(n).e();
02155 setA(m_nc, pa, ta);
02156 setAT(pa, m_nc, ta.T());
02157 break;
02158 }
02159 case 1 : {
02160 HepMatrix ta(kc.nc(), NTRKPAR, 0);
02161 ta[0][0] = -p4Infit(n).py();
02162 ta[0][1] = -p4Infit(n).px()*(lambda/(1 + lambda*lambda));
02163 ta[0][2] = -p4Infit(n).m()*p4Infit(n).px()/(p4Infit(n).rho()*p4Infit(n).rho());
02164 ta[0][3] = p4Infit(n).e()*p4Infit(n).px()/(p4Infit(n).rho()*p4Infit(n).rho());
02165 ta[1][0] = p4Infit(n).px();
02166 ta[1][1] = -p4Infit(n).py()*(lambda/(1 + lambda*lambda));
02167 ta[1][2] = -p4Infit(n).m()*p4Infit(n).py()/(p4Infit(n).rho()*p4Infit(n).rho());
02168 ta[1][3] = p4Infit(n).e()*p4Infit(n).py()/(p4Infit(n).rho()*p4Infit(n).rho());
02169 ta[2][0] = 0;
02170 ta[2][1] = p4Infit(n).pz()*(1/(lambda * (1 + lambda*lambda)));
02171 ta[2][2] = -p4Infit(n).m()*p4Infit(n).pz()/(p4Infit(n).rho()*p4Infit(n).rho());
02172 ta[2][3] = p4Infit(n).e()*p4Infit(n).pz()/(p4Infit(n).rho()*p4Infit(n).rho());
02173 ta[3][0] = 0;
02174 ta[3][1] = 0;
02175 ta[3][2] = 0;
02176 ta[3][3] = 1.0;
02177 setA(m_nc, pa, ta);
02178 setAT(pa, m_nc, ta.T());
02179 break;
02180 }
02181 case 2 : {
02182 HepMatrix tb(kc.nc(), 4, 0);
02183 tb[0][0] = 1.0;
02184 tb[1][1] = 1.0;
02185 tb[2][2] = 1.0;
02186 tb[3][3] = 1.0;
02187 setB(m_nc, pb, tb);
02188 setBT(pb, m_nc, tb.T());
02189 break;
02190 }
02191 case 3 : {
02192 HepMatrix ta(kc.nc(), 1, 0);
02193 ta[0][0] = -p4Infit(n).m()*p4Infit(n).px()/(p4Infit(n).rho()*p4Infit(n).rho());
02194 ta[1][0] = -p4Infit(n).m()*p4Infit(n).py()/(p4Infit(n).rho()*p4Infit(n).rho());
02195 ta[2][0] = -p4Infit(n).m()*p4Infit(n).pz()/(p4Infit(n).rho()*p4Infit(n).rho());
02196 ta[3][0] = 0;
02197 setA(m_nc, pa, ta);
02198 setAT(pa, m_nc, ta.T());
02199
02200 HepMatrix tb(kc.nc(), 3, 0);
02201 tb[0][0] = -p4Infit(n).py();
02202 tb[0][1] = -p4Infit(n).px()*(lambda/(1 + lambda*lambda));
02203 tb[0][2] = p4Infit(n).e()*p4Infit(n).px()/(p4Infit(n).rho()*p4Infit(n).rho());
02204 tb[1][0] = p4Infit(n).px();
02205 tb[1][1] = -p4Infit(n).py()*(lambda/(1 + lambda*lambda));
02206 tb[1][2] = p4Infit(n).e()*p4Infit(n).py()/(p4Infit(n).rho()*p4Infit(n).rho());
02207 tb[2][0] = 0;
02208 tb[2][1] = p4Infit(n).pz()*(1/(lambda * (1 + lambda*lambda)));
02209 tb[2][2] = p4Infit(n).e()*p4Infit(n).pz()/(p4Infit(n).rho()*p4Infit(n).rho());
02210 tb[3][0] = 0;
02211 tb[3][1] = 0;
02212 tb[3][2] = 1.0;
02213
02214 setB(m_nc, pb, tb);
02215 setBT(pb, m_nc, tb.T());
02216
02217 break;
02218 }
02219 case 4 : {
02220 HepMatrix ta(kc.nc(), 2, 0);
02221 ta[0][0] = -p4Infit(n).py();
02222 ta[0][1] = -p4Infit(n).px()*(lambda/sqrt(1 + lambda*lambda));
02223 ta[1][0] = p4Infit(n).px();
02224 ta[1][1] = -p4Infit(n).py()*(lambda/sqrt(1 + lambda*lambda));
02225 ta[2][0] = 0;
02226 ta[2][1] = p4Infit(n).pz()*(1/(lambda * (1 + lambda*lambda)));
02227
02228 setA(m_nc, pa, ta);
02229 setAT(pa, m_nc, ta.T());
02230
02231 HepMatrix tb(kc.nc(), 2, 0);
02232
02233
02234
02235
02236
02237 tb[0][0] = -p4Infit(n).m()*p4Infit(n).px()/(p4Infit(n).rho()*p4Infit(n).rho());
02238 tb[1][0] = -p4Infit(n).m()*p4Infit(n).py()/(p4Infit(n).rho()*p4Infit(n).rho());
02239 tb[2][0] = -p4Infit(n).m()*p4Infit(n).pz()/(p4Infit(n).rho()*p4Infit(n).rho());
02240 tb[0][1] = p4Infit(n).e()*p4Infit(n).px()/(p4Infit(n).rho()*p4Infit(n).rho());
02241 tb[1][1] = p4Infit(n).e()*p4Infit(n).py()/(p4Infit(n).rho()*p4Infit(n).rho());
02242 tb[2][1] = p4Infit(n).e()*p4Infit(n).pz()/(p4Infit(n).rho()*p4Infit(n).rho());
02243 tb[3][1] = 1;
02244 setB(m_nc, pb, tb);
02245 setBT(pb, m_nc, tb.T());
02246 break;
02247 }
02248 case 5 : {
02249 HepMatrix ta(kc.nc(), 3, 0);
02250 ta[0][0] = -p4Infit(n).py();
02251 ta[0][1] = -p4Infit(n).px()*(lambda/sqrt(1 + lambda*lambda));
02252 ta[1][0] = p4Infit(n).px();
02253 ta[1][1] = -p4Infit(n).py()*(lambda/sqrt(1 + lambda*lambda));
02254 ta[2][0] = 0;
02255 ta[2][1] = p4Infit(n).pz()*(1/(lambda * (1 + lambda*lambda)));
02256 ta[3][2] = p4Infit(n).m()/p4Infit(n).e();
02257 setA(m_nc, pa, ta);
02258 setAT(pa, m_nc, ta.T());
02259
02260 HepMatrix tb(kc.nc(), 1, 0);
02261 tb[0][0] = p4Infit(n).px()/p4Infit(n).rho();
02262 tb[1][0] = p4Infit(n).py()/p4Infit(n).rho();
02263 tb[2][0] = p4Infit(n).pz()/p4Infit(n).rho();
02264 tb[3][0] = p4Infit(n).rho()/p4Infit(n).e();
02265 setB(m_nc, pb, tb);
02266 setBT(pb, m_nc, tb.T());
02267 break;
02268 }
02269 case 6 : {
02270 double ptrk = m_p.sub(pa+3, pa+3)[0];
02271 double x = m_p.sub(pa, pa)[0] - m_q.sub(pb, pb)[0];
02272 double y = m_p.sub(pa+1, pa+1)[0] - m_q.sub(pb+1, pb+1)[0];
02273 double z = m_p.sub(pa+2, pa+2)[0] - m_q.sub(pb+2, pb+2)[0];
02274 double R = sqrt(x*x + y*y + z*z);
02275 HepMatrix ta(kc.nc(), 4, 0);
02276 ta[0][0] = ptrk*(y*y + z*z)/(R*R*R);
02277 ta[0][1] = -ptrk*x*y/(R*R*R);
02278 ta[0][2] = -ptrk*x*z/(R*R*R);
02279 ta[0][3] = x/R;
02280 ta[1][0] = -ptrk*y*x/(R*R*R);
02281 ta[1][1] = ptrk*(x*x + z*z)/(R*R*R);
02282 ta[1][2] = -ptrk*y*z/(R*R*R);
02283 ta[1][3] = y/R;
02284 ta[2][0] = -ptrk*z*x/(R*R*R);
02285 ta[2][1] = -ptrk*z*y/(R*R*R);
02286 ta[2][2] = ptrk*(x*x + y*y)/(R*R*R);
02287 ta[2][3] = z/R;
02288 ta[3][3] = 1;
02289 setA(m_nc, pa, ta);
02290 setAT(pa, m_nc, ta.T());
02291
02292 HepMatrix tb(kc.nc(), 3, 0);
02293 tb[0][0] = -ptrk*(y*y + z*z)/(R*R*R);
02294 tb[0][1] = ptrk*x*y/(R*R*R);
02295 tb[0][2] = ptrk*x*z/(R*R*R);
02296 tb[1][0] = ptrk*y*x/(R*R*R);
02297 tb[1][1] = -ptrk*(x*x + z*z)/(R*R*R);
02298 tb[1][2] = ptrk*y*z/(R*R*R);
02299 tb[2][0] = ptrk*z*x/(R*R*R);
02300 tb[2][1] = ptrk*z*y/(R*R*R);
02301 tb[2][2] = -ptrk*(x*x + y*y)/(R*R*R);
02302 HepMatrix tbp = m_B.sub(1, 4, 1, 3);
02303 tb = tbp + tb;
02304 setB(m_nc, pb, tb);
02305 setBT(pb, m_nc, tb.T());
02306 break;
02307 }
02308 case 7 : {
02309 HepMatrix ta(kc.nc(), NTRKPAR, 0);
02310 ta[0][0] = 1.0;
02311 ta[1][1] = 1.0;
02312 ta[2][2] = 1.0;
02313 ta[3][0] = p4Infit(n).px() / p4Infit(n).e();
02314 ta[3][1] = p4Infit(n).py() / p4Infit(n).e();
02315 ta[3][2] = p4Infit(n).pz() / p4Infit(n).e();
02316 ta[3][3] = p4Infit(n).m() / p4Infit(n).e();
02317 setA(m_nc, pa, ta);
02318 setAT(pa, m_nc, ta.T());
02319 break;
02320 }
02321 }
02322 }
02323
02324 HepVector dc(kc.nc(), 0);
02325 dc[0] = pmis.px() - p4.px();
02326 dc[1] = pmis.py() - p4.py();
02327 dc[2] = pmis.pz() - p4.pz();
02328 dc[3] = pmis.e() - p4.e();
02329 for(int i = 0; i < kc.nc(); i++) {
02330 m_G[m_nc+i] = dc[i];
02331 }
02332
02333 m_nc += 4;
02334
02335 break;
02336 }
02337 }
02338 }
02339
02340