TRunge.cxx

Go to the documentation of this file.

//-----------------------------------------------------------------------------
// $Id: TRunge.cxx,v 1.38 2012/05/28 05:16:29 maoh Exp $
//-----------------------------------------------------------------------------
// Filename : TRunge.cc
// Section  : Tracking
// Owner    : Kenji Inami
// Email    : [email protected]
//-----------------------------------------------------------------------------
// Description : A class to represent a track using Runge Kutta method
//               See http://bsunsrv1.kek.jp/~yiwasaki/tracking/
//-----------------------------------------------------------------------------
 
#include <float.h>
#include <string.h>
#include "TrkReco/TRunge.h"
#include "TrkReco/TRungeFitter.h"
#include "TrkReco/TMDCWire.h"
#include "TrkReco/TTrack.h"
 
#include "CLHEP/Matrix/Matrix.h"
#include "GaudiKernel/StatusCode.h"
#include "GaudiKernel/IInterface.h"
#include "GaudiKernel/Kernel.h"
#include "GaudiKernel/Service.h"
#include "GaudiKernel/ISvcLocator.h"
#include "GaudiKernel/SvcFactory.h"
#include "GaudiKernel/IDataProviderSvc.h"
#include "GaudiKernel/Bootstrap.h"
#include "GaudiKernel/MsgStream.h"
#include "GaudiKernel/SmartDataPtr.h"
#include "GaudiKernel/IMessageSvc.h"
typedef HepGeom::Transform3D  HepTransform3D;
 
//const double alpha2 = 333.5640952;  //(cm Tesla /(GeV/c))
const TRungeFitter TRunge::_fitter = TRungeFitter("TRunge Default fitter");
 
//const double default_stepSize = 0;
const double default_stepSize = 1.5;    //cm
const double default_stepSize0 = 1.5;   //cm
const double default_stepSizeMax =5;    //cm
const double default_stepSizeMin = 0.001;//cm
 
const double EPS = 1.0e-6;
 
const double default_maxflightlength = 1000; //10m
 
TRunge::TRunge()
  : TTrackBase(),
    _pivot(ORIGIN),_a(Vector(5,0)),_Ea(SymMatrix(5,0)), 
    _chi2(0),_ndf(0),_charge(1),_bfieldID(21),_Nstep(0),
    _stepSize(default_stepSize),_mass(0.140),_eps(EPS),
    _stepSizeMax(default_stepSizeMax),_stepSizeMin(default_stepSizeMin),
    _maxflightlength(default_maxflightlength) {
 
  //...Set a default fitter...
  fitter(& TRunge::_fitter);
 
  _fitted = false;
  _fittedWithCathode = false;
 
  _bfield = Bfield::getBfield(_bfieldID);
 
  _mass2=_mass*_mass;
 
  _yscal[0]=0.1; //1mm  -> error = 1mm*EPS
  _yscal[1]=0.1; //1mm
  _yscal[2]=0.1; //1mm
  _yscal[3]=0.001; //1MeV
  _yscal[4]=0.001; //1MeV
  _yscal[5]=0.001; //1MeV
  //jialk
  StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF); 
  if(scmgn!=StatusCode::SUCCESS) { 
    std::cout<< __FILE__<<" Unable to open Magnetic field service"<<std::endl;
  }
 
}
 
TRunge::TRunge(const RecMdcTrack& a)
  : TTrackBase(),
  _pivot(a.getPivot()),_a(a.helix()),_Ea(a.err()),
  _chi2(a.chi2()),_ndf(a.ndof()),_bfieldID(21),_Nstep(0),
  _stepSize(default_stepSize),_mass(0.140),_eps(EPS),
  _stepSizeMax(default_stepSizeMax),_stepSizeMin(default_stepSizeMin),
  _maxflightlength(default_maxflightlength) {
//    _a[2]=-_a[2];
    //...Set a default fitter...
    fitter(& TRunge::_fitter);
 
    _fitted = false;
    _fittedWithCathode = false;
 
    if(_a[2]<0) _charge=-1;
    else        _charge=1;
 
    _bfield = Bfield::getBfield(_bfieldID);
 
    _mass2=_mass*_mass;
 
    _yscal[0]=0.1; //1mm  -> error = 1mm*EPS
    _yscal[1]=0.1; //1mm
    _yscal[2]=0.1; //1mm
    _yscal[3]=0.001; //1MeV
    _yscal[4]=0.001; //1MeV
    _yscal[5]=0.001; //1MeV
    StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF);
    if(scmgn!=StatusCode::SUCCESS) {
      std::cout<< "Unable to open Magnetic field service"<<std::endl;
    }
    //SetFlightLength();
    //cout<<"TR:: _maxflightlength="<<_maxflightlength<<endl;
 
  }
TRunge::TRunge(const TTrack& a)
  : TTrackBase(a),
    _pivot(a.helix().pivot()),_a(a.helix().a()),_Ea(a.helix().Ea()),
    _chi2(0),_ndf(0),_bfieldID(21),_Nstep(0),
    _stepSize(default_stepSize),_mass(0.140),_eps(EPS),
    _stepSizeMax(default_stepSizeMax),_stepSizeMin(default_stepSizeMin),
    _maxflightlength(default_maxflightlength) {
//  _a[2]=-_a[2];
  //...Set a default fitter...
  fitter(& TRunge::_fitter);
 
  _fitted = false;
  _fittedWithCathode = false;
 
  if(_a[2]<0) _charge=-1;
  else        _charge=1;
 
  _bfield = Bfield::getBfield(_bfieldID);
 
  _mass2=_mass*_mass;
 
  _yscal[0]=0.1; //1mm  -> error = 1mm*EPS
  _yscal[1]=0.1; //1mm
  _yscal[2]=0.1; //1mm
  _yscal[3]=0.001; //1MeV
  _yscal[4]=0.001; //1MeV
  _yscal[5]=0.001; //1MeV
  StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF);
  if(scmgn!=StatusCode::SUCCESS) {
          std::cout<< "Unable to open Magnetic field service"<<std::endl;
  }
  //SetFlightLength();
  //cout<<"TR:: _maxflightlength="<<_maxflightlength<<endl;
}
 
TRunge::TRunge(const Helix & h)
  : TTrackBase(),
    _pivot(h.pivot()),_a(h.a()),_Ea(h.Ea()),
    _chi2(0),_ndf(0),_bfieldID(21),_Nstep(0),
    _stepSize(default_stepSize),_mass(0.140),_eps(EPS),
    _stepSizeMax(default_stepSizeMax),_stepSizeMin(default_stepSizeMin),
    _maxflightlength(default_maxflightlength) {
 
  //...Set a default fitter...
  fitter(& TRunge::_fitter);
 
  _fitted = false;
  _fittedWithCathode = false;
 
  if(_a[2]<0) _charge=-1;
  else        _charge=1;
 
  _bfield = Bfield::getBfield(_bfieldID);
 
  _mass2=_mass*_mass;
 
  _yscal[0]=0.1; //1mm  -> error = 1mm*EPS
  _yscal[1]=0.1; //1mm
  _yscal[2]=0.1; //1mm
  _yscal[3]=0.001; //1MeV
  _yscal[4]=0.001; //1MeV
  _yscal[5]=0.001; //1MeV
}
 
