178{
179 bool debug = false;
180
181 int state = 0;
182
183 double tension = 9999.;
184
185 int nPar=5;
186 if(myFitCircleOnly) nPar=3;
187
188
189
190 for(int i=0; i<43; i++) myNumMdcDigiPerLayer[i]=0;
191 myNActiveHits=0;
192 int nXHits[3]={0,0,0}, nVHits[3]={0,0,0};
193 int nDigi=myVecMdcDigi.size();
194 double* vec_zini = new double[nDigi];
195 int maxLayer = 0;
196 int i_digi=0;
197 vector<const MdcDigi*>::iterator iter_mdcDigi = myVecMdcDigi.begin();
198 for(; iter_mdcDigi!=myVecMdcDigi.end(); iter_mdcDigi++, i_digi++)
199 {
200
201 vec_zini[i_digi]=0;
202
206
207 if(myMdcDigiIsActive[i_digi])
208 {
209 myNActiveHits++;
210
211 int hitFlag = myWireFlag[layer];
212 if(hitFlag==0)
213 {
214 nXHits[0]++;
215 nXHits[1]++;
216 }
217 else
218 {
219 nVHits[0]++;
220 nVHits[1]++;
221 }
222 }
223 if(maxLayer<layer) maxLayer=layer;
224 int nHits = myNumMdcDigiPerLayer[layer];
225 if(nHits<2) {
226 myIdxMdcDigiNeighbour[layer][nHits]=i_digi;
227 myWireIdMdcDigiNeighbour[layer][nHits]=wire;
228 }
229 myNumMdcDigiPerLayer[layer]++;
230 }
231
233 double maxRTrk = farPoint.perp();
234 for(int i=0; i<43; i++)
235 {
236 if(maxRTrk<myRWires[i]+2) break;
237 if(myNumMdcDigiPerLayer[i]==2)
238 {
239 int wire_1 = myWireIdMdcDigiNeighbour[i][0];
240 int wire_2 = myWireIdMdcDigiNeighbour[i][1];
241 int delta_n =
abs(wire_1-wire_2);
242 int ambi_1 = 0;
243 int ambi_2 = 0;
244 if(delta_n==1)
245 {
246 ambi_1 = wire_1>wire_2? 1:-1;
247 ambi_2 = wire_1>wire_2? -1:1;
248 }
249 else if(delta_n==myNWire[i]-1)
250 {
251 ambi_1 = wire_1<=1? 1:-1;
252 ambi_2 = wire_2<=1? 1:-1;
253 }
254 if(debug)
255 cout<<"layer, wire1, wire2, ambi1, ambi2 = "
256 <<setw(5)<<i
257 <<setw(5)<<wire_1
258 <<setw(5)<<wire_2
259 <<setw(5)<<ambi_1
260 <<setw(5)<<ambi_2
261 <<endl;
262
263
264
265 }
266 }
267 i_digi=0;
268 vector<const RecCgemCluster*>::iterator iter_cluster = myVecCgemCluster.begin();
269 for(; iter_cluster!=myVecCgemCluster.end(); iter_cluster++, i_digi++)
270 {
271 int flag = (*iter_cluster)->getflag();
272 if(flag==0)
273 {
274 nXHits[0]++;
275 nXHits[2]++;
276 }
277 else if(flag==1)
278 {
279 nVHits[0]++;
280 nVHits[2]++;
281 }
282 myNActiveHits++;
283 }
284 if(myFitCircleOnly)
285 {
286 if(nXHits[0]<myMinXhitsInCircleFit) return 1;
287 }
288 else
289 {
290 if((nXHits[0]+nVHits[0])<myMinHitsInHelixFit) return 2;
291 if(nVHits[0]<myMinVhitsInHelixFit) return 3;
292 }
293
294
295
296
297
298 double lastTotChi2 = 999999;
299 int nIterations = 0;
300 int n_chi2_increase = 0;
301
302 while(1)
303 {
304 myMapFlylenIdx.clear();
305
306 HepSymMatrix U(nPar, 0);
307
308
309
310 HepMatrix
P(nPar,1,0);
311 HepMatrix J(nPar,1,0), JT(1,nPar,0);
312 HepMatrix J_dmdx(1,3,0), J_dxda(3,nPar,0);
313
314
315 double totChi2=0;
316 i_digi=0;
317 vector<const MdcDigi*>::iterator iter_mdcDigi = myVecMdcDigi.begin();
318 for(; iter_mdcDigi!=myVecMdcDigi.end(); iter_mdcDigi++, i_digi++)
319 {
320
324 double charge = (*iter_mdcDigi)->getChargeChannel();
325
326
327
328 double wpos[7]={myEastPosWires[layer][wire][0], myEastPosWires[layer][wire][1], myEastPosWires[layer][wire][2]
329 , myWestPosWires[layer][wire][0], myWestPosWires[layer][wire][1], myWestPosWires[layer][wire][2], tension};
330 double doca_trk;
331 double whitPos[3];
332 if(debug)
333 {
334
335 cout<<"* layer "<<layer<<", wire "<<wire<<", is active "<<myMdcDigiIsActive[i_digi] <<endl;
336
337
338 }
339 getDoca(myHelix_a, wpos, doca_trk, whitPos, vec_zini[i_digi]);
340
341 if(debug) {
342 cout<<"doca = "<<doca_trk<<endl;
343
344 }
345 int leftRight = 2;
346 if(doca_trk!=0) {
347 leftRight=int(doca_trk/fabs(doca_trk));
348 }
349
350
351
352
353
354 int signDoca=1;
355 signDoca=leftRight/fabs(leftRight);
356
357
358 if(leftRight==1) leftRight=0;
359 else leftRight=1;
360
361
362 vec_zini[i_digi] = whitPos[2];
363
364
366 double tprop = myMdcCalibFunSvc->
getTprop(layer, vec_zini[i_digi]);
367 double T0Walk = myMdcCalibFunSvc->
getT0(layer,wire) + myMdcCalibFunSvc->
getTimeWalk(layer, charge);
368
370 HepPoint3D aNewPivot(whitPos[0],whitPos[1],whitPos[2]);
371 aHelix.
pivot(aNewPivot);
372 double newPhi0 = aHelix.
phi0();
373 double dphi = newPhi0-myHelix->
phi0();
376 double flightLength = fabs(dphi*myHelix->
radius())*sqrt(1+myHelix->
tanl()*myHelix->
tanl());
377 myMapFlylenIdx[flightLength]=i_digi;
378 HepLorentzVector p4_pi = myHelix->
momentum(dphi, 0.13957);
379 double speed = p4_pi.beta()*
CC;
380 double TOF = flightLength/speed*1.e9;
381
382 double driftT = rawTime - myEventT0 - TOF - T0Walk - tprop;
383
384
385 double phiWire = atan2(whitPos[1],whitPos[0]);
386 double phiP = p4_pi.phi();
387 double entranceAngle = phiP-phiWire;
388 while(entranceAngle<-
M_PI) entranceAngle+=2*
M_PI;
389 while(entranceAngle>
M_PI) entranceAngle-=2*
M_PI;
390
391 double doca_hit = 0.1 * myMdcCalibFunSvc->
driftTimeToDist(driftT,layer,wire,leftRight, entranceAngle);
392
393 double docaErr_hit = 0.1 * myMdcCalibFunSvc->
getSigma(layer, leftRight, doca_hit, entranceAngle, myHelix_a[4], vec_zini[i_digi], charge);
394
395
396
397 doca_hit*=signDoca;
398
399 double delD = doca_hit-doca_trk;
400 double chi = delD/docaErr_hit;
401 if(debug)
402 cout<<"delta_m, err_m, chi = "<<delD<<", "<<docaErr_hit<<", "<<chi<<endl;
403 myChiVecMdcDigi[i_digi]=chi;
404
405 if(!myMdcDigiIsActive[i_digi]) continue;
406 if(myUseAxialHitsOnly && myWireFlag[layer]!=0) continue;
407
408 totChi2+=chi*chi;
409 for(int ipar=0; ipar<nPar; ipar++)
410 {
411 double deri;
412
413 getDeriLoc(ipar,myHelix_a, deri, wpos, vec_zini[i_digi]);
414 J(ipar+1,1) = deri;
415
416
417
418 double scale=1;
419 P(ipar+1,1) += J(ipar+1,1)*(doca_hit-doca_trk)/(docaErr_hit*docaErr_hit*scale);
420 for(int ipar2=0; ipar2<=ipar; ipar2++)
421 {
422
423 U(ipar+1,ipar2+1)+=J(ipar+1,1)*J(ipar2+1,1)/(docaErr_hit*docaErr_hit*scale);
424 }
425 }
426 }
427 if(debug) cout<<"end of MDC hits loop"<<endl;
428
429
430
431 i_digi=0;
432 vector<const RecCgemCluster*>::iterator iter_cluster = myVecCgemCluster.begin();
433 for(; iter_cluster!=myVecCgemCluster.end(); iter_cluster++, i_digi++)
434 {
435
436 int layer = (*iter_cluster)->getlayerid();
437 int flag = (*iter_cluster)->getflag();
438 if(myUseAxialHitsOnly && flag!=0) continue;
439 int sheet = (*iter_cluster)->getsheetid();
441 if(debug)
442 {
443 cout<<endl<<"layer, sheet, flag = "<<setw(5)<<layer<<setw(5)<<sheet<<setw(5)<<flag<<endl;
444 }
445
446
449
450 double phi_trk = pos.phi();
451 double z_trk = pos.z();
453 Hep3Vector p3_trk = myHelix->
momentum(dphi);
454 double phi_p3trk = p3_trk.phi();
455 double incidentPhi = phi_p3trk-phi_trk;
456 while(incidentPhi>CLHEP::pi) incidentPhi-=CLHEP::twopi;
457 while(incidentPhi<-CLHEP::pi) incidentPhi+=CLHEP::twopi;
458
459
460 double phi_CC(0.0);
461 double X_CC(0.), V_CC(0.);
462 double delta_m, err_m;
463 double Q(0.);
464 double T(100);
465 int mode(2);
466 Q=(*iter_cluster)->getenergydeposit();
467 if(flag==0) {
468 phi_CC = (*iter_cluster)->getrecphi();
469
470 double del_phi = phi_CC-phi_trk;
471 while(del_phi<-
M_PI) del_phi+=2*
M_PI;
472 while(del_phi>
M_PI) del_phi-=2*
M_PI;
473 X_CC=phi_CC*myRmidDGapCgem[layer];
474 if(debug) cout<<"phi_trk, phi_rec = "<<phi_trk<<", "<<phi_CC<<endl;
475
476
477 delta_m=del_phi;
478 err_m=myCgemCalibSvc->
getSigma(layer,flag,mode,incidentPhi,Q,T)/10.;
479 err_m/=myRmidDGapCgem[layer];
480 J_dmdx(1,1)=-1*pos.y()/myR2midDGapCgem[layer];
481 J_dmdx(1,2)=pos.x()/myR2midDGapCgem[layer];
482 J_dmdx(1,3)=0.;
483 }
484 else if(flag==1) {
485 double V_trk = readoutPlane->
getVFromPhiZ(phi_trk,z_trk*10,
false)/10.0;
486 V_CC=(*iter_cluster)->getrecv()/10.;
487
488 if(debug) cout<<"V_trk, V_rec = "<<V_trk<<", "<<V_CC<<endl;
489 delta_m=V_CC-V_trk;
490 if(fabs(delta_m)>5) {
491
492
493
494
495
496
497
499 double delta_m_2=V_CC-V_trk_nearPhiMin;
500 if(fabs(delta_m_2)<fabs(delta_m)) delta_m=delta_m_2;
501 if(debug) cout<<"V_trk_nearPhiMin= "<<V_trk_nearPhiMin<<endl;
502 }
503 err_m=myCgemCalibSvc->
getSigma(layer,flag,mode,incidentPhi,Q,T)/10.;
504 J_dmdx(1,1)=-myRVCgem[layer]*
cos(myAngStereoCgem[layer])*pos.y()/myR2midDGapCgem[layer];
505 J_dmdx(1,2)= myRVCgem[layer]*
cos(myAngStereoCgem[layer])*pos.x()/myR2midDGapCgem[layer];
506 J_dmdx(1,3)= -
sin(myAngStereoCgem[layer]);
507 }
508 else {
509 cout<<"flag ="<<flag<<", DotsHelixFitter::calculateNewHelix() is not ready for it!"<<endl;
510 continue;
511 }
512 JT=J_dmdx*J_dxda;
513 J=JT.T();
514 double chi = delta_m/err_m;
515 myChiVecCgemCluster[i_digi]=chi;
516 if(debug)
517 cout<<"delta_m, err_m, chi = "<<delta_m<<", "<<err_m<<", "<<chi<<endl;
518 totChi2+=chi*chi;
519 for(int ipar=0; ipar<nPar; ipar++)
520 {
521
522
523 P(ipar+1,1) += J(ipar+1,1)*chi/err_m;
524 for(int ipar2=0; ipar2<=ipar; ipar2++)
525 {
526 U(ipar+1,ipar2+1)+=J(ipar+1,1)*J(ipar2+1,1)/(err_m*err_m);
527 }
528 }
529 }
530 if(debug)
531 cout<<"end of CGEM cluster loop"<<endl;
532
533
534
535
536
537 int ifail=99;
538 U.invert(ifail);
539
540
541
542
543
544
546
547
548
549
550 for(int ipar=0; ipar<nPar; ipar++)
551 {
552 myHelix_a[ipar]+=da[ipar];
553 myHelix_aVec[ipar]=myHelix_a[ipar];
554 }
555 myHelix->
a(myHelix_aVec);
556 if(debug)
557 {
558 cout<<"aNew = "<<myHelix_aVec
559 <<" chi2 = "<<totChi2
560 <<endl;
561 }
562 myChi2=totChi2;
563
564
565
566 nIterations++;
567 if(fabs(lastTotChi2-totChi2)<myDchi2Converge) {
568 if(debug)
569 cout<<"DotsHelixFitter::calculateNewHelix(): converge after "<<nIterations<<" iterations"
570 <<" with lastTotChi2="<<lastTotChi2<<", totChi2="<<totChi2
571 <<endl;
572 if(nPar==5) {
573 myHelix_Ea = U;
575 }
576
577 break;
578 }
579 else if(totChi2-lastTotChi2>myDchi2Diverge) {
580 n_chi2_increase++;
581 if(n_chi2_increase>myMaxNChi2Increase||totChi2>myChi2Diverge)
582 return 5;
583 }
584 if(nIterations>myMaxIteration) {
585 if(debug) cout<<"DotsHelixFitter::calculateNewHelix(): "<<nIterations
586 <<" iterations, break with lastTotChi2, totChi2 = "<<lastTotChi2<<", "<<totChi2<<endl;
587 break;
588 }
589 lastTotChi2=totChi2;
590 }
591
592
593 delete []vec_zini;
594
595 if(nIterations>myMaxIteration) return 4;
596 else return 0;
597}
double sin(const BesAngle a)
double cos(const BesAngle a)
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)
double IntersectCylinder(double r)
virtual double getSigma(int layer, int xvFlag, int readoutMode, double angle, double Q, double T) const =0
Hep3Vector momentum(double dPhi=0.) const
returns momentum vector after rotating angle dPhi in phi direction.
const HepSymMatrix & Ea(void) const
returns error matrix.
HepPoint3D x(double dPhi=0.) const
returns position after rotating angle dPhi in phi direction.
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 int layer(const Identifier &id)
Values of different levels (failure returns 0)
static int wire(const Identifier &id)
static double MdcTime(int timeChannel)
bool getDeriLoc(int ipar, double helix[], double &deri, double wpos[], double zini)