00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "MdcGeom/Constants.h"
00018 #include "MdcGeom/BesAngle.h"
00019 #include "TrkBase/TrkSimpTraj.h"
00020 #include "TrkBase/TrkParams.h"
00021 #include "CLHEP/Matrix/SymMatrix.h"
00022 #include "TrkBase/TrkPocaXY.h"
00023
00024 using std::ostream;
00025
00026
00027 HepPoint3D TrkSimpTraj::_theOrigin(0.0,0.0,0.0);
00028
00029
00030 TrkSimpTraj::TrkSimpTraj(const HepVector& params, const HepSymMatrix& cov,
00031 const double lowlim,const double hilim,
00032 const HepPoint3D& refpoint) :
00033
00034 TrkDifTraj(lowlim, hilim),_dtparams(params, cov),_refpoint(refpoint)
00035 {;}
00036
00037 TrkSimpTraj::TrkSimpTraj(const TrkParams& params,
00038 const double lowlim,const double hilim,
00039 const HepPoint3D& refpoint) :
00040
00041 TrkDifTraj(lowlim, hilim),_dtparams(params),_refpoint(refpoint)
00042 {;}
00043
00044
00045 TrkSimpTraj::TrkSimpTraj(const TrkSimpTraj& other) :
00046
00047 TrkDifTraj(other.lowRange(),other.hiRange()),
00048 _dtparams(other._dtparams),
00049 _refpoint(other._refpoint)
00050 {}
00051
00052
00053 TrkSimpTraj::~TrkSimpTraj()
00054
00055 { }
00056
00057
00058 const TrkSimpTraj*
00059 TrkSimpTraj::localTrajectory(double fltLen, double& localFlt) const {
00060
00061 localFlt = fltLen;
00062 return this;
00063 }
00064
00065
00066
00067
00068
00069 void
00070 TrkSimpTraj::changePoint(const HepPoint3D& newpoint,double& fltlen) {
00071
00072 if(newpoint != _refpoint){
00073
00074 TrkPocaXY endpoca(*this,fltlen,newpoint);
00075 if(endpoca.status().failure()){
00076 std::cout<<"ErrMsg(error)" << "poca failure changing reference point" << std::endl;
00077 return;
00078 } else {
00079
00080 fltlen = endpoca.flt1();
00081
00082 int nrow = parameters()->covariance().num_row();
00083 HepSymMatrix cov(nrow);
00084 for(int irow=0;irow<nrow;irow++)
00085 for(int icol=0;icol<=irow;icol++)
00086 cov.fast(irow+1,icol+1) = parameters()->covariance().fast(irow+1,icol+1);
00087
00088 TranslateParams pfunc = paramFunction();
00089
00090 pfunc(_refpoint,newpoint,
00091 parameters()->parameter(),cov,
00092 _dtparams.parameter(),cov,
00093 fltlen);
00094
00095 _dtparams.covariance() = cov;
00096 _refpoint = newpoint;
00097
00098 double newrange[2];
00099 newrange[0] = lowRange() - fltlen;
00100 newrange[1] = hiRange() - fltlen;
00101 setFlightRange(newrange);
00102 }
00103 }
00104 return;
00105 }
00106
00107 void
00108 TrkSimpTraj::printAll(ostream& os) const {
00109
00110 os << "Simple ";
00111 Trajectory::printAll(os);
00112 os << "SimpTraj parameter vector = "
00113 << _dtparams.parameter();
00114 os << " and covariance matrix = "
00115 << _dtparams.covariance();
00116 }
00117
00118 void
00119 TrkSimpTraj::print(ostream& os) const {
00120
00121 os << "Simple ";
00122 Trajectory::print(os);
00123 }
00124
00125
00126
00127 TrkSimpTraj&
00128 TrkSimpTraj::invert()
00129 {
00130
00131 std::vector<bool> flags(parameters()->nPar(),false);
00132 invertParams(parameters(), flags);
00133
00134 for(int iparam=0;iparam<parameters()->nPar();iparam++){
00135 bool iinvert = flags[iparam];
00136
00137 for(int jparam=iparam+1;jparam<parameters()->nPar();jparam++){
00138 bool jinvert = flags[jparam];
00139 if( (iinvert && !jinvert) || (!iinvert && jinvert) ) {
00140
00141 parameters()->covariance()[iparam][jparam] *= -1.0;
00142 }
00143 }
00144 }
00145
00146 double range[2];
00147 range[0] = -hiRange();
00148 range[1] = -lowRange();
00149 setFlightRange(range);
00150
00151 return *this;
00152 }
00153
00154
00155 bool
00156 TrkSimpTraj::operator==(const TrkSimpTraj& x) const
00157 {
00158 if (lowRange()!=x.lowRange() || hiRange()!=x.hiRange()) return false;
00159 const HepVector &m=_dtparams.parameter();
00160 unsigned int mp=m.num_row();
00161 const HepVector &n=x._dtparams.parameter();
00162 unsigned int np=n.num_row();
00163 if (np!=mp) return false;
00164 for(unsigned i=0;i<np;++i){
00165 if(m[i] != n[i]) return false;
00166 }
00167 return _refpoint==x._refpoint;
00168 }