TRunge::TRunge(const TRunge& a)
  : TTrackBase((TTrackBase &) a),
    _pivot(a.pivot()),_a(a.a()),_Ea(a.Ea()),
    _chi2(a.chi2()),_ndf(a.ndf()),_bfieldID(a.BfieldID()),_Nstep(0),
    _stepSize(a.StepSize()),_mass(a.Mass()),_eps(a.Eps()),
    _stepSizeMax(a.StepSizeMax()),_stepSizeMin(a.StepSizeMin()),
    _maxflightlength(a.MaxFlightLength()) {
 
  //...Set a default fitter...
  fitter(& TRunge::_fitter);
 
  _fitted = false;
  _fittedWithCathode = false;
 
  if(_a[2]<0) _charge=-1;
  else        _charge=1;
 
  _bfield = Bfield::getBfield(_bfieldID);
 
  _mass2=_mass*_mass;
 
  for(unsigned i=0;i<6;i++) _yscal[i]=a.Yscal()[i];
}
 
//destructor
TRunge::~TRunge() {
}
 
double TRunge::dr(void) const{
  return(_a[0]);
}
 
double TRunge::phi0(void) const{
  return(_a[1]);
}
 
double TRunge::kappa(void) const{
  return(_a[2]);
}
 
double TRunge::dz(void) const{
  return(_a[3]);
}
 
double TRunge::tanl(void) const{
  return(_a[4]);
}
 
const HepPoint3D& TRunge::pivot(void) const{
  return(_pivot);
}
 
const Vector& TRunge::a(void) const{
  return(_a);
}
 
const SymMatrix& TRunge::Ea(void) const{
  return(_Ea);
}
 
Helix TRunge::helix(void) const{
  return(Helix(_pivot,_a,_Ea));
}
 
unsigned TRunge::ndf(void) const{
  return _ndf;
}
 
double TRunge::chi2(void) const{
  return _chi2;
}
 
double TRunge::reducedchi2(void) const{
  if(_ndf==0){
    std::cout<<"error at TRunge::reducedchi2  ndf=0"<<std::endl;
    return 0;
  }
  return (_chi2/_ndf);
}
 
int TRunge::BfieldID(void) const{
  return(_bfieldID);
}
 
double TRunge::StepSize(void) const{
  return(_stepSize);
}
 
const double* TRunge::Yscal(void) const{
  return(_yscal);
}
double TRunge::Eps(void) const{
  return(_eps);
}
double TRunge::StepSizeMax(void) const{
  return(_stepSizeMax);
}
double TRunge::StepSizeMin(void) const{
  return(_stepSizeMin);
}
 
float TRunge::Mass(void) const{
  return(_mass);
}
 
double TRunge::MaxFlightLength(void) const{
  return(_maxflightlength);
}
 
const HepPoint3D& TRunge::pivot(const HepPoint3D& newpivot){
  /// !!!!! under construction !!!!!
  ///   track parameter should be extracted after track propagation.
  Helix tHelix(helix());
  tHelix.pivot(newpivot);
  _pivot=newpivot;
  _a=tHelix.a();
  _Ea=tHelix.Ea();
  _Nstep=0;
  return(_pivot);
}
 
const Vector& TRunge::a(const Vector& ta){
  _a=ta;
  if(_a[2]<0) _charge=-1;
  else        _charge=1;
  _Nstep=0;
  return(_a);
}
 
const SymMatrix& TRunge::Ea(const SymMatrix& tEa){
  _Ea=tEa;
  _Nstep=0;
  return(_Ea);
}
 
int TRunge::BfieldID(int id){
  _bfieldID=id;
  _bfield = Bfield::getBfield(_bfieldID);
  _Nstep=0;
  return(_bfieldID);
}
 
double TRunge::StepSize(double step){
  _stepSize=step;
  _Nstep=0;
  return(_stepSize);
}
 
const double* TRunge::Yscal(const double y[6]){
  for(unsigned i=0;i<6;i++) _yscal[i]=y[i];
  _Nstep=0;
  return(_yscal);
}
double TRunge::Eps(double eps){
  _eps=eps;
  _Nstep=0;
  return(_eps);
}
double TRunge::StepSizeMax(double step){
  _stepSizeMax=step;
  _Nstep=0;
  return(_stepSizeMax);
}
double TRunge::StepSizeMin(double step){
  _stepSizeMin=step;
  _Nstep=0;
  return(_stepSizeMin);
}
 
float TRunge::Mass(float mass){
  _mass=mass;
  _mass2=_mass*_mass;
  return(_mass);
}
 
double TRunge::MaxFlightLength(double length){
  if(length>0) _maxflightlength=length;
  else _maxflightlength=default_maxflightlength;
  _Nstep=0;
  return(_maxflightlength);
}
 
int TRunge::approach(TMLink& l,const RkFitMaterial  material,bool doSagCorrection) {
  float tof;
  HepVector3D p;
  return(approach(l,tof,p,material,doSagCorrection));
}
 
int TRunge::approach(TMLink& l,float& tof,HepVector3D& p,
             const RkFitMaterial  material,bool doSagCorrection) {
  HepPoint3D xw;
  HepPoint3D wireBackwardPosition;
  HepVector3D v;
  HepPoint3D onWire,onTrack;
  double onWire_x,onWire_y, onWire_z, zwf, zwb;
  const TMDCWire& w=*l.wire();
  xw = w.xyPosition();
  wireBackwardPosition = w.backwardPosition();
  v = w.direction();
 
  unsigned stepNum=0;
  if(approach_line(l,wireBackwardPosition,v,onWire,onTrack,tof,p,material,stepNum)<0){
    //cout<<"TR::error approach_line"<<endl;
    return(-1);
  }
  zwf = w.forwardPosition().z();
  zwb = w.backwardPosition().z();
  // protection for strange wire hit info. (Only 'onWire' is corrected)
  if(onWire.z() > zwf) 
    w.wirePosition(zwf,onWire,wireBackwardPosition,(HepVector3D&)v);
  else if(onWire.z() < zwb) 
    w.wirePosition(zwb,onWire,wireBackwardPosition,(HepVector3D&)v);
 
  // onWire,onTrack filled
  
  if(!doSagCorrection){
    l.positionOnWire(onWire);
    l.positionOnTrack(onTrack);
    double phipoint=atan((onTrack.y()-_pivot.y())/onTrack.x()-_pivot.x());
//    cout<<"dphi track:"<<l.dPhi()<<endl;
 //   cout<<"dphi rk:"<<phipoint-_a[0]<<endl;
//    l.dPhi(phipoint-_a[0]);
    return(0);              // no sag correction
  }
  // Sag correction
  //   loop for sag correction
  onWire_y = onWire.y();
  onWire_z = onWire.z();
 
  unsigned nTrial = 1;
  while(nTrial<100){
    //cout<<"TR: nTrial "<<nTrial<<endl;
    w.wirePosition(onWire_z,xw,wireBackwardPosition,(HepVector3D&)v);
    if(approach_line(l,wireBackwardPosition,v,onWire,onTrack,tof,p,material,stepNum)<0)
      return(-1);
    if(fabs(onWire_y - onWire.y())<0.0001) break;   // |dy|< 1 micron
    onWire_y = onWire.y();
    onWire_z += (onWire.z()-onWire_z)/2;
    // protection for strange wire hit info.
    if(onWire_z > zwf)      onWire_z=zwf;
    else if(onWire_z < zwb) onWire_z=zwb;
 
    nTrial++;
  }
  //  cout<<"TR  nTrial="<<nTrial<<endl;
  l.positionOnWire(onWire);
  l.positionOnTrack(onTrack);
  return(nTrial);
}
 
