00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include "TrkFitter/TrkDifLineTraj.h"
00013 #include <assert.h>
00014 #include <math.h>
00015 #include "MdcGeom/BesAngle.h"
00016 #include "TrkFitter/TrkDifLineTraj.h"
00017 #include "TrkBase/TrkExchangePar.h"
00018 #include "TrkBase/TrkVisitor.h"
00019 #include "MdcRecoUtil/DifNumber.h"
00020 #include "MdcRecoUtil/DifPoint.h"
00021 #include "MdcRecoUtil/DifVector.h"
00022
00023 using CLHEP::Hep3Vector;
00024
00025 TrkDifLineTraj::TrkDifLineTraj(const HepVector& pvec, const HepSymMatrix& pcov,
00026 double lowlim, double hilim, const HepPoint3D& refpoint) :
00027 TrkSimpTraj(pvec, pcov, lowlim,hilim,refpoint)
00028 {
00029 }
00030
00031 TrkDifLineTraj::TrkDifLineTraj(const TrkExchangePar& inpar,
00032 double lowlim, double hilim, const HepPoint3D& refpoint) :
00033 TrkSimpTraj(HepVector(NLINPRM,1),HepSymMatrix(NLINPRM,1), lowlim,hilim,refpoint)
00034 {
00035
00036
00037 HepVector subvect(NLINPRM,1);
00038 HepSymMatrix submat(NLINPRM,1);
00039 const HepSymMatrix covar = inpar.covariance();
00040 subvect[d0Ind] = inpar.d0();
00041 subvect[phi0Ind] = inpar.phi0();
00042 subvect[z0Ind] = inpar.z0();
00043 subvect[tanDipInd] = inpar.tanDip();
00044
00045 submat.fast(d0Ind+1,d0Ind+1) = covar.fast(TrkExchangePar::ex_d0+1,TrkExchangePar::ex_d0+1);
00046 submat.fast(d0Ind+1,phi0Ind+1) = covar.fast(TrkExchangePar::ex_d0+1,TrkExchangePar::ex_phi0+1);
00047 submat.fast(d0Ind+1,z0Ind+1) = covar.fast(TrkExchangePar::ex_d0+1,TrkExchangePar::ex_z0+1);
00048 submat.fast(d0Ind+1,tanDipInd+1) = covar.fast(TrkExchangePar::ex_d0+1,TrkExchangePar::ex_tanDip+1);
00049 submat.fast(phi0Ind+1,phi0Ind+1) = covar.fast(TrkExchangePar::ex_phi0+1,TrkExchangePar::ex_phi0+1);
00050 submat.fast(phi0Ind+1,z0Ind+1) = covar.fast(TrkExchangePar::ex_phi0+1,TrkExchangePar::ex_z0+1);
00051 submat.fast(phi0Ind+1,tanDipInd+1) = covar.fast(TrkExchangePar::ex_phi0+1,TrkExchangePar::ex_tanDip+1);
00052 submat.fast(z0Ind+1,z0Ind+1) = covar.fast(TrkExchangePar::ex_z0+1,TrkExchangePar::ex_z0+1);
00053 submat.fast(z0Ind+1,tanDipInd+1) = covar.fast(TrkExchangePar::ex_z0+1,TrkExchangePar::ex_tanDip+1);
00054 submat.fast(tanDipInd+1,tanDipInd+1) = covar.fast(TrkExchangePar::ex_tanDip+1,TrkExchangePar::ex_tanDip+1);
00055
00056 (*parameters()) = TrkParams(subvect,submat);
00057 }
00058
00059 TrkDifLineTraj::TrkDifLineTraj( const TrkDifLineTraj& h )
00060 : TrkSimpTraj(h.parameters()->parameter(), h.parameters()->covariance(),
00061 h.lowRange(),h.hiRange(),h.referencePoint())
00062 {
00063 }
00064
00065 TrkDifLineTraj*
00066 TrkDifLineTraj::clone() const
00067 {
00068 return new TrkDifLineTraj(*this);
00069 }
00070
00071 TrkDifLineTraj&
00072 TrkDifLineTraj::operator=(const TrkDifLineTraj& h)
00073 {
00074 if( &h != this ){
00075 Trajectory::operator=(h);
00076 _dtparams = *h.parameters();
00077 _refpoint = h._refpoint;
00078 }
00079 return *this;
00080 }
00081
00082 TrkDifLineTraj::~TrkDifLineTraj()
00083 {
00084 }
00085
00086 double
00087 TrkDifLineTraj::x( const double& f ) const
00088 {
00089 return -d0() * sin(phi0()) + f * cos(phi0()) * cosDip() +
00090 referencePoint().x();
00091 }
00092
00093 double
00094 TrkDifLineTraj::y( const double& f ) const
00095 {
00096 return d0() * cos(phi0()) + f * sin(phi0()) * cosDip() +
00097 referencePoint().y();
00098 }
00099
00100 double
00101 TrkDifLineTraj::z( const double& f ) const
00102 {
00103 return z0() + f * tanDip() * cosDip() + referencePoint().z();
00104 }
00105
00106 HepPoint3D
00107 TrkDifLineTraj::position( double f) const
00108 {
00109 double cosd = cosDip();
00110 double cp = cos(phi0());
00111 double sp = sin(phi0());
00112 return HepPoint3D(-d0()*sp + f*cp*cosd + referencePoint().x(),
00113 d0()*cp + f*sp*cosd + referencePoint().y(),
00114 z0() + f*tanDip()*cosd + referencePoint().z());
00115 }
00116
00117 Hep3Vector
00118 TrkDifLineTraj::direction( double ) const
00119 {
00120 double cdip = cosDip();
00121 return Hep3Vector ( cos(phi0()) * cdip,
00122 sin(phi0()) * cdip,
00123 tanDip() * cdip);
00124 }
00125
00126 Hep3Vector
00127 TrkDifLineTraj::delDirect( double ) const
00128 {
00129 return Hep3Vector(0.0, 0.0, 0.0);
00130 }
00131
00132 double
00133 TrkDifLineTraj::distTo1stError(double, double, int) const
00134 {
00135 return 999.e4;
00136 }
00137
00138 double
00139 TrkDifLineTraj::distTo2ndError(double, double, int) const
00140 {
00141 return 999.e4;
00142 }
00143
00144 void
00145 TrkDifLineTraj::getInfo(double fltLen, HepPoint3D& pos, Hep3Vector& dir,
00146 Hep3Vector& delDir) const
00147 {
00148
00149 pos = position(fltLen);
00150 dir = direction(fltLen);
00151 delDir = delDirect(fltLen);
00152 }
00153
00154 void
00155 TrkDifLineTraj::getInfo( double fltLen, HepPoint3D& pos, Hep3Vector& dir ) const
00156 {
00157
00158 pos = position(fltLen);
00159 dir = direction(fltLen);
00160 }
00161
00162 void
00163 TrkDifLineTraj::getDFInfo(double flt, DifPoint& pos, DifVector& dir,
00164 DifVector& delDir) const
00165 {
00166
00167
00168
00169
00170 DifNumber phi0Df(phi0(), phi0Index()+1, nLinPrm());
00171 DifNumber d0Df(d0(), d0Index()+1, nLinPrm());
00172 DifNumber z0Df(z0(), z0Index()+1, nLinPrm());
00173 DifNumber tanDipDf(tanDip(), tanDipIndex()+1, nLinPrm());
00174 DifNumber zero(0.0, nLinPrm());
00175 phi0Df.setIndepPar( parameters() );
00176 d0Df.setIndepPar( parameters() );
00177 z0Df.setIndepPar( parameters() );
00178 tanDipDf.setIndepPar( parameters() );
00179 zero.setIndepPar( parameters() );
00180
00181 DifNumber sphi0, cphi0;
00182 phi0Df.cosAndSin(cphi0, sphi0);
00183
00184 DifNumber px(referencePoint().x());
00185 DifNumber py(referencePoint().y());
00186 DifNumber pz(referencePoint().z());
00187
00188 DifNumber cdip = 1. / sqrt(1. + tanDipDf*tanDipDf);
00189 DifNumber xx = -d0Df * sphi0 + flt * cphi0 * cdip + px;
00190 DifNumber yy = d0Df * cphi0 + flt * sphi0 * cdip + py;
00191 DifNumber zz = z0Df + flt * tanDipDf * cdip + pz;
00192
00193 pos = DifPoint(xx, yy, zz);
00194 dir = DifVector( cphi0 * cdip,
00195 sphi0 * cdip,
00196 tanDipDf * cdip);
00197
00198 delDir = DifVector(zero, zero, zero);
00199 }
00200
00201 double
00202 TrkDifLineTraj::curvature(double ) const
00203 {
00204 return 0.;
00205 }
00206
00207 double
00208 TrkDifLineTraj::phi0() const
00209 {
00210 return BesAngle(parameters()->parameter()[phi0Index()]).rad();
00211 }
00212
00213 HepMatrix
00214 TrkDifLineTraj::derivDeflect(double fltlen,deflectDirection idirect) const
00215 {
00216
00217
00218
00219
00220
00221
00222 HepMatrix ddflct(nLinPrm(),1);
00223
00224 double cosd = cosDip();
00225
00226 switch (idirect) {
00227 case theta1:
00228 ddflct[tanDipIndex()][0] = 1.0/(cosd*cosd);
00229 ddflct[d0Index()][0] = 0.0;
00230 ddflct[phi0Index()][0] = 0.0;
00231 ddflct[z0Index()][0] = -fltlen/cosd;;
00232 break;
00233 case theta2:
00234 ddflct[tanDipIndex()][0] = 0;
00235 ddflct[d0Index()][0] = -fltlen;
00236 ddflct[phi0Index()][0] = 1.0/cosd;
00237 ddflct[z0Index()][0] = -(tanDip()/cosd)*d0();
00238 break;
00239 }
00240 return ddflct;
00241 }
00242
00243 HepMatrix
00244 TrkDifLineTraj::derivDisplace(double fltlen,deflectDirection idirect) const
00245 {
00246
00247
00248
00249
00250
00251
00252 HepMatrix ddflct(nLinPrm(),1);
00253
00254 double cosd = cosDip();
00255
00256 switch (idirect) {
00257 case theta1:
00258 ddflct[tanDipIndex()][0] = 0.0;
00259 ddflct[d0Index()][0] = 0.0;
00260 ddflct[phi0Index()][0] = 0.0;
00261 ddflct[z0Index()][0] = 1.0/cosd;
00262 break;
00263 case theta2:
00264 ddflct[tanDipIndex()][0] = 0;
00265 ddflct[d0Index()][0] = 1.0;
00266 ddflct[phi0Index()][0] = 0.0;
00267 ddflct[z0Index()][0] = 0.0;
00268 break;
00269 }
00270 return ddflct;
00271 }
00272
00273 HepMatrix
00274 TrkDifLineTraj::derivPFract(double) const
00275 {
00276
00277
00278
00279
00280
00281
00282 return HepMatrix(nLinPrm(),1, 0);
00283 }
00284
00285 void
00286 TrkDifLineTraj::paramFunc(const HepPoint3D& oldpoint,const HepPoint3D& newpoint,
00287 const HepVector& oldvec,const HepSymMatrix& oldcov,
00288 HepVector& newvec,HepSymMatrix& newcov,
00289 double fltlen)
00290 {
00291
00292 std::cout<<"ErrMsg(fatal)" << "TrkDifLineTraj::paramFunc() is not implemented!" << std::endl;
00293 }
00294
00295 void
00296 TrkDifLineTraj::invertParams(TrkParams* params, std::vector<bool>& flags) const
00297 {
00298
00299
00300 for (unsigned iparam = 0; iparam < NLINPRM; iparam++) {
00301 switch ( iparam ) {
00302 case d0Ind:
00303 case tanDipInd:
00304 params->parameter()[iparam] *= -1.0;
00305 flags[iparam] = true;
00306 break;
00307 case phi0Ind:
00308 params->parameter()[iparam] =
00309 BesAngle(params->parameter()[iparam] + Constants::pi);
00310 flags[iparam] = false;
00311 break;
00312 case z0Ind:
00313 flags[iparam] = false;
00314 }
00315 }
00316 return;
00317 }
00318
00319 void
00320 TrkDifLineTraj::visitAccept(TrkVisitor* vis) const
00321 {
00322
00323 vis->trkVisitLineTraj(this);
00324 }