00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013 #include <float.h>
00014 #include <string.h>
00015 #include "TrkReco/TRunge.h"
00016 #include "TrkReco/TRungeFitter.h"
00017 #include "TrkReco/TMDCWire.h"
00018 #include "TrkReco/TTrack.h"
00019
00020 #include "CLHEP/Matrix/Matrix.h"
00021 #include "GaudiKernel/StatusCode.h"
00022 #include "GaudiKernel/IInterface.h"
00023 #include "GaudiKernel/Kernel.h"
00024 #include "GaudiKernel/Service.h"
00025 #include "GaudiKernel/ISvcLocator.h"
00026 #include "GaudiKernel/SvcFactory.h"
00027 #include "GaudiKernel/IDataProviderSvc.h"
00028 #include "GaudiKernel/Bootstrap.h"
00029 #include "GaudiKernel/MsgStream.h"
00030 #include "GaudiKernel/SmartDataPtr.h"
00031 #include "GaudiKernel/IMessageSvc.h"
00032 typedef HepGeom::Transform3D HepTransform3D;
00033
00034
00035 const TRungeFitter TRunge::_fitter = TRungeFitter("TRunge Default fitter");
00036
00037
00038 const double default_stepSize = 1.5;
00039 const double default_stepSize0 = 1.5;
00040 const double default_stepSizeMax =5;
00041 const double default_stepSizeMin = 0.001;
00042
00043 const double EPS = 1.0e-6;
00044
00045 const double default_maxflightlength = 1000;
00046
00047 TRunge::TRunge()
00048 : TTrackBase(),
00049 _pivot(ORIGIN),_a(Vector(5,0)),_Ea(SymMatrix(5,0)),
00050 _chi2(0),_ndf(0),_charge(1),_bfieldID(21),_Nstep(0),
00051 _stepSize(default_stepSize),_mass(0.140),_eps(EPS),
00052 _stepSizeMax(default_stepSizeMax),_stepSizeMin(default_stepSizeMin),
00053 _maxflightlength(default_maxflightlength) {
00054
00055
00056 fitter(& TRunge::_fitter);
00057
00058 _fitted = false;
00059 _fittedWithCathode = false;
00060
00061 _bfield = Bfield::getBfield(_bfieldID);
00062
00063 _mass2=_mass*_mass;
00064
00065 _yscal[0]=0.1;
00066 _yscal[1]=0.1;
00067 _yscal[2]=0.1;
00068 _yscal[3]=0.001;
00069 _yscal[4]=0.001;
00070 _yscal[5]=0.001;
00071
00072 StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF);
00073 if(scmgn!=StatusCode::SUCCESS) {
00074 std::cout<< __FILE__<<" Unable to open Magnetic field service"<<std::endl;
00075 }
00076
00077 }
00078
00079 TRunge::TRunge(const RecMdcTrack& a)
00080 : TTrackBase(),
00081 _pivot(a.getPivot()),_a(a.helix()),_Ea(a.err()),
00082 _chi2(a.chi2()),_ndf(a.ndof()),_bfieldID(21),_Nstep(0),
00083 _stepSize(default_stepSize),_mass(0.140),_eps(EPS),
00084 _stepSizeMax(default_stepSizeMax),_stepSizeMin(default_stepSizeMin),
00085 _maxflightlength(default_maxflightlength) {
00086
00087
00088 fitter(& TRunge::_fitter);
00089
00090 _fitted = false;
00091 _fittedWithCathode = false;
00092
00093 if(_a[2]<0) _charge=-1;
00094 else _charge=1;
00095
00096 _bfield = Bfield::getBfield(_bfieldID);
00097
00098 _mass2=_mass*_mass;
00099
00100 _yscal[0]=0.1;
00101 _yscal[1]=0.1;
00102 _yscal[2]=0.1;
00103 _yscal[3]=0.001;
00104 _yscal[4]=0.001;
00105 _yscal[5]=0.001;
00106 StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF);
00107 if(scmgn!=StatusCode::SUCCESS) {
00108 std::cout<< "Unable to open Magnetic field service"<<std::endl;
00109 }
00110
00111
00112
00113 }
00114 TRunge::TRunge(const TTrack& a)
00115 : TTrackBase(a),
00116 _pivot(a.helix().pivot()),_a(a.helix().a()),_Ea(a.helix().Ea()),
00117 _chi2(0),_ndf(0),_bfieldID(21),_Nstep(0),
00118 _stepSize(default_stepSize),_mass(0.140),_eps(EPS),
00119 _stepSizeMax(default_stepSizeMax),_stepSizeMin(default_stepSizeMin),
00120 _maxflightlength(default_maxflightlength) {
00121
00122
00123 fitter(& TRunge::_fitter);
00124
00125 _fitted = false;
00126 _fittedWithCathode = false;
00127
00128 if(_a[2]<0) _charge=-1;
00129 else _charge=1;
00130
00131 _bfield = Bfield::getBfield(_bfieldID);
00132
00133 _mass2=_mass*_mass;
00134
00135 _yscal[0]=0.1;
00136 _yscal[1]=0.1;
00137 _yscal[2]=0.1;
00138 _yscal[3]=0.001;
00139 _yscal[4]=0.001;
00140 _yscal[5]=0.001;
00141 StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF);
00142 if(scmgn!=StatusCode::SUCCESS) {
00143 std::cout<< "Unable to open Magnetic field service"<<std::endl;
00144 }
00145
00146
00147 }
00148
00149 TRunge::TRunge(const Helix & h)
00150 : TTrackBase(),
00151 _pivot(h.pivot()),_a(h.a()),_Ea(h.Ea()),
00152 _chi2(0),_ndf(0),_bfieldID(21),_Nstep(0),
00153 _stepSize(default_stepSize),_mass(0.140),_eps(EPS),
00154 _stepSizeMax(default_stepSizeMax),_stepSizeMin(default_stepSizeMin),
00155 _maxflightlength(default_maxflightlength) {
00156
00157
00158 fitter(& TRunge::_fitter);
00159
00160 _fitted = false;
00161 _fittedWithCathode = false;
00162
00163 if(_a[2]<0) _charge=-1;
00164 else _charge=1;
00165
00166 _bfield = Bfield::getBfield(_bfieldID);
00167
00168 _mass2=_mass*_mass;
00169
00170 _yscal[0]=0.1;
00171 _yscal[1]=0.1;
00172 _yscal[2]=0.1;
00173 _yscal[3]=0.001;
00174 _yscal[4]=0.001;
00175 _yscal[5]=0.001;
00176 }
00177
00178 TRunge::TRunge(const TRunge& a)
00179 : TTrackBase((TTrackBase &) a),
00180 _pivot(a.pivot()),_a(a.a()),_Ea(a.Ea()),
00181 _chi2(a.chi2()),_ndf(a.ndf()),_bfieldID(a.BfieldID()),_Nstep(0),
00182 _stepSize(a.StepSize()),_mass(a.Mass()),_eps(a.Eps()),
00183 _stepSizeMax(a.StepSizeMax()),_stepSizeMin(a.StepSizeMin()),
00184 _maxflightlength(a.MaxFlightLength()) {
00185
00186
00187 fitter(& TRunge::_fitter);
00188
00189 _fitted = false;
00190 _fittedWithCathode = false;
00191
00192 if(_a[2]<0) _charge=-1;
00193 else _charge=1;
00194
00195 _bfield = Bfield::getBfield(_bfieldID);
00196
00197 _mass2=_mass*_mass;
00198
00199 for(unsigned i=0;i<6;i++) _yscal[i]=a.Yscal()[i];
00200 }
00201
00202
00203 TRunge::~TRunge() {
00204 }
00205
00206 double TRunge::dr(void) const{
00207 return(_a[0]);
00208 }
00209
00210 double TRunge::phi0(void) const{
00211 return(_a[1]);
00212 }
00213
00214 double TRunge::kappa(void) const{
00215 return(_a[2]);
00216 }
00217
00218 double TRunge::dz(void) const{
00219 return(_a[3]);
00220 }
00221
00222 double TRunge::tanl(void) const{
00223 return(_a[4]);
00224 }
00225
00226 const HepPoint3D& TRunge::pivot(void) const{
00227 return(_pivot);
00228 }
00229
00230 const Vector& TRunge::a(void) const{
00231 return(_a);
00232 }
00233
00234 const SymMatrix& TRunge::Ea(void) const{
00235 return(_Ea);
00236 }
00237
00238 Helix TRunge::helix(void) const{
00239 return(Helix(_pivot,_a,_Ea));
00240 }
00241
00242 unsigned TRunge::ndf(void) const{
00243 return _ndf;
00244 }
00245
00246 double TRunge::chi2(void) const{
00247 return _chi2;
00248 }
00249
00250 double TRunge::reducedchi2(void) const{
00251 if(_ndf==0){
00252 std::cout<<"error at TRunge::reducedchi2 ndf=0"<<std::endl;
00253 return 0;
00254 }
00255 return (_chi2/_ndf);
00256 }
00257
00258 int TRunge::BfieldID(void) const{
00259 return(_bfieldID);
00260 }
00261
00262 double TRunge::StepSize(void) const{
00263 return(_stepSize);
00264 }
00265
00266 const double* TRunge::Yscal(void) const{
00267 return(_yscal);
00268 }
00269 double TRunge::Eps(void) const{
00270 return(_eps);
00271 }
00272 double TRunge::StepSizeMax(void) const{
00273 return(_stepSizeMax);
00274 }
00275 double TRunge::StepSizeMin(void) const{
00276 return(_stepSizeMin);
00277 }
00278
00279 float TRunge::Mass(void) const{
00280 return(_mass);
00281 }
00282
00283 double TRunge::MaxFlightLength(void) const{
00284 return(_maxflightlength);
00285 }
00286
00287 const HepPoint3D& TRunge::pivot(const HepPoint3D& newpivot){
00290 Helix tHelix(helix());
00291 tHelix.pivot(newpivot);
00292 _pivot=newpivot;
00293 _a=tHelix.a();
00294 _Ea=tHelix.Ea();
00295 _Nstep=0;
00296 return(_pivot);
00297 }
00298
00299 const Vector& TRunge::a(const Vector& ta){
00300 _a=ta;
00301 if(_a[2]<0) _charge=-1;
00302 else _charge=1;
00303 _Nstep=0;
00304 return(_a);
00305 }
00306
00307 const SymMatrix& TRunge::Ea(const SymMatrix& tEa){
00308 _Ea=tEa;
00309 _Nstep=0;
00310 return(_Ea);
00311 }
00312
00313 int TRunge::BfieldID(int id){
00314 _bfieldID=id;
00315 _bfield = Bfield::getBfield(_bfieldID);
00316 _Nstep=0;
00317 return(_bfieldID);
00318 }
00319
00320 double TRunge::StepSize(double step){
00321 _stepSize=step;
00322 _Nstep=0;
00323 return(_stepSize);
00324 }
00325
00326 const double* TRunge::Yscal(const double y[6]){
00327 for(unsigned i=0;i<6;i++) _yscal[i]=y[i];
00328 _Nstep=0;
00329 return(_yscal);
00330 }
00331 double TRunge::Eps(double eps){
00332 _eps=eps;
00333 _Nstep=0;
00334 return(_eps);
00335 }
00336 double TRunge::StepSizeMax(double step){
00337 _stepSizeMax=step;
00338 _Nstep=0;
00339 return(_stepSizeMax);
00340 }
00341 double TRunge::StepSizeMin(double step){
00342 _stepSizeMin=step;
00343 _Nstep=0;
00344 return(_stepSizeMin);
00345 }
00346
00347 float TRunge::Mass(float mass){
00348 _mass=mass;
00349 _mass2=_mass*_mass;
00350 return(_mass);
00351 }
00352
00353 double TRunge::MaxFlightLength(double length){
00354 if(length>0) _maxflightlength=length;
00355 else _maxflightlength=default_maxflightlength;
00356 _Nstep=0;
00357 return(_maxflightlength);
00358 }
00359
00360 int TRunge::approach(TMLink& l,const RkFitMaterial material,bool doSagCorrection) {
00361 float tof;
00362 HepVector3D p;
00363 return(approach(l,tof,p,material,doSagCorrection));
00364 }
00365
00366 int TRunge::approach(TMLink& l,float& tof,HepVector3D& p,
00367 const RkFitMaterial material,bool doSagCorrection) {
00368 HepPoint3D xw;
00369 HepPoint3D wireBackwardPosition;
00370 HepVector3D v;
00371 HepPoint3D onWire,onTrack;
00372 double onWire_x,onWire_y, onWire_z, zwf, zwb;
00373 const TMDCWire& w=*l.wire();
00374 xw = w.xyPosition();
00375 wireBackwardPosition = w.backwardPosition();
00376 v = w.direction();
00377
00378 unsigned stepNum=0;
00379 if(approach_line(l,wireBackwardPosition,v,onWire,onTrack,tof,p,material,stepNum)<0){
00380
00381 return(-1);
00382 }
00383 zwf = w.forwardPosition().z();
00384 zwb = w.backwardPosition().z();
00385
00386 if(onWire.z() > zwf)
00387 w.wirePosition(zwf,onWire,wireBackwardPosition,(HepVector3D&)v);
00388 else if(onWire.z() < zwb)
00389 w.wirePosition(zwb,onWire,wireBackwardPosition,(HepVector3D&)v);
00390
00391
00392
00393 if(!doSagCorrection){
00394 l.positionOnWire(onWire);
00395 l.positionOnTrack(onTrack);
00396 double phipoint=atan((onTrack.y()-_pivot.y())/onTrack.x()-_pivot.x());
00397
00398
00399
00400 return(0);
00401 }
00402
00403
00404 onWire_y = onWire.y();
00405 onWire_z = onWire.z();
00406
00407 unsigned nTrial = 1;
00408 while(nTrial<100){
00409
00410 w.wirePosition(onWire_z,xw,wireBackwardPosition,(HepVector3D&)v);
00411 if(approach_line(l,wireBackwardPosition,v,onWire,onTrack,tof,p,material,stepNum)<0)
00412 return(-1);
00413 if(fabs(onWire_y - onWire.y())<0.0001) break;
00414 onWire_y = onWire.y();
00415 onWire_z += (onWire.z()-onWire_z)/2;
00416
00417 if(onWire_z > zwf) onWire_z=zwf;
00418 else if(onWire_z < zwb) onWire_z=zwb;
00419
00420 nTrial++;
00421 }
00422
00423 l.positionOnWire(onWire);
00424 l.positionOnTrack(onTrack);
00425 return(nTrial);
00426 }
00427
00428 int TRunge::approach_line(TMLink& l,const HepPoint3D& w0,const HepVector3D& v,
00429 HepPoint3D& onLine,HepPoint3D& onTrack,const RkFitMaterial material) {
00430 float tof;
00431 HepVector3D p;
00432 return(approach_line(l,w0,v,onLine,onTrack,tof,p,material));
00433 }
00434
00435 int TRunge::approach_line(TMLink& l,const HepPoint3D& w0,const HepVector3D& v,
00436 HepPoint3D& onLine,HepPoint3D& onTrack,
00437 float& tof,HepVector3D& p,const RkFitMaterial material) {
00438 unsigned stepNum=0;
00439 return(approach_line(l,w0,v,onLine,onTrack,tof,p,material ,stepNum));
00440 }
00441
00442 int TRunge::approach_line(TMLink& l,const HepPoint3D& w0,const HepVector3D& v,
00443 HepPoint3D& onLine,HepPoint3D& onTrack,
00444 float& tof,HepVector3D& p ,const RkFitMaterial material,unsigned& stepNum) {
00445
00446 if(_Nstep==0){
00447 if(_stepSize==0) Fly_SC();
00448 else Fly(material);
00449
00450 }
00451
00452
00453 const double w0x = w0.x();
00454 const double w0y = w0.y();
00455 const double w0z = w0.z();
00456
00457 const double vx = v.x();
00458 const double vy = v.y();
00459 const double vz = v.z();
00460 const double v_2 = vx*vx+vy*vy+vz*vz;
00461
00462 const float clight=29.9792458;
00463
00464
00465 const float p2 = _y[0][3]*_y[0][3]+_y[0][4]*_y[0][4]+_y[0][5]*_y[0][5];
00466 const float tof_factor=1./clight*sqrt(1+_mass2/p2);
00467
00468
00469
00470 double l2_old= DBL_MAX;
00471 if(stepNum>_Nstep) stepNum=0;
00472 unsigned stepNumLo;
00473 unsigned stepNumHi;
00474 if(stepNum==0 && _stepSize!=0){
00475 const double dx = _y[0][0]-w0x;
00476 const double dy = _y[0][1]-w0y;
00477 stepNum=(unsigned)
00478 (sqrt( (dx*dx+dy*dy)*(1+_a[4]*_a[4]) )/_stepSize);
00479 }
00480 unsigned mergin;
00481 if(_stepSize==0){
00482 mergin=10;
00483 }else{
00484 mergin=(unsigned)(1.0/_stepSize);
00485 }
00486 if(stepNum>mergin) stepNum-=mergin;
00487 else stepNum=0;
00488 if(stepNum>=_Nstep) stepNum=_Nstep-1;
00489
00490
00491 unsigned inc=(mergin>>1)+1;
00492 stepNumLo=stepNum;
00493 stepNumHi=stepNum;
00494 for(;;){
00495 const double dx = _y[stepNumHi][0]-w0x;
00496 const double dy = _y[stepNumHi][1]-w0y;
00497 const double dz = _y[stepNumHi][2]-w0z;
00498 const double t = (dx*vx+dy*vy+dz*vz)/v_2;
00499
00500 HepVector3D zvec;
00501 zvec.setX(0);
00502 zvec.setY(0);
00503 zvec.setZ(1);
00504 HepVector3D hitv=t*v;
00505 HepPoint3D pp,onw;
00506 pp.setX(_y[stepNumHi][0]);
00507 pp.setY(_y[stepNumHi][1]);
00508 pp.setZ(_y[stepNumHi][2]);
00509 double zwire=hitv.dot(zvec)+w0z;
00510 double dtl=DisToWire(l,zwire,pp,onw);
00511 const double l2=dtl*dtl;
00512 if(l2 > l2_old) break;
00513 l2_old=l2;
00514 stepNumLo=stepNumHi;
00515
00516 stepNumHi+=inc;
00517 if(stepNumHi>=_Nstep){
00518 stepNumHi=_Nstep;
00519 break;
00520 }
00521 }
00522
00523 while(stepNumHi-stepNumLo>1){
00524 unsigned j=(stepNumHi+stepNumLo)>>1;
00525 const double dx = _y[j][0]-w0x;
00526 const double dy = _y[j][1]-w0y;
00527 const double dz = _y[j][2]-w0z;
00528 const double t = (dx*vx+dy*vy+dz*vz)/v_2;
00529
00530 HepVector3D zv;
00531 zv.setX(0);
00532 zv.setY(0);
00533 zv.setZ(1);
00534 HepVector3D hitv=t*v;
00535 HepPoint3D point,onwir;
00536 point.setX(_y[j][0]);
00537 point.setY(_y[j][1]);
00538 point.setZ(_y[j][2]);
00539 double zwir=hitv.dot(zv)+w0z;
00540 double dtll=DisToWire(l,zwir,point,onwir);
00541 const double l2 =dtll*dtll;
00542 if(l2 > l2_old){
00543 stepNumHi=j;
00544 }else{
00545 l2_old=l2;
00546 stepNumLo=j;
00547 }
00548 }
00549
00550 stepNum=stepNumLo;
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569 if(_stepSize==0){
00570 double dstep=0;
00571 for(unsigned i=0;i<stepNum;i++) dstep+=_h[i];
00572 tof=dstep*tof_factor;
00573 }else{
00574 tof=stepNum*_stepSize*tof_factor;
00575 }
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686 double y[6];
00687 for(unsigned i=0;i<6;i++) y[i]=_y[stepNum][i];
00688
00689 double step2=0;
00690 const double A[3][3]={{1-vx*vx/v_2, -vx*vy/v_2, -vx*vz/v_2},
00691 { -vy*vx/v_2,1-vy*vy/v_2, -vy*vz/v_2},
00692 { -vz*vx/v_2, -vz*vy/v_2,1-vz*vz/v_2}};
00693 double factor=1;
00694 double g[3];
00695 double f[6];
00696 double Af[3],Af2[3];
00697 unsigned i,j,k;
00698 HepPoint3D point,onwir;
00699 for(i=0;i<10;i++){
00700
00701 const double dx = y[0]-w0x;
00702 const double dy = y[1]-w0y;
00703 const double dz = y[2]-w0z;
00704 const double t = (dx*vx+dy*vy+dz*vz)/v_2;
00705 const double d[3]={dx-t*vx,dy-t*vy,dz-t*vz};
00706
00707 HepVector3D zv;
00708 zv.setX(0);
00709 zv.setY(0);
00710 zv.setZ(1);
00711 HepVector3D hitv=t*v;
00712 point.setX(y[0]);
00713 point.setY(y[1]);
00714 point.setZ(y[2]);
00715 double zwir=hitv.dot(zv)+w0z;
00716 double dtll=DisToWire(l,zwir,point,onwir);
00717 const double l2=dtll*dtll;
00718 for(j=0;j<3;j++){
00719 g[j]=0;
00720 for(k=0;k<3;k++) g[j]+=A[j][k]*d[k];
00721 }
00722
00723 Function(y,f);
00724
00725
00726
00727
00728 double Y=0;
00729 for(j=0;j<3;j++) Y+=y[j+3]*g[j];
00730 double dYds=0;
00731 for(j=0;j<3;j++) dYds+=f[j+3]*g[j];
00732
00733 for(j=0;j<3;j++){
00734 Af[j]=0;
00735 Af2[j]=0;
00736 }
00737 for(j=0;j<3;j++){
00738
00739
00740 for(k=0;k<3;k++) Af[j]+=(A[j][k]*f[k]);
00741 for(k=0;k<3;k++) Af2[j]+=(A[j][k]*Af[k]);
00742 dYds+=y[j+3]*Af2[j];
00743
00744 }
00745
00746 const double step=-Y/dYds*factor;
00747
00748 if(fabs(step) < 0.00001) break;
00749 if(l2 > l2_old) factor/=2;
00750 l2_old=l2;
00751 Propagate(y,step,material);
00752 step2+=step;
00753 }
00754
00755 tof+=step2*tof_factor;
00756
00757 onTrack.setX(y[0]);
00758 onTrack.setY(y[1]);
00759 onTrack.setZ(y[2]);
00760 p.setX(y[3]);
00761 p.setY(y[4]);
00762 p.setZ(y[5]);
00763
00764 const double dx = y[0]-w0x;
00765 const double dy = y[1]-w0y;
00766 const double dz = y[2]-w0z;
00767 const double s = (dx*vx+dy*vy+dz*vz)/v_2;
00768
00769
00770
00771 onLine.setX(onwir.x());
00772 onLine.setY(onwir.y());
00773 onLine.setZ(onwir.z());
00774 return(0);
00775 }
00776
00777 int TRunge::approach_point(TMLink& l,const HepPoint3D& p0,HepPoint3D& onTrack,const RkFitMaterial material) const{
00778 if(_Nstep==0){
00779 if(_stepSize==0) Fly_SC();
00780 else Fly(material);
00781 }
00782
00783 double x0=p0.x();
00784 double y0=p0.y();
00785 double z0=p0.z();
00786
00787
00788
00789
00790
00791
00792 unsigned stepNum;
00793 double l2_old= DBL_MAX;
00794 for(stepNum=0;stepNum<_Nstep;stepNum++){
00795 double l2=(_y[stepNum][0]-x0)*(_y[stepNum][0]-x0)+
00796 (_y[stepNum][1]-y0)*(_y[stepNum][1]-y0)+
00797 (_y[stepNum][2]-z0)*(_y[stepNum][2]-z0);
00798 if(l2 > l2_old) break;
00799 l2_old=l2;
00800
00801
00802
00803
00804 }
00805 if(stepNum>=_Nstep) return(-1);
00806 stepNum--;
00807
00808
00809 double y[6],y_old[6];
00810 for(unsigned i=0;i<6;i++) y[i]=_y[stepNum][i];
00811 double step=_stepSize;
00812 for(;;){
00813 for(unsigned i=0;i<6;i++) y_old[i]=y[i];
00814 Propagate(y,step,material);
00815 double l2=(y[0]-x0)*(y[0]-x0)+(y[1]-y0)*(y[1]-y0)+(y[2]-z0)*(y[2]-z0);
00816 if(l2 > l2_old){
00817 for(unsigned i=0;i<6;i++) y[i]=y_old[i];
00818 }else{
00819 l2_old=l2;
00820
00821
00822 }
00823 step/=2;
00824 if(step < 0.0001) break;
00825 }
00826
00827 onTrack.setX(y[0]);
00828 onTrack.setY(y[1]);
00829 onTrack.setZ(y[2]);
00830
00831
00832
00833 return(0);
00834 }
00835
00836 unsigned TRunge::Fly(const RkFitMaterial material) const{
00837 double y[6];
00838 unsigned Nstep;
00839 double flightlength;
00840
00841 flightlength=0;
00842 SetFirst(y);
00843 for(Nstep=0;Nstep<TRunge_MAXstep;Nstep++){
00844 for(unsigned j=0;j<6;j++) _y[Nstep][j]=y[j];
00845
00846
00847 Propagate(y,_stepSize,material);
00848
00849 if( y[2]>160 || y[2]<-80 || (y[0]*y[0]+y[1]*y[1])>8100 ) break;
00850
00851
00852
00853 flightlength += _stepSize;
00854 if(flightlength>_maxflightlength) break;
00855
00856 _h[Nstep]=_stepSize;
00857 }
00858 _Nstep=Nstep+1;
00859 return(_Nstep);
00860 }
00861
00862 void TRunge::Propagate(double y[6],const double& step,const RkFitMaterial material) const{
00863
00864 double f1[6],f2[6],f3[6],f4[6],yt[6];
00865 double hh;
00866 double h6;
00867 unsigned i;
00868 const double mpi=0.13957;
00869 double me=0.000511;
00870
00871 hh = step*0.5;
00872
00873 Function(y,f1);
00874 for(i=0;i<6;i++) yt[i]=y[i]+hh*f1[i];
00875
00876
00877
00878
00879
00880
00881
00882 Function(yt,f2);
00883 for(i=0;i<6;i++) yt[i]=y[i]+hh*f2[i];
00884
00885
00886
00887
00888
00889
00890
00891 Function(yt,f3);
00892 for(i=0;i<6;i++) yt[i]=y[i]+step*f3[i];
00893
00894
00895
00896
00897
00898
00899
00900 Function(yt,f4);
00901
00902 h6 = step/6;
00903 for(i=0;i<6;i++) y[i]+=h6*(f1[i]+2*f2[i]+2*f3[i]+f4[i]);
00904
00905
00906
00907
00908
00909
00910
00911
00912
00913
00914 }
00915
00916 void TRunge::Function(const double y[6],double f[6]) const{
00917
00918
00919
00920
00921
00922
00923
00924
00925
00926 double pmag;
00927 double factor;
00928 HepVector3D B;
00929 HepPoint3D pos;
00930 pos.setX((float)y[0]);
00931 pos.setY((float)y[1]);
00932 pos.setZ((float)y[2]);
00933
00934
00935
00936
00937 m_pmgnIMF->fieldVector(10*pos,B);
00938 pmag = sqrt(y[3]*y[3]+y[4]*y[4]+y[5]*y[5]);
00939 f[0] = y[3]/pmag;
00940 f[1] = y[4]/pmag;
00941 f[2] = y[5]/pmag;
00942
00943
00944
00945
00946 double Bmag=sqrt(B.x()*B.x()+B.y()*B.y()+B.z()*B.z());
00947 factor = ((double)_charge)/(0.333564095);
00948
00949
00950
00951 f[3] = factor*(f[1]*B.z()-f[2]*B.y());
00952 f[4] = factor*(f[2]*B.x()-f[0]*B.z());
00953 f[5] = factor*(f[0]*B.y()-f[1]*B.x());
00954 }
00955
00956 void TRunge::SetFirst(double y[6]) const{
00957
00958 const double cosPhi0=cos(_a[1]);
00959 const double sinPhi0=sin(_a[1]);
00960 const double invKappa=1/abs(_a[2]);
00961
00962 y[0]= _pivot.x()+_a[0]*cosPhi0;
00963 y[1]= _pivot.y()+_a[0]*sinPhi0;
00964 y[2]= _pivot.z()+_a[3];
00965 y[3]= -sinPhi0*invKappa;
00966 y[4]= cosPhi0*invKappa;
00967 y[5]= _a[4]*invKappa;
00968 }
00969
00970
00971
00972
00973
00974
00975 unsigned TRunge::Nstep(void) const{
00976 return(_Nstep);
00977 }
00978
00979 int TRunge::GetXP(unsigned stepNum,double y[6]) const{
00980 if(stepNum>=_Nstep || stepNum>=TRunge_MAXstep) return(-1);
00981
00982 for(unsigned i=0;i<6;i++) y[i]=_y[stepNum][i];
00983 return(0);
00984 }
00985 int TRunge::GetStep(unsigned stepNum,double& step) const{
00986 if(stepNum>=_Nstep || stepNum>=TRunge_MAXstep) return(-1);
00987 step=_h[stepNum];
00988 return(0);
00989 }
00990
00991 void TRunge::Propagate1(const double y[6], const double dydx[6],
00992 const double& step, double yout[6]) const{
00993
00994 double f2[6],f3[6],f4[6],yt[6];
00995 double hh;
00996 double h6;
00997 unsigned i;
00998
00999 hh = step*0.5;
01000 for(i=0;i<6;i++) yt[i]=y[i]+hh*dydx[i];
01001
01002
01003
01004
01005
01006
01007
01008 Function(yt,f2);
01009 for(i=0;i<6;i++) yt[i]=y[i]+hh*f2[i];
01010
01011
01012
01013
01014
01015
01016
01017 Function(yt,f3);
01018 for(i=0;i<6;i++) yt[i]=y[i]+step*f3[i];
01019
01020
01021
01022
01023
01024
01025
01026 Function(yt,f4);
01027
01028 h6 = step/6;
01029 for(i=0;i<6;i++) yout[i]=y[i]+h6*(dydx[i]+2*f2[i]+2*f3[i]+f4[i]);
01030
01031
01032
01033
01034
01035
01036
01037
01038
01039
01040
01041 }
01042
01043 #define PGROW -0.20
01044 #define PSHRNK -0.25
01045 #define FCOR 0.06666666 // 1.0/15.0
01046 #define SAFETY 0.9
01047 #define ERRCON 6.0e-4 // (4/SAFETY)^(1/PGROW)
01048 void TRunge::Propagate_QC(double y[6],double dydx[6],const double& steptry,
01049 const double& eps, const double yscal[6],
01050 double& stepdid, double& stepnext) const{
01051
01052 double ysav[6],dysav[6],ytemp[6];
01053 double h,hh,errmax,temp;
01054 unsigned i;
01055
01056 for(i=0;i<6;i++) ysav[i]=y[i];
01057
01058 for(i=0;i<6;i++) dysav[i]=dydx[i];
01059
01060
01061 h=steptry;
01062 for(;;){
01063 hh=h*0.5;
01064 Propagate1(ysav,dysav,hh,ytemp);
01065 Function(ytemp,dydx);
01066 Propagate1(ytemp,dydx,hh,y);
01067
01068 Propagate1(ysav,dysav,h,ytemp);
01069
01070 errmax=0.0;
01071 for(i=0;i<6;i++){
01072 ytemp[i]=y[i]-ytemp[i];
01073 temp=fabs(ytemp[i]/yscal[i]);
01074 if(errmax < temp) errmax=temp;
01075 }
01076
01077 errmax/=eps;
01078 if(errmax <= 1.0){
01079 stepdid=h;
01080 stepnext=(errmax>ERRCON ? SAFETY*h*exp(PGROW*log(errmax)) : 4.0*h);
01081 break;
01082 }
01083 h=SAFETY*h*exp(PSHRNK*log(errmax));
01084 }
01085 for(i=0;i<6;i++) y[i]+=ytemp[i]*FCOR;
01086
01087
01088
01089
01090
01091
01092
01093 }
01094
01095 #define TINY 1.0e-30
01096
01097 unsigned TRunge::Fly_SC(void) const{
01098 double y[6],dydx[6],yscal[6];
01099 unsigned Nstep;
01100 double step,stepdid,stepnext;
01101 unsigned i;
01102 double flightlength;
01103
01104
01105
01106
01107
01108
01109
01110
01111 step=default_stepSize0;
01112 flightlength=0;
01113 SetFirst(y);
01114 for(Nstep=0;Nstep<TRunge_MAXstep;Nstep++){
01115 for(i=0;i<6;i++) _y[Nstep][i]=y[i];
01116
01117
01118 Function(y,dydx);
01119
01120
01121 Propagate_QC(y,dydx,step,_eps,_yscal,stepdid,stepnext);
01122
01123 if( y[2]>160 || y[2]<-80 || (y[0]*y[0]+y[1]*y[1])>8100 ) break;
01124
01125
01126
01127 flightlength += stepdid;
01128 if(flightlength>_maxflightlength) break;
01129
01130 _h[Nstep]=stepdid;
01131
01132 if(stepnext<_stepSizeMin) step=_stepSizeMin;
01133 else if(stepnext>_stepSizeMax) step=_stepSizeMax;
01134 else step=stepnext;
01135 }
01136 _Nstep=Nstep+1;
01137
01138 return(_Nstep);
01139 }
01140
01141 double TRunge::SetFlightLength(void){
01142
01143
01144 const AList<TMLink>& cores=this->cores();
01145
01146
01147 const Helix hel(this->helix());
01148 double tanl=hel.tanl();
01149
01150
01151
01152 const double Bz = 1000*m_pmgnIMF->getReferField();
01153 double rho = 333.564095/(hel.kappa()*Bz);
01154
01155
01156 double phi_max=- DBL_MAX;
01157 double phi_min= DBL_MAX;
01158
01159 for(int j=0;j<cores.length();j++){
01160 TMLink& l=*cores[j];
01161
01162 const TMDCWire& wire=*l.wire();
01163 double Wz=0;
01164
01165 HepPoint3D onWire;
01166 HepPoint3D dummy_bP;
01167 HepVector3D dummy_dir;
01168 Helix th(hel);
01169 for(int i=0;i<10;i++){
01170 wire.wirePosition(Wz,onWire,dummy_bP,dummy_dir);
01171 th.pivot(onWire);
01172 if(abs(th.dz())<0.1){
01173
01174 break;
01175 }
01176 Wz+=th.dz();
01177 }
01178
01179
01180
01181
01182
01183
01184
01185
01186
01187
01188
01189
01190 const HepPoint3D & xc = hel.center();
01191 const HepPoint3D & xt = hel.x();
01192 HepVector3D v0, v1;
01193 v0 = xt - xc;
01194 v1 = th.x() - xc;
01195 const double vCrs = v0.x() * v1.y() - v0.y() * v1.x();
01196 const double vDot = v0.x() * v1.x() + v0.y() * v1.y();
01197 double phi = atan2(vCrs, vDot);
01198
01199 double tz=hel.x(phi).z();
01200
01201
01202 if(phi>phi_max) phi_max=phi;
01203 if(phi<phi_min) phi_min=phi;
01204
01205 }
01206
01207
01208
01209
01210 _maxflightlength=
01211 abs( rho*(phi_max-phi_min)*sqrt(1+tanl*tanl) )*1.2;
01212
01213 return(_maxflightlength);
01214 }
01215 double TRunge::DisToWire(TMLink& l,double z, HepPoint3D a, HepPoint3D& b){
01216 double zinter[400];
01217 double ddist=999;
01218 double finalz=0;
01219 const TMDCWire& w=*l.wire();
01220 HepPoint3D onwire;
01221
01222
01223 HepPoint3D point=w.xyPosition(z);
01224
01225 onwire.setX(point.x());
01226 onwire.setY(point.y());
01227
01228 onwire.setZ(z);
01229
01230 double dist=sqrt((point.x()-a.x())*(point.x()-a.x())+(point.y()-a.y())*(point.y()-a.y())+(z-a.z())*(z-a.z()));
01231 if(dist<ddist){
01232 ddist=dist;
01233
01234 b=onwire;
01235 }
01236
01237 return ddist;
01238 }
01239 double TRunge::dEpath(double mass, double path, double p)const{
01240 double z=4.72234;
01241 double a=9.29212;
01242 double Density=0.00085144;
01243 double coeff=Density*z/a;
01244 double i=51.4709;
01245 double isq=i * i * 1e-18;
01246
01247
01248
01249
01250
01251
01252
01253
01254
01255
01256
01257
01258
01259
01260 const double Me = 0.000510999;
01261 double psq = p * p;
01262 double bsq = psq / (psq + mass * mass);
01263 double esq = psq / (mass * mass);
01264
01265 double s = Me / mass;
01266 double w = (4 * Me * esq
01267 / (1 + 2 * s * sqrt(1 + esq)
01268 + s * s));
01269
01270
01271 double cc, x0;
01272 cc = 1+2*log(sqrt(isq)/(28.8E-09*sqrt(coeff)));
01273 if (cc < 5.215)
01274 x0 = 0.2;
01275 else
01276 x0 = 0.326*cc-1.5;
01277 double x1(3), xa(cc/4.606), aa;
01278 aa = 4.606*(xa-x0)/((x1-x0)*(x1-x0)*(x1-x0));
01279 double delta(0);
01280 double x(log10(sqrt(esq)));
01281 if (x > x0){
01282 delta = 4.606*x-cc;
01283 if (x < x1) delta=delta+aa*(x1-x)*(x1-x)*(x1-x);
01284 }
01285
01286
01287 float f1, f2, f3, f4, f5, ce;
01288 f1 = 1/esq;
01289 f2 = f1*f1;
01290 f3 = f1*f2;
01291 f4 = (f1*0.42237+f2*0.0304-f3*0.00038)*1E12;
01292 f5 = (f1*3.858-f2*0.1668+f3*0.00158)*1E18;
01293 ce = f4*isq+f5*isq*sqrt(isq);
01294
01295 return (0.0001535 * coeff / bsq
01296 * (log(Me * esq * w / isq)
01297 - 2 * bsq-delta-2.0*ce/z)) * path;
01298 }
01299 void TRunge::eloss(double path ,const RkFitMaterial * materiral, double mass,double y[6],int index)const{
01300 double psq=y[5]*y[5]+y[3]*y[3]+y[4]*y[4];
01301
01302 double p=sqrt(psq);
01303
01304 double dE=materiral->dE(mass,path,p);
01305 if (index > 0)
01306 psq += dE * (dE + 2 * sqrt(mass * mass + psq));
01307 else
01308 psq += dE * (dE - 2 * sqrt(mass * mass + psq));
01309 double pnew=sqrt(psq);
01310 double coeff=pnew/p;
01311
01312 y[3]=y[3]*coeff;
01313 y[4]=y[4]*coeff;
01314 y[5]=y[5]*coeff;
01315 }
01316 double TRunge::intersect_cylinder(double r) const {
01317 double m_rad = helix().radius();
01318 double l = helix().center().perp();
01319 double cosPhi = (m_rad * m_rad + l * l - r * r) / (2 * m_rad * l);
01320
01321 if(cosPhi < -1 || cosPhi > 1) return 0.;
01322
01323 double dPhi = helix().center().phi() - acos(cosPhi) - helix().phi0();
01324 if(dPhi < -M_PI) dPhi += 2 * M_PI;
01325
01326 return dPhi;
01327 }
01328 double TRunge::intersect_zx_plane(const HepTransform3D& plane, double y) const {
01329 HepPoint3D xc = plane * helix().center();
01330 double r = helix().radius();
01331 double d = r * r - (y - xc.y()) * (y - xc.y());
01332 if(d < 0) return 0;
01333
01334 double xx = xc.x();
01335 if(xx > 0) xx -= sqrt(d);
01336 else xx += sqrt(d);
01337
01338 double l = (plane.inverse() *
01339 HepPoint3D(xx, y, 0)).perp();
01340
01341 return intersect_cylinder(l);
01342 }
01343
01344
01345 double TRunge::intersect_yz_plane(const HepTransform3D& plane, double x) const {
01346 HepPoint3D xc = plane * helix().center();
01347 double r = helix().radius();
01348 double d = r * r - (x - xc.x()) * (x - xc.x());
01349 if(d < 0) return 0;
01350
01351 double yy = xc.y();
01352 if(yy > 0) yy -= sqrt(d);
01353 else yy += sqrt(d);
01354
01355 double l = (plane.inverse() *
01356 HepPoint3D(x, yy, 0)).perp();
01357
01358 return intersect_cylinder(l);
01359 }
01360
01361
01362 double TRunge::intersect_xy_plane(double z) const {
01363 if (helix().tanl() != 0 && helix().radius() != 0)
01364 return (helix().pivot().z() + helix().dz() - z) / (helix().radius() * helix().tanl());
01365 else return 0;
01366 }