int TRunge::approach_line(TMLink& l,const HepPoint3D& w0,const HepVector3D& v,
              HepPoint3D& onLine,HepPoint3D& onTrack,const RkFitMaterial  material) {
  float tof;
  HepVector3D p;
  return(approach_line(l,w0,v,onLine,onTrack,tof,p,material));
}
 
int TRunge::approach_line(TMLink& l,const HepPoint3D& w0,const HepVector3D& v,
              HepPoint3D& onLine,HepPoint3D& onTrack,
              float& tof,HepVector3D& p,const RkFitMaterial  material) {
  unsigned stepNum=0;
  return(approach_line(l,w0,v,onLine,onTrack,tof,p,material ,stepNum));
}
 
int TRunge::approach_line(TMLink& l,const HepPoint3D& w0,const HepVector3D& v,
              HepPoint3D& onLine,HepPoint3D& onTrack,
              float& tof,HepVector3D& p ,const RkFitMaterial  material,unsigned& stepNum) {
  //  line = [w0] + t * [v]    -> [onLine]
  if(_Nstep==0){
    if(_stepSize==0) Fly_SC();
    else Fly(material);
 
  }
  //cout<<"TR::approach_line stepNum="<<stepNum<<endl;
 
  const double w0x = w0.x();
  const double w0y = w0.y();
  const double w0z = w0.z();
  //cout<<"TR::line w0="<<w0x<<","<<w0y<<","<<w0z<<endl;
  const double vx = v.x();
  const double vy = v.y();
  const double vz = v.z();
  const double v_2 = vx*vx+vy*vy+vz*vz;
 
  const float clight=29.9792458; //[cm/ns]   
  //const float M2=_mass*_mass;
  //const float& M2=_mass2;
  const float p2 = _y[0][3]*_y[0][3]+_y[0][4]*_y[0][4]+_y[0][5]*_y[0][5];
  const float tof_factor=1./clight*sqrt(1+_mass2/p2);
 
  // search for the closest point in cache   (point - line)
  //  unsigned stepNum;
  double l2_old= DBL_MAX;
  if(stepNum>_Nstep) stepNum=0;
  unsigned stepNumLo;
  unsigned stepNumHi;
  if(stepNum==0 && _stepSize!=0){ // skip
    const double dx = _y[0][0]-w0x;
    const double dy = _y[0][1]-w0y;
    stepNum=(unsigned)
      (sqrt( (dx*dx+dy*dy)*(1+_a[4]*_a[4]) )/_stepSize);
  }
  unsigned mergin;
  if(_stepSize==0){
    mergin=10;  //10 step back
  }else{
    mergin=(unsigned)(1.0/_stepSize);   //1mm back
  }
  if(stepNum>mergin) stepNum-=mergin;
  else           stepNum=0;
  if(stepNum>=_Nstep) stepNum=_Nstep-1;
  // hunt
  //  unsigned inc=1;
  unsigned inc=(mergin>>1)+1;
  stepNumLo=stepNum;
  stepNumHi=stepNum;
  for(;;){
    const double dx = _y[stepNumHi][0]-w0x;
    const double dy = _y[stepNumHi][1]-w0y;
    const double dz = _y[stepNumHi][2]-w0z;
    const double t = (dx*vx+dy*vy+dz*vz)/v_2;
//    const double l2 = (dx*dx+dy*dy+dz*dz)-(t*t)/v_2;
    HepVector3D zvec;
    zvec.setX(0);
    zvec.setY(0);
    zvec.setZ(1);
    HepVector3D hitv=t*v;
    HepPoint3D pp,onw;
    pp.setX(_y[stepNumHi][0]);
    pp.setY(_y[stepNumHi][1]);
    pp.setZ(_y[stepNumHi][2]); 
    double zwire=hitv.dot(zvec)+w0z;
    double dtl=DisToWire(l,zwire,pp,onw);
    const double l2=dtl*dtl;      
    if(l2 > l2_old) break;
    l2_old=l2;
    stepNumLo=stepNumHi;
    //    inc+=inc;
    stepNumHi+=inc;
    if(stepNumHi>=_Nstep){
      stepNumHi=_Nstep;
      break;
    }
  }
  // locate (2-bun-hou, bisection method)
  while(stepNumHi-stepNumLo>1){
    unsigned j=(stepNumHi+stepNumLo)>>1;
    const double dx = _y[j][0]-w0x;
    const double dy = _y[j][1]-w0y;
    const double dz = _y[j][2]-w0z;
    const double t = (dx*vx+dy*vy+dz*vz)/v_2;
//    const double l2 = (dx*dx+dy*dy+dz*dz)-(t*t)/v_2;
    HepVector3D zv;
    zv.setX(0);
    zv.setY(0);
    zv.setZ(1);
    HepVector3D hitv=t*v;
    HepPoint3D point,onwir;
    point.setX(_y[j][0]);
    point.setY(_y[j][1]);
    point.setZ(_y[j][2]);
    double zwir=hitv.dot(zv)+w0z;
    double dtll=DisToWire(l,zwir,point,onwir);
    const double l2 =dtll*dtll;
    if(l2 > l2_old){
      stepNumHi=j;
    }else{
      l2_old=l2;
      stepNumLo=j;
    }
  }
  //stepNum=stepNumHi;
  stepNum=stepNumLo;
  /*
  for(;stepNum<_Nstep;stepNum++){
    const double dx = _y[stepNum][0]-w0x;
    const double dy = _y[stepNum][1]-w0y;
    const double dz = _y[stepNum][2]-w0z;
    const double t = (dx*vx+dy*vy+dz*vz)/v_2;
    const double l2 = (dx*dx+dy*dy+dz*dz)-(t*t)/v_2;
    if(l2 > l2_old) break;
    l2_old=l2;
    //    const float p2 = _y[stepNum][3]*_y[stepNum][3]+
    //                      _y[stepNum][4]*_y[stepNum][4]+
    //                      _y[stepNum][5]*_y[stepNum][5];
    //    tof+=_stepSize/clight*sqrt(1+M2/p2);
  }
  */
  //  cout<<"TR  stepNum="<<stepNum<<endl;
  //if(stepNum>=_Nstep) return(-1); // not found
  //stepNum--;
  if(_stepSize==0){
    double dstep=0;
    for(unsigned i=0;i<stepNum;i++) dstep+=_h[i];
    tof=dstep*tof_factor;
  }else{
    tof=stepNum*_stepSize*tof_factor;
  }
 
  // propagate the track and search for the closest point
  //2-bun-hou (bisection method)
  /*
  double y[6],y_old[6];
  for(unsigned i=0;i<6;i++) y[i]=_y[stepNum][i];
  double step=_stepSize;
  double step2=0;
  for(;;){
    for(unsigned i=0;i<6;i++) y_old[i]=y[i];
    Propagate(y,step);
    const double dx = y[0]-w0x;
    const double dy = y[1]-w0y;
    const double dz = y[2]-w0z;
    const double t = (dx*vx+dy*vy+dz*vz)/v_2;
    const double l2 = (dx*dx+dy*dy+dz*dz)-(t*t)/v_2;
    if(l2 > l2_old){  // back
      for(unsigned i=0;i<6;i++) y[i]=y_old[i];
    }else{        // propagate
      l2_old=l2;
      //      const float p2=y[3]*y[3]+y[4]*y[4]+y[5]*y[5];
      //      tof+=step/clight*sqrt(1+M2/p2);
      step2+=step;
    }
    step/=2;
    if(step < 0.0001) break;    // step < 1 [um]
  }
  */
  // Hasamiuchi-Hou (false position method)
  /*
  double y[6],y1[6],y2[6];
  for(unsigned i=0;i<6;i++) y1[i]=_y[stepNum][i];
  for(unsigned i=0;i<6;i++) y2[i]=_y[stepNum+2][i];
  double minStep=0;
  double maxStep=_stepSize*2;
  double step2=0;
  const double A[3][3]={{1-vx*vx/v_2, -vx*vy/v_2, -vx*vz/v_2},
            { -vy*vx/v_2,1-vy*vy/v_2, -vy*vz/v_2},
            { -vz*vx/v_2, -vz*vy/v_2,1-vz*vz/v_2}};
 
  double Y1=0;
  {
    const double dx = y1[0]-w0x;
    const double dy = y1[1]-w0y;
    const double dz = y1[2]-w0z;
    const double t = (dx*vx+dy*vy+dz*vz)/v_2;
    const double d[3]={dx-t*vx,dy-t*vy,dz-t*vz};
    const double l = sqrt( (dx*dx+dy*dy+dz*dz)-(t*t)/v_2 );
    const double pmag=sqrt(y1[3]*y1[3]+y1[4]*y1[4]+y1[5]*y1[5]);
    for(int j=0;j<3;j++){
      double g=0;
      for(int k=0;k<3;k++) g+=A[j][k]*d[k];
      Y1+=y1[j+3]*g;
    }
    Y1=Y1/pmag/l;
  }
  double Y2=0;
  {
    const double dx = y2[0]-w0x;
    const double dy = y2[1]-w0y;
    const double dz = y2[2]-w0z;
    const double t = (dx*vx+dy*vy+dz*vz)/v_2;
    const double d[3]={dx-t*vx,dy-t*vy,dz-t*vz};
    const double l = sqrt( (dx*dx+dy*dy+dz*dz)-(t*t)/v_2 );
    const double pmag=sqrt(y2[3]*y2[3]+y2[4]*y2[4]+y2[5]*y2[5]);
    for(int j=0;j<3;j++){
      double g=0;
      for(int k=0;k<3;k++) g+=A[j][k]*d[k];
      Y2+=y2[j+3]*g;
    }
    Y2=Y2/pmag/l;
  }
  if(Y1*Y2>=0) cout<<"TR  fatal error!"<<endl;
  double step_old= DBL_MAX;
  for(;;){
    const double step=(minStep*Y2-maxStep*Y1)/(Y2-Y1);
    if(fabs(step-step_old)<0.0001) break;
    step_old=step;
    for(unsigned i=0;i<6;i++) y[i]=y1[i];
    Propagate(y,step-minStep);
    double Y=0;
    {
      const double dx = y[0]-w0x;
      const double dy = y[1]-w0y;
      const double dz = y[2]-w0z;
      const double t = (dx*vx+dy*vy+dz*vz)/v_2;
      const double d[3]={dx-t*vx,dy-t*vy,dz-t*vz};
      const double l = sqrt( (dx*dx+dy*dy+dz*dz)-(t*t)/v_2 );
      const double pmag=sqrt(y[3]*y[3]+y[4]*y[4]+y[5]*y[5]);
      for(int j=0;j<3;j++){
    double g=0;
    for(int k=0;k<3;k++) g+=A[j][k]*d[k];
    Y+=y[j+3]*g;
      }
      Y=Y/pmag/l;
    }
    if(Y1*Y<0){ //Y->Y2
      Y2=Y;
      for(unsigned i=0;i<6;i++) y2[i]=y[i];
      maxStep=step;
    }else{  //Y->Y1
      Y1=Y;
      for(unsigned i=0;i<6;i++) y1[i]=y[i];
      minStep=step;
    }
    if(Y1==Y2) break;
  }
  step2=step_old;
  */
  // Newton method
  double y[6];
  for(unsigned i=0;i<6;i++) y[i]=_y[stepNum][i];
  //memcpy(y,_y[stepNum],sizeof(double)*6);
  double step2=0;
  const double A[3][3]={{1-vx*vx/v_2, -vx*vy/v_2, -vx*vz/v_2},
            { -vy*vx/v_2,1-vy*vy/v_2, -vy*vz/v_2},
            { -vz*vx/v_2, -vz*vy/v_2,1-vz*vz/v_2}};
  double factor=1;
  double g[3];
  double f[6];
  double Af[3],Af2[3];
  unsigned i,j,k;
  HepPoint3D point,onwir;
  for(i=0;i<10;i++){
    //cout<<"TR::line "<<y[0]<<","<<y[1]<<","<<y[2]<<","<<y[3]<<","<<y[4]<<","<<y[5]<<endl;
    const double dx = y[0]-w0x;
    const double dy = y[1]-w0y;
    const double dz = y[2]-w0z;
    const double t = (dx*vx+dy*vy+dz*vz)/v_2;
    const double d[3]={dx-t*vx,dy-t*vy,dz-t*vz};
   // const double l2 = (dx*dx+dy*dy+dz*dz)-(t*t)/v_2;
    HepVector3D zv;
    zv.setX(0);
    zv.setY(0);
    zv.setZ(1);
    HepVector3D hitv=t*v;
    point.setX(y[0]);
    point.setY(y[1]);
    point.setZ(y[2]);
    double zwir=hitv.dot(zv)+w0z;
    double dtll=DisToWire(l,zwir,point,onwir);
    const double l2=dtll*dtll;
    for(j=0;j<3;j++){
      g[j]=0;
      for(k=0;k<3;k++) g[j]+=A[j][k]*d[k];
    }
    //cout<<"g="<<g[0]<<","<<g[1]<<","<<g[2]<<endl;
    Function(y,f);
    //cout<<"f="<<f[0]<<","<<f[1]<<","<<f[2]<<","<<f[3]<<","<<f[4]<<","<<f[5]<<endl;
    //cout<<"A="<<A[0][0]<<","<<A[0][1]<<","<<A[0][2]<<endl;
    //cout<<"A="<<A[1][0]<<","<<A[1][1]<<","<<A[1][2]<<endl;
    //cout<<"A="<<A[2][0]<<","<<A[2][1]<<","<<A[2][2]<<endl;
    double Y=0;
    for(j=0;j<3;j++) Y+=y[j+3]*g[j];
    double dYds=0;
    for(j=0;j<3;j++) dYds+=f[j+3]*g[j];
    //cout<<"dYds="<<dYds<<endl;
    for(j=0;j<3;j++){
      Af[j]=0;
      Af2[j]=0;
    }
    for(j=0;j<3;j++){
      //Af[j]=0;
      //Af2[j]=0;
      for(k=0;k<3;k++) Af[j]+=(A[j][k]*f[k]);
      for(k=0;k<3;k++) Af2[j]+=(A[j][k]*Af[k]);
      dYds+=y[j+3]*Af2[j];
      //cout<<j<<" dYds="<<dYds<<endl;
    }
    //cout<<"dYds="<<dYds<<endl;
    const double step=-Y/dYds*factor;
    //cout<<"TR  step="<<step<<" i="<<i<<endl;
    if(fabs(step) < 0.00001) break; // step < 1 [um]
    if(l2 > l2_old) factor/=2;
    l2_old=l2;
    Propagate(y,step,material);
    step2+=step;
  }
 
  tof+=step2*tof_factor;
 
  onTrack.setX(y[0]);
  onTrack.setY(y[1]);
  onTrack.setZ(y[2]);
  p.setX(y[3]);
  p.setY(y[4]);
  p.setZ(y[5]);
 
  const double dx = y[0]-w0x;
  const double dy = y[1]-w0y;
  const double dz = y[2]-w0z;
  const double s = (dx*vx+dy*vy+dz*vz)/v_2;
 // onLine.setX(w0x+s*vx);
 // onLine.setY(w0y+s*vy);
 // onLine.setZ(w0z+s*vz);
  onLine.setX(onwir.x());
  onLine.setY(onwir.y());
  onLine.setZ(onwir.z());
  return(0);
}
 
