260{
261 bool debug = false;
262
263 int state = 0;
264
265 double tension = 9999.;
266
267 int nPar=5;
268 if(myFitCircleOnly) nPar=3;
269 if(myFitCircleOriginOnly) nPar=2;
270
271
272
273 for(int i=0; i<43; i++) myNumMdcDigiPerLayer[i]=0;
274 myNActiveHits=0;
275 myNFittedHits=0;
276 int nXHits[3]={0,0,0}, nVHits[3]={0,0,0};
277 int nDigi=myVecMdcDigi.size();
278
279 vector<double> vec_zini(nDigi);
280 int maxLayer = 0;
281 int i_digi=0;
282 vector<const MdcDigi*>::iterator iter_mdcDigi = myVecMdcDigi.begin();
283 for(; iter_mdcDigi!=myVecMdcDigi.end(); iter_mdcDigi++, i_digi++)
284 {
285
286 vec_zini[i_digi]=0;
287
291
292 if(myMdcDigiIsActive[i_digi]==1)
293 {
294 myNActiveHits++;
295
296 int hitFlag = myWireFlag[layer];
297 if(hitFlag==0)
298 {
299 nXHits[0]++;
300 nXHits[1]++;
301 }
302 else
303 {
304 nVHits[0]++;
305 nVHits[1]++;
306 }
307 }
308 if(maxLayer<layer) maxLayer=layer;
309 int nHits = myNumMdcDigiPerLayer[layer];
310 if(nHits<2) {
311 myIdxMdcDigiNeighbour[layer][nHits]=i_digi;
312 myWireIdMdcDigiNeighbour[layer][nHits]=wire;
313 }
314 myNumMdcDigiPerLayer[layer]++;
315 }
316
318 double maxRTrk = farPoint.perp();
319 for(int i=0; i<43; i++)
320 {
321 if(maxRTrk<myRWires[i]+2) break;
322 if(myNumMdcDigiPerLayer[i]==2)
323 {
324 int wire_1 = myWireIdMdcDigiNeighbour[i][0];
325 int wire_2 = myWireIdMdcDigiNeighbour[i][1];
326 int delta_n =
abs(wire_1-wire_2);
327 int ambi_1 = 0;
328 int ambi_2 = 0;
329 if(delta_n==1)
330 {
331 ambi_1 = wire_1>wire_2? 1:-1;
332 ambi_2 = wire_1>wire_2? -1:1;
333 }
334 else if(delta_n==myNWire[i]-1)
335 {
336 ambi_1 = wire_1<=1? 1:-1;
337 ambi_2 = wire_2<=1? 1:-1;
338 }
339
340
341
342
343
344
345
346
347
348
349
350 }
351 }
352 i_digi=0;
353 vector<const RecCgemCluster*>::iterator iter_cluster = myVecCgemCluster.begin();
354 for(; iter_cluster!=myVecCgemCluster.end(); iter_cluster++, i_digi++)
355 {
356 int flag = (*iter_cluster)->getflag();
357 if(flag==0)
358 {
359 nXHits[0]++;
360 nXHits[2]++;
361 }
362 else if(flag==1)
363 {
364 nVHits[0]++;
365 nVHits[2]++;
366 }
367 myNActiveHits++;
368 }
369 if(myFitCircleOnly)
370 {
371 if(nXHits[0]<myMinXhitsInCircleFit) return 1;
372 }
373 else
374 {
375 if((nXHits[0]+nVHits[0])<myMinHitsInHelixFit) return 2;
376 if(nVHits[0]<myMinVhitsInHelixFit) return 3;
377 }
378
379
380
381
382
383 double lastTotChi2 = 999999;
384 double secLastTotChi2 = 999999;
385 double thirdLastTotChi2 = 999999;
386 int nIterations = 0;
387 int n_chi2_increase = 0;
388 int n_oscillation=0;
389
390 while(1)
391 {
392 myNFittedHits=0;
393 myMapFlylenIdx.clear();
394
395 HepSymMatrix U(nPar, 0);
396
397
398
399 HepMatrix
P(nPar,1,0);
400 HepMatrix J(nPar,1,0), JT(1,nPar,0);
401 HepMatrix J_dmdx(1,3,0), J_dxda(3,nPar,0);
402
403
404 double totChi2=0;
405 i_digi=0;
406 vector<const MdcDigi*>::iterator iter_mdcDigi = myVecMdcDigi.begin();
407 for(; iter_mdcDigi!=myVecMdcDigi.end(); iter_mdcDigi++, i_digi++)
408 {
409
413 double charge = (*iter_mdcDigi)->getChargeChannel();
414
415
416
417 double wpos[7]={myEastPosWires[layer][wire][0], myEastPosWires[layer][wire][1], myEastPosWires[layer][wire][2]
418 , myWestPosWires[layer][wire][0], myWestPosWires[layer][wire][1], myWestPosWires[layer][wire][2], tension};
419 double doca_trk;
420 double whitPos[3];
421 if(debug)
422 {
423
424 cout<<"* layer "<<layer<<", wire "<<wire<<", is active "<<myMdcDigiIsActive[i_digi] <<endl;
425
426
427 }
429
430
431
432
433
434 int leftRight = 2;
435 if(doca_trk!=0) {
436 leftRight=int(doca_trk/fabs(doca_trk));
437 }
438
439
440
441
442
443 int signDoca=1;
444 signDoca=leftRight/fabs(leftRight);
445
446
447 if(leftRight==1) leftRight=0;
448 else leftRight=1;
449
450
451 vec_zini[i_digi] = whitPos[2];
452
453
455 double tprop = myMdcCalibFunSvc->
getTprop(layer, vec_zini[i_digi]);
456 double T0Walk = myMdcCalibFunSvc->
getT0(layer,wire) + myMdcCalibFunSvc->
getTimeWalk(layer, charge);
457
458
460 HepPoint3D aNewPivot(whitPos[0],whitPos[1],whitPos[2]);
461
462 aHelix.
pivot(aNewPivot);
463
464 double newPhi0 = aHelix.
phi0();
465
466 double dphi = newPhi0-myHelix->
phi0();
467
468
469
470 if(dphi<-M_PI||dphi>
M_PI) dphi=atan2(
sin(dphi),
cos(dphi));
471 double flightLength = fabs(dphi*myHelix->
radius())*sqrt(1+myHelix->
tanl()*myHelix->
tanl());
472
473 myMapFlylenIdx[flightLength]=i_digi;
474 HepLorentzVector p4_pi = myHelix->
momentum(dphi, 0.13957);
476 double TOF = flightLength/speed*1.e9;
477
478
479 double driftT = rawTime - myEventT0 - TOF - T0Walk - tprop;
480
481
482 double phiWire = atan2(whitPos[1],whitPos[0]);
483 double phiP = p4_pi.phi();
484 double entranceAngle = phiP-phiWire;
485
486
487 if(entranceAngle<-M_PI||entranceAngle>
M_PI) entranceAngle=atan2(
sin(entranceAngle),
cos(entranceAngle));
488
489 double doca_hit = 0.1 * myMdcCalibFunSvc->
driftTimeToDist(driftT,layer,wire,leftRight, entranceAngle);
490
491 double docaErr_hit = 0.1 * myMdcCalibFunSvc->
getSigma(layer, leftRight, doca_hit, entranceAngle, myHelix_a[4], vec_zini[i_digi], charge);
492
493
494
495 doca_hit*=signDoca;
496
497 double delD = doca_hit-doca_trk;
498 double chi = delD/docaErr_hit;
499 if(debug) cout<<"delta_m, err_m, chi = "<<delD<<", "<<docaErr_hit<<", "<<chi<<endl;
500 myChiVecMdcDigi[i_digi]=chi;
501
502 if(myMdcDigiIsActive[i_digi]!=1) continue;
503 if(myUseAxialHitsOnly && myWireFlag[layer]!=0) continue;
504
505 myNFittedHits++;
506 totChi2+=chi*chi;
507 for(int ipar=0; ipar<nPar; ipar++)
508 {
509 double deri;
510
512 J(ipar+1,1) = deri;
513
514
515
516 double scale=1;
517 P(ipar+1,1) += J(ipar+1,1)*(doca_hit-doca_trk)/(docaErr_hit*docaErr_hit*scale);
518 for(int ipar2=0; ipar2<=ipar; ipar2++)
519 {
520
521 U(ipar+1,ipar2+1)+=J(ipar+1,1)*J(ipar2+1,1)/(docaErr_hit*docaErr_hit*scale);
522 }
523 }
524
525 }
526 if(debug) cout<<"end of MDC hits loop"<<endl;
527
528
529
530 i_digi=0;
531 vector<const RecCgemCluster*>::iterator iter_cluster = myVecCgemCluster.begin();
532 for(; iter_cluster!=myVecCgemCluster.end(); iter_cluster++, i_digi++)
533 {
534
535 int layer = (*iter_cluster)->getlayerid();
536 int flag = (*iter_cluster)->getflag();
537 if(myUseAxialHitsOnly && flag!=0) continue;
538 if(myUseMdcHitsOnly) continue;
539 int sheet = (*iter_cluster)->getsheetid();
541 if(debug)
542 {
543 cout<<endl<<"layer, sheet, flag = "<<setw(5)<<layer<<setw(5)<<sheet<<setw(5)<<flag<<endl;
544 }
545
546
548 if(fabs(dphi)<1e-10) {
549 myChiVecCgemCluster[i_digi]=-9999;
550
551 continue;
552 }
553 double flightLength = fabs(dphi*myHelix->
radius())*sqrt(1+myHelix->
tanl()*myHelix->
tanl());
554 myMapFlylenIdx[flightLength]=-(i_digi+1);
556 if(debug) cout<<"dphi="<<dphi<<", pos="<<pos<<endl;
557 double phi_trk = pos.phi();
558 double z_trk = pos.z();
560 Hep3Vector p3_trk = myHelix->
momentum(dphi);
561 double phi_p3trk = p3_trk.phi();
562 double incidentPhi = phi_p3trk-phi_trk;
563
564
565 if(incidentPhi<-M_PI||incidentPhi>
M_PI) incidentPhi=atan2(
sin(incidentPhi),
cos(incidentPhi));
566
567
568 double phi_CC(0.0);
569 double X_CC(0.), V_CC(0.);
570 double delta_m, err_m;
571 double Q(0.);
572 double T(100);
573 int mode(2);
574 Q=(*iter_cluster)->getenergydeposit();
575 if(flag==0) {
576 phi_CC = (*iter_cluster)->getrecphi();
577
578 double del_phi = phi_CC-phi_trk;
579
580
581 if(del_phi<-M_PI||del_phi>
M_PI) del_phi=atan2(
sin(del_phi),
cos(del_phi));
582 X_CC=phi_CC*myRmidDGapCgem[layer];
583 if(debug) cout<<"phi_trk, phi_rec = "<<phi_trk<<", "<<phi_CC<<endl;
584
585
586 delta_m=del_phi;
587 err_m=myCgemCalibSvc->
getSigma(layer,flag,mode,incidentPhi,Q,T)/10.;
588 err_m/=myRmidDGapCgem[layer];
589 J_dmdx(1,1)=-1*pos.y()/myR2midDGapCgem[layer];
590 J_dmdx(1,2)=pos.x()/myR2midDGapCgem[layer];
591 J_dmdx(1,3)=0.;
592 }
593 else if(flag==1) {
594 double V_trk = readoutPlane->
getVFromPhiZ(phi_trk,z_trk*10,
false)/10.0;
595 V_CC=(*iter_cluster)->getrecv()/10.;
596
597 if(debug) cout<<"V_trk, V_rec = "<<V_trk<<", "<<V_CC<<endl;
598 delta_m=V_CC-V_trk;
599 if(fabs(delta_m)>5) {
600
601
602
603
604
605
606
608 double delta_m_2=V_CC-V_trk_nearPhiMin;
609 if(fabs(delta_m_2)<fabs(delta_m)) delta_m=delta_m_2;
610 if(debug) cout<<"V_trk_nearPhiMin= "<<V_trk_nearPhiMin<<endl;
611 }
612 err_m=myCgemCalibSvc->
getSigma(layer,flag,mode,incidentPhi,Q,T)/10.;
613 J_dmdx(1,1)=-myRVCgem[layer]*
cos(myAngStereoCgem[layer])*pos.y()/myR2midDGapCgem[layer];
614 J_dmdx(1,2)= myRVCgem[layer]*
cos(myAngStereoCgem[layer])*pos.x()/myR2midDGapCgem[layer];
615 J_dmdx(1,3)= -
sin(myAngStereoCgem[layer]);
616 }
617 else {
618 cout<<"flag ="<<flag<<", DotsHelixFitter::calculateNewHelix() is not ready for it!"<<endl;
619 continue;
620 }
621 JT=J_dmdx*J_dxda;
622 J=JT.T();
623
624
625
626
627
628 double chi = delta_m/err_m;
629 myChiVecCgemCluster[i_digi]=chi;
630 if(debug) cout<<"delta_m, err_m, chi = "<<delta_m<<", "<<err_m<<", "<<chi<<endl;
631 totChi2+=chi*chi;
632 myNFittedHits++;
633 for(int ipar=0; ipar<nPar; ipar++)
634 {
635
636
637 P(ipar+1,1) += J(ipar+1,1)*chi/err_m;
638 for(int ipar2=0; ipar2<=ipar; ipar2++)
639 {
640 U(ipar+1,ipar2+1)+=J(ipar+1,1)*J(ipar2+1,1)/(err_m*err_m);
641 }
642 }
643 if(debug) cout<<"U="<<U<<endl;
644 }
645 if(debug)
646 cout<<"end of CGEM cluster loop"<<endl;
647
648
649
650
651 int ifail=99;
652 U.invert(ifail);
653
654
655
656
657
658
660
661
662
663 for(int ipar=0; ipar<nPar; ipar++)
664 {
665 myHelix_a[ipar]+=da[ipar];
666 if(ipar==1&&(myHelix_a[ipar]>2*
M_PI||myHelix_a[ipar]<0))
667 {
668 double aphi=myHelix_a[ipar];
669 aphi=atan2(
sin(aphi),
cos(aphi));
670 if(aphi<0) aphi+=2*
M_PI;
671 myHelix_a[ipar]=aphi;
672 }
673 myHelix_aVec[ipar]=myHelix_a[ipar];
674 }
675 myHelix->
a(myHelix_aVec);
676 if(debug)
677 {
678 cout<<"aNew = "<<myHelix_aVec
679 <<" chi2 = "<<totChi2
680 <<endl;
681 }
682 myChi2=totChi2;
683
684
685
686 nIterations++;
687 if(debug) cout<<" iteration "<<nIterations<<", chi2="<<totChi2<<endl;
688 if(fabs(lastTotChi2-totChi2)<myDchi2Converge) {
689 if(debug)
690 cout<<"DotsHelixFitter::calculateNewHelix(): converge after "<<nIterations<<" iterations"
691 <<" with lastTotChi2="<<lastTotChi2<<", totChi2="<<totChi2
692 <<endl;
693 if(nPar==5) {
694 myHelix_Ea = U;
696 }
697 return 0;
698
699
700 }
701 else if(totChi2-lastTotChi2>myDchi2Diverge) {
702 n_chi2_increase++;
703 if(n_chi2_increase>myMaxNChi2Increase||totChi2>myChi2Diverge)
704 return 5;
705 }
706 if( nIterations>3 && fabs(secLastTotChi2-totChi2)<(0.1*myDchi2Converge) && fabs(thirdLastTotChi2-lastTotChi2)<(0.1*myDchi2Converge) ) {
707 n_oscillation++;
708
709 if(n_oscillation>0&&totChi2<lastTotChi2) {
710 if(nPar==5) {
711 myHelix_Ea = U;
713 }
714 if(debug) cout<<"DotsHelixFitter::calculateNewHelix(): oscillation happened, thirdLastTotChi2, secLastTotChi2, lastTotChi2 = "<<thirdLastTotChi2<<", "<<secLastTotChi2<<", "<<lastTotChi2<<endl;
715 return 0;
716 }
717 }
718 if(nIterations>myMaxIteration) {
719 if(debug) cout<<"DotsHelixFitter::calculateNewHelix(): "<<nIterations
720 <<" iterations, break with lastTotChi2, totChi2 = "<<lastTotChi2<<", "<<totChi2<<endl;
721 return 4;
722
723 }
724 thirdLastTotChi2=secLastTotChi2;
725 secLastTotChi2=lastTotChi2;
726 lastTotChi2=totChi2;
727 }
728
729
730
731
732
733}
double P(RecMdcKalTrack *trk)
double abs(const EvtComplex &c)
double getVFromPhiZ(double phi, double z, bool checkRange=true) const
double getVFromPhiZ_nearPhiMin(double phi, double z, bool checkRange=true) const
CgemGeoReadoutPlane * getReadoutPlane(int iLayer, int iSheet) const
HepMatrix dxda_cgem(KalmanFit::Helix a, double phi)
const HepSymMatrix & Ea(void) const
returns error matrix.
const HepVector & a(void) const
returns helix parameters.
double getSigma(int layid, int lr, double dist, double entrance=0.0, double tanlam=0.0, double z=0.0, double Q=1000.0) const
double getT0(int layid, int cellid) const
double driftTimeToDist(double drifttime, int layid, int cellid, int lr, double entrance=0.0) const
double getTprop(int lay, double z) const
double getTimeWalk(int layid, double Q) const
static double MdcTime(int timeChannel)
bool getDeriLoc(int ipar, double helix[], double &deri, double wpos[], double zini)