int TRunge::approach_point(TMLink& l,const HepPoint3D& p0,HepPoint3D& onTrack,const RkFitMaterial  material) const{
  if(_Nstep==0){
    if(_stepSize==0) Fly_SC();
    else Fly(material);
  }
 
  double x0=p0.x();
  double y0=p0.y();
  double z0=p0.z();
 
  //tof=0;
  //const float clight=29.9792458; //[cm/ns]   
  //const float M2=_mass*_mass;
 
  // search for the closest point in cache
  unsigned stepNum;
  double l2_old= DBL_MAX;
  for(stepNum=0;stepNum<_Nstep;stepNum++){
    double l2=(_y[stepNum][0]-x0)*(_y[stepNum][0]-x0)+
             (_y[stepNum][1]-y0)*(_y[stepNum][1]-y0)+
             (_y[stepNum][2]-z0)*(_y[stepNum][2]-z0);
    if(l2 > l2_old) break;
    l2_old=l2;
    //const double p2 = _y[stepNum][3]*_y[stepNum][3]+
    //                  _y[stepNum][4]*_y[stepNum][4]+
    //                  _y[stepNum][5]*_y[stepNum][5];
    //tof+=_stepSize/clight*sqrt(1+M2/p2);
  }
  if(stepNum>=_Nstep) return(-1);   // not found
  stepNum--;
 
  // propagate the track and search for the closest point
  double y[6],y_old[6];
  for(unsigned i=0;i<6;i++) y[i]=_y[stepNum][i];
  double step=_stepSize;
  for(;;){
    for(unsigned i=0;i<6;i++) y_old[i]=y[i];
    Propagate(y,step,material);
    double l2=(y[0]-x0)*(y[0]-x0)+(y[1]-y0)*(y[1]-y0)+(y[2]-z0)*(y[2]-z0);
    if(l2 > l2_old){  // back
      for(unsigned i=0;i<6;i++) y[i]=y_old[i];
    }else{        // propagate
      l2_old=l2;
      //const double p2=y[3]*y[3]+y[4]*y[4]+y[5]*y[5];
      //tof+=step/clight*sqrt(1+M2/p2);
    }
    step/=2;
    if(step < 0.0001) break;    // step < 1 [um]
  }
 
  onTrack.setX(y[0]);
  onTrack.setY(y[1]);
  onTrack.setZ(y[2]);
  //p.setX(y[3]);
  //p.setY(y[4]);
  //p.setZ(y[5]);
  return(0);
}
 
unsigned TRunge::Fly(const RkFitMaterial  material) const{
  double y[6];
  unsigned Nstep;
  double flightlength;
 
  flightlength=0;
  SetFirst(y);
  for(Nstep=0;Nstep<TRunge_MAXstep;Nstep++){
    for(unsigned j=0;j<6;j++) _y[Nstep][j]=y[j];
    //memcpy(_y[Nstep],y,sizeof(double)*6);
 
    Propagate(y,_stepSize,material);
 
    if( y[2]>160 || y[2]<-80 || (y[0]*y[0]+y[1]*y[1])>8100 ) break;
    //if position is out side of MDC, stop to fly
    //  R>90cm, z<-80cm,160cm<z
 
    flightlength += _stepSize;
    if(flightlength>_maxflightlength) break;
 
    _h[Nstep]=_stepSize;
  }
  _Nstep=Nstep+1;
  return(_Nstep);
}
 
void TRunge::Propagate(double y[6],const double& step,const RkFitMaterial  material) const{
  //  y[6] = (x,y,z,px,py,pz)
  double f1[6],f2[6],f3[6],f4[6],yt[6];
  double hh;
  double h6;
  unsigned i;
  const double mpi=0.13957;
  double me=0.000511;
  //eloss(step,&material, me,y,0);
  hh = step*0.5;
  //cout<<"TR:Pro hh="<<hh<<endl;
  Function(y,f1);
  for(i=0;i<6;i++) yt[i]=y[i]+hh*f1[i];
  //{
  //  register double *a=y;
  //  register double *b=f1;
  //  register double *t=yt;
  //  register double *e=y+6;
  //  for(;a<e;a++,b++,t++) (*t)=(*a)+hh*(*b);
  //}
  Function(yt,f2);
  for(i=0;i<6;i++) yt[i]=y[i]+hh*f2[i];
  //{
  //  register double *a=y;
  //  register double *b=f2;
  //  register double *t=yt;
  //  register double *e=y+6;
  //  for(;a<e;a++,b++,t++) (*t)=(*a)+hh*(*b);
  //}
  Function(yt,f3);
  for(i=0;i<6;i++) yt[i]=y[i]+step*f3[i];
  //{
  //  register double *a=y;
  //  register double *b=f3;
  //  register double *t=yt;
  //  register double *e=y+6;
  //  for(;a<e;a++,b++,t++) (*t)=(*a)+step*(*b);
  //}
  Function(yt,f4);
 
  h6 = step/6;
  for(i=0;i<6;i++) y[i]+=h6*(f1[i]+2*f2[i]+2*f3[i]+f4[i]);
  //{
  //  register double *a=f1;
  //  register double *b=f2;
  //  register double *c=f3;
  //  register double *d=f4;
  //  register double *t=y;
  //  register double *e=y+6;
  //  for(;t<e;a++,b++,c++,d++,t++)
  //    (*t)+=h6*((*a)+2*(*b)+2*(*c)+(*d));
  //}
}
 
void TRunge::Function(const double y[6],double f[6]) const{
  // return the value of formula
  //  y[6] = (x,y,z,px,py,pz)
  //  f[6] = ( dx[3]/ds, dp[3]/ds )
  //  dx/ds = p/|p|
  //  dp/ds = e/|e| 1/alpha (p x B)/|p||B|
  //    alpha = 1/cB = 333.5640952/B[Tesla]  [cm/(GeV/c)]
  //  const float Bx = _bfield->bx(y[0],y[1],y[2]);
  //  const float By = _bfield->by(y[0],y[1],y[2]);
  //  const float Bz = _bfield->bz(y[0],y[1],y[2]);  //[kGauss]
  double pmag;
  double factor;
  HepVector3D B;
  HepPoint3D pos;
  pos.setX((float)y[0]);
  pos.setY((float)y[1]);
  pos.setZ((float)y[2]);
  //cout<<"TR::pos="<<pos[0]<<","<<pos[1]<<","<<pos[2]<<endl;
//  _bfield->fieldMap(pos,B);
  //cout<<"TR::B="<<B[0]<<","<<B[1]<<","<<B[2]<<endl;
  //  const double Bmag = sqrt(Bx*Bx+By*By+Bz*Bz);
  m_pmgnIMF->fieldVector(10*pos,B);
  pmag = sqrt(y[3]*y[3]+y[4]*y[4]+y[5]*y[5]);
  f[0] = y[3]/pmag; // p/|p|
  f[1] = y[4]/pmag;
  f[2] = y[5]/pmag;
 
  //  const factor = _charge/(alpha2/Bmag)/pmag/Bmag;
 // factor = ((double)_charge)/alpha2/10.; //[(GeV/c)/(cm kG)]
 // factor = ((double)_charge)/(333.564095/(-1000*(m_pmgnIMF->getReferField())))/10.; //[(GeV/c)/(cm kG)]
  double Bmag=sqrt(B.x()*B.x()+B.y()*B.y()+B.z()*B.z());
  factor = ((double)_charge)/(0.333564095);
  //  f[3] = factor*(f[1]*Bz-f[2]*By);
  //  f[4] = factor*(f[2]*Bx-f[0]*Bz);
  //  f[5] = factor*(f[0]*By-f[1]*Bx);
  f[3] = factor*(f[1]*B.z()-f[2]*B.y());
  f[4] = factor*(f[2]*B.x()-f[0]*B.z());
  f[5] = factor*(f[0]*B.y()-f[1]*B.x());
}
 
void TRunge::SetFirst(double y[6]) const{
  //  y[6] = (x,y,z,px,py,pz)
  const double cosPhi0=cos(_a[1]);
  const double sinPhi0=sin(_a[1]);
  const double invKappa=1/abs(_a[2]);
 
  y[0]= _pivot.x()+_a[0]*cosPhi0;   //x
  y[1]= _pivot.y()+_a[0]*sinPhi0;   //y
  y[2]= _pivot.z()+_a[3];       //z
  y[3]= -sinPhi0*invKappa;      //px
  y[4]=  cosPhi0*invKappa;      //py
  y[5]= _a[4]*invKappa;         //pz
}
 
 
 
//  const double alpha = alpha2/1.5;  //[cm/(GeV/c)]  #### 1.5T fix ####!!!!
//  The unit of kappa is defined by this constant. [about 1/(GeV/c)]
 
unsigned TRunge::Nstep(void) const{
  return(_Nstep);
}
 
int TRunge::GetXP(unsigned stepNum,double y[6]) const{
  if(stepNum>=_Nstep || stepNum>=TRunge_MAXstep) return(-1);
 
  for(unsigned i=0;i<6;i++) y[i]=_y[stepNum][i];
  return(0);
}
int TRunge::GetStep(unsigned stepNum,double& step) const{
  if(stepNum>=_Nstep || stepNum>=TRunge_MAXstep) return(-1);
  step=_h[stepNum];
  return(0);
}
 
void TRunge::Propagate1(const double y[6], const double dydx[6],
            const double& step, double yout[6]) const{
  //  y[6] = (x,y,z,px,py,pz)
  double f2[6],f3[6],f4[6],yt[6];
  double hh;
  double h6;
  unsigned i;
 
  hh = step*0.5;
  for(i=0;i<6;i++) yt[i]=y[i]+hh*dydx[i];   // 1st step
  //{
  //  const register double *a=y;
  //  const register double *b=dydx;
  //  register double *t=yt;
  //  const register double *e=y+6;
  //  for(;a<e;a++,b++,t++) (*t)=(*a)+hh*(*b);
  //}
  Function(yt,f2);              // 2nd step
  for(i=0;i<6;i++) yt[i]=y[i]+hh*f2[i];
  //{
  //  const register double *a=y;
  //  const register double *b=f2;
  //  register double *t=yt;
  //  const register double *e=y+6;
  //  for(;a<e;a++,b++,t++) (*t)=(*a)+hh*(*b);
  //}
  Function(yt,f3);              // 3rd step
  for(i=0;i<6;i++) yt[i]=y[i]+step*f3[i];
  //{
  //  const register double *a=y;
  //  const register double *b=f3;
  //  register double *t=yt;
  //  const register double *e=y+6;
  //  for(;a<e;a++,b++,t++) (*t)=(*a)+step*(*b);
  //}
  Function(yt,f4);              // 4th step
 
  h6 = step/6;
  for(i=0;i<6;i++) yout[i]=y[i]+h6*(dydx[i]+2*f2[i]+2*f3[i]+f4[i]);
  //{
  //  const register double *a=dydx;
  //  const register double *b=f2;
  //  const register double *c=f3;
  //  const register double *d=f4;
  //  const register double *e=y;
  //  register double *t=yout;
  //  const register double *s=yout+6;
  //  for(;t<s;a++,b++,c++,d++,e++,t++)
  //    (*t)=(*e)+h6*((*a)+2*(*b)+2*(*c)+(*d));
  //}
}
 
#define PGROW -0.20
#define PSHRNK -0.25
#define FCOR 0.06666666     // 1.0/15.0
#define SAFETY 0.9
#define ERRCON 6.0e-4       // (4/SAFETY)^(1/PGROW)
void TRunge::Propagate_QC(double y[6],double dydx[6],const double& steptry,
              const double& eps, const double yscal[6],
              double& stepdid, double& stepnext) const{
  //propagate with quality check, step will be scaled automatically
  double ysav[6],dysav[6],ytemp[6];
  double h,hh,errmax,temp;
  unsigned i;
 
  for(i=0;i<6;i++) ysav[i]=y[i];
  //memcpy(ysav,y,sizeof(double)*6);
  for(i=0;i<6;i++) dysav[i]=dydx[i];
  //memcpy(dysav,dydx,sizeof(double)*6);
 
  h=steptry;
  for(;;){
    hh=h*0.5;               // 2 half step
    Propagate1(ysav,dysav,hh,ytemp);
    Function(ytemp,dydx);
    Propagate1(ytemp,dydx,hh,y);
    //if  check step size
    Propagate1(ysav,dysav,h,ytemp); // 1 full step
    // error calculation
    errmax=0.0;
    for(i=0;i<6;i++){
      ytemp[i]=y[i]-ytemp[i];
      temp=fabs(ytemp[i]/yscal[i]);
      if(errmax < temp) errmax=temp;
    }
    //cout<<"TR: errmax="<<errmax<<endl;
    errmax/=eps;
    if(errmax <= 1.0){          // step O.K.  calc. for next step
      stepdid=h;
      stepnext=(errmax>ERRCON ? SAFETY*h*exp(PGROW*log(errmax)) : 4.0*h);
      break;
    }
    h=SAFETY*h*exp(PSHRNK*log(errmax));
  }
  for(i=0;i<6;i++) y[i]+=ytemp[i]*FCOR;
  //{
  //  register double *a=ytemp;
  //  register double *t=y;
  //  register double *e=y+6;
  //  for(;t<e;a++,t++) (*t)+=(*a)*FCOR;
  //}
 
}
 
#define TINY 1.0e-30
//#define EPS 1.0e-6
unsigned TRunge::Fly_SC(void) const{//fly the particle track with stepsize control
  double y[6],dydx[6],yscal[6];
  unsigned Nstep;
  double step,stepdid,stepnext;
  unsigned i;
  double flightlength;
 
  //yscal[0]=0.1; //1mm  -> error = 1mm*EPS
  //yscal[1]=0.1; //1mm
  //yscal[2]=0.1; //1mm
  //yscal[3]=0.001; //1MeV
  //yscal[4]=0.001; //1MeV
  //yscal[5]=0.001; //1MeV
 
  step=default_stepSize0;
  flightlength=0;
  SetFirst(y);
  for(Nstep=0;Nstep<TRunge_MAXstep;Nstep++){
    for(i=0;i<6;i++) _y[Nstep][i]=y[i]; // save each step
    //memcpy(_y[Nstep],y,sizeof(double)*6);
 
    Function(y,dydx);
    //for(i=0;i<6;i++) yscal[i]=abs(y[i])+abs(dydx[i]*step)+TINY;
 
    Propagate_QC(y,dydx,step,_eps,_yscal,stepdid,stepnext);
 
    if( y[2]>160 || y[2]<-80 || (y[0]*y[0]+y[1]*y[1])>8100 ) break;
    //if position is out side of MDC, stop to fly
    //  R>90cm, z<-80cm,160cm<z
 
    flightlength += stepdid;
    if(flightlength>_maxflightlength) break;
 
    _h[Nstep]=stepdid;
    //cout<<"TR:"<<Nstep<<":step="<<stepdid<<endl;
    if(stepnext<_stepSizeMin)      step=_stepSizeMin;
    else if(stepnext>_stepSizeMax) step=_stepSizeMax;
    else step=stepnext;
  }
  _Nstep=Nstep+1;
  //cout<<"TR: Nstep="<<_Nstep<<endl;
  return(_Nstep);
}
 
double TRunge::SetFlightLength(void){
 
  // get a list of links to a wire hit
  const AList<TMLink>& cores=this->cores();
  //cout<<"TR:: cores.length="<<cores.length()<<endl;
 
  const Helix hel(this->helix());
  double tanl=hel.tanl();
  //double curv=hel.curv();
  //double rho = Helix::ConstantAlpha / hel.kappa();
 // double rho = 333.564095 / hel.kappa();  
  const double Bz = 1000*m_pmgnIMF->getReferField();
  double rho = 333.564095/(hel.kappa()*Bz);  
 
  // search max phi
  double phi_max=- DBL_MAX;
  double phi_min= DBL_MAX;
  // loop over link
  for(int j=0;j<cores.length();j++){
    TMLink& l=*cores[j];
    // fitting valid?
    const TMDCWire& wire=*l.wire();
    double Wz=0;
    //double mindist;
    HepPoint3D onWire;
    HepPoint3D dummy_bP;
    HepVector3D dummy_dir;
    Helix th(hel);
    for(int i=0;i<10;i++){
      wire.wirePosition(Wz,onWire,dummy_bP,dummy_dir);
      th.pivot(onWire);
      if(abs(th.dz())<0.1){ //<1mm
    //mindist=abs(hel.dr());
    break;
      }
      Wz+=th.dz();
    }
    //onWire is closest point , th.x() is closest point on trk
    //Wz is z position of closest point
    //Z->dphi
    /*
    // Calculate position (x,y,z) along helix.
    // x = x0 + dr * cos(phi0) + (alpha / kappa) * (cos(phi0) - cos(phi0+phi))
    // y = y0 + dr * sin(phi0) + (alpha / kappa) * (sin(phi0) - sin(phi0+phi))
    // z = z0 + dz             - (alpha / kappa) * tan(lambda) * phi
    cout<<"TR:: Wz="<<Wz<<" tanl="<<tanl<<endl;
    double phi=( hel.pivot().z()+hel.dz()-Wz )/( curv*tanl );
    */
    //...Cal. dPhi to rotate...
    const HepPoint3D & xc = hel.center();
    const HepPoint3D & xt = hel.x();
    HepVector3D v0, v1;
    v0 = xt - xc;
    v1 = th.x() - xc;
    const double vCrs = v0.x() * v1.y() - v0.y() * v1.x();
    const double vDot = v0.x() * v1.x() + v0.y() * v1.y();
    double phi = atan2(vCrs, vDot);
 
    double tz=hel.x(phi).z();
    //cout<<"TR::  Wz="<<Wz<<" tz="<<tz<<endl;
    
    if(phi>phi_max) phi_max=phi;
    if(phi<phi_min) phi_min=phi;
    //cout<<"TR:: phi="<<phi<<endl;
  }
  //cout<<"TR:: phi_max="<<phi_max<<" phi_min="<<phi_min
  //    <<" rho="<<rho<<" tanl="<<tanl<<endl;
  //phi->length, set max filght length
  //tanl*=1.2; // for mergin
  _maxflightlength=
    abs( rho*(phi_max-phi_min)*sqrt(1+tanl*tanl) )*1.2; //x1.1 mergin
 
  return(_maxflightlength);
}
double TRunge::DisToWire(TMLink& l,double z, HepPoint3D a, HepPoint3D&  b){// by liucy 2011/7/28
   double zinter[400];
   double ddist=999;
   double finalz=0;
   const TMDCWire& w=*l.wire();
   HepPoint3D onwire;
//   for(int i=0;i<20;i++){
  //   zinter[i]=z-0.0002*i+0.002;
    HepPoint3D point=w.xyPosition(z);
//     HepPoint3D point=w.xyPosition(zinter[i]);
     onwire.setX(point.x());
     onwire.setY(point.y());
   //  onwire.setZ(zinter[i]);
     onwire.setZ(z);
//     double dist=sqrt((point.x()-a.x())*(point.x()-a.x())+(point.y()-a.y())*(point.y()-a.y())+(zinter[i]-a.z())*(zinter[i]-a.z()));
      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()));
     if(dist<ddist){
       ddist=dist;
//       finalz=zinter[i];
       b=onwire;
     }
//   }   
   return ddist;
}
double TRunge::dEpath(double mass, double path, double p)const{
       double z=4.72234;
       double a=9.29212;
       double Density=0.00085144;
       double coeff=Density*z/a;
       double i=51.4709;
       double isq=i * i * 1e-18;
//       double sigma0_2 = 0.1569*coeff*path;
//  
//  if (sigma0_2<0) return 0;
// 
//  double psq = p * p;
//  double bsq = psq / (psq + mass * mass);
//  
//  // Correction for relativistic particles :
//  double sigma_2 = sigma0_2*(1-0.5*bsq)/(1-bsq);
//
//  if (sigma_2<0) return 0;
//
//  // Return sigma in GeV !!
//  return sqrt(sigma_2)*0.001;
const double Me = 0.000510999;
  double psq = p * p;
  double bsq = psq / (psq + mass * mass);
  double esq = psq / (mass * mass);
 
  double s = Me / mass;
  double w = (4 * Me * esq
          / (1 + 2 * s * sqrt(1 + esq)
         + s * s));
 
  // Density correction :
  double cc, x0;
  cc = 1+2*log(sqrt(isq)/(28.8E-09*sqrt(coeff)));
  if (cc < 5.215)
    x0 = 0.2;
  else
    x0 = 0.326*cc-1.5;
  double x1(3), xa(cc/4.606), aa;
  aa = 4.606*(xa-x0)/((x1-x0)*(x1-x0)*(x1-x0));
  double delta(0);
double x(log10(sqrt(esq)));
  if (x > x0){
    delta = 4.606*x-cc;
    if (x < x1) delta=delta+aa*(x1-x)*(x1-x)*(x1-x);
  }
 
 // Shell correction :  
  float f1, f2, f3, f4, f5, ce;
  f1 = 1/esq;
  f2 = f1*f1;
  f3 = f1*f2;
  f4 = (f1*0.42237+f2*0.0304-f3*0.00038)*1E12;
  f5 = (f1*3.858-f2*0.1668+f3*0.00158)*1E18;
  ce = f4*isq+f5*isq*sqrt(isq);
 
  return (0.0001535 * coeff / bsq
      * (log(Me * esq * w / isq)
         - 2 * bsq-delta-2.0*ce/z)) * path;
}
void TRunge::eloss(double path ,const RkFitMaterial * materiral, double mass,double y[6],int index)const{
double psq=y[5]*y[5]+y[3]*y[3]+y[4]*y[4];
 
double p=sqrt(psq);
 
double dE=materiral->dE(mass,path,p);
if (index > 0)
  psq += dE * (dE + 2 * sqrt(mass * mass + psq));
else
  psq += dE * (dE - 2 * sqrt(mass * mass + psq));
double pnew=sqrt(psq);
double coeff=pnew/p;
 
y[3]=y[3]*coeff;
y[4]=y[4]*coeff;
y[5]=y[5]*coeff;
}
double TRunge::intersect_cylinder(double r) const {
    double m_rad = helix().radius();
    double l = helix().center().perp();
    double cosPhi = (m_rad * m_rad + l * l  - r * r) / (2 * m_rad * l);
 
    if(cosPhi < -1 || cosPhi > 1) return 0.;
 
    double dPhi = helix().center().phi() - acos(cosPhi) - helix().phi0();
    if(dPhi < -M_PI) dPhi += 2 * M_PI;
 
    return dPhi;
}
double TRunge::intersect_zx_plane(const HepTransform3D& plane, double y) const {
    HepPoint3D xc = plane * helix().center();
    double r = helix().radius();
    double d = r * r - (y - xc.y()) * (y - xc.y());
    if(d < 0) return 0;
 
    double xx = xc.x();
    if(xx > 0) xx -= sqrt(d);
    else xx += sqrt(d);
 
    double l = (plane.inverse() *
                HepPoint3D(xx, y, 0)).perp();
 
    return intersect_cylinder(l);
}
 
 
double TRunge::intersect_yz_plane(const HepTransform3D& plane, double x) const {
    HepPoint3D xc = plane * helix().center();
    double r = helix().radius();
    double d = r * r - (x - xc.x()) * (x - xc.x());
    if(d < 0) return 0;
 
    double yy = xc.y();
    if(yy > 0) yy -= sqrt(d);
    else yy += sqrt(d);
 
    double l = (plane.inverse() *
                HepPoint3D(x, yy, 0)).perp();
 
    return intersect_cylinder(l);
}
 
 
double TRunge::intersect_xy_plane(double z) const {
    if (helix().tanl() != 0 && helix().radius() != 0)
      return (helix().pivot().z() + helix().dz() - z) / (helix().radius() * helix().tanl());
    else return 0;
}