00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include <assert.h>
00013 #include <math.h>
00014 #include "MdcGeom/Constants.h"
00015 #include "MdcGeom/BesAngle.h"
00016 #include "TrkFitter/TrkCircleTraj.h"
00017 #include "TrkBase/TrkVisitor.h"
00018 #include "MdcRecoUtil/DifNumber.h"
00019 #include "MdcRecoUtil/DifPoint.h"
00020 #include "MdcRecoUtil/DifVector.h"
00021 #include "TrkBase/TrkExchangePar.h"
00022
00023
00024
00025 #ifndef M_2PI
00026 #define M_2PI 2*M_PI
00027 #endif
00028 using CLHEP::Hep3Vector;
00029 using CLHEP::HepSymMatrix;
00030
00031
00032 TrkCircleTraj::TrkCircleTraj(const HepVector& pvec, const HepSymMatrix& pcov,
00033 double lowlim, double hilim, const HepPoint3D& refpoint) :
00034 TrkSimpTraj(pvec, pcov, lowlim,hilim,refpoint)
00035
00036 {
00037
00038 if( pvec.num_row() != nCirPrm() ||
00039 pcov.num_row() != nCirPrm() ){
00040 std::cout<<"ErrMsg(fatal)" <<
00041 "CircleTraj: incorrect constructor vector/matrix dimension" << std::endl;
00042 }
00043
00044 if (omega() == 0.0) parameters()->parameter()[omegaIndex()] = 1.e-9;
00045 }
00046
00047
00048
00049 TrkCircleTraj::TrkCircleTraj(const TrkExchangePar& inpar,
00050 double lowlim, double hilim, const HepPoint3D& refpoint) :
00051 TrkSimpTraj(inpar.params(), inpar.covariance(), lowlim,hilim,refpoint) {
00052
00053
00054 if (omega() == 0.0) parameters()->parameter()[omegaIndex()] = 1.e-9;
00055 }
00056
00057 TrkCircleTraj::TrkCircleTraj(const TrkCircleTraj& h )
00058 : TrkSimpTraj(h.parameters()->parameter(), h.parameters()->covariance(),
00059 h.lowRange(),h.hiRange(),h.referencePoint())
00060 {
00061
00062 }
00063
00064 TrkCircleTraj*
00065 TrkCircleTraj::clone() const
00066 {
00067 return new TrkCircleTraj(*this);
00068 }
00069
00070 TrkCircleTraj&
00071 TrkCircleTraj::operator=(const TrkCircleTraj& h)
00072 {
00073 if( &h != this ){
00074 Trajectory::operator=(h);
00075 _dtparams = *(h.parameters());
00076 _refpoint = h._refpoint;
00077 }
00078 return *this;
00079 }
00080
00081 TrkCircleTraj::~TrkCircleTraj()
00082 {
00083 }
00084
00085 double
00086 TrkCircleTraj::x( const double& f ) const
00087 {
00088 double sphi0 = sin(phi0());
00089 return ( sin(angle(f))-sphi0)/omega() - d0()*sphi0 + referencePoint().x();
00090 }
00091
00092 double
00093 TrkCircleTraj::y( const double& f ) const
00094 {
00095 double cphi0 = cos(phi0());
00096 return -(cos(angle(f))-cphi0)/omega() + d0()*cphi0 + referencePoint().y();
00097 }
00098
00099 HepPoint3D
00100 TrkCircleTraj::position( double f) const
00101 {
00102 double sphi0 = sin(phi0());
00103 double cphi0 = cos(phi0());
00104 double ang = angle(f);
00105 double xt = ( sin(ang)-sphi0)/omega() - d0()*sphi0 + referencePoint().x();
00106 double yt = -(cos(ang)-cphi0)/omega() + d0()*cphi0 + referencePoint().y();
00107 return HepPoint3D(xt, yt, referencePoint().z());
00108 }
00109
00110 Hep3Vector
00111 TrkCircleTraj::direction( double f) const
00112 {
00113
00114
00115 double alpha = angle( f );
00116
00117 return Hep3Vector ( cos(alpha),
00118 sin(alpha),
00119 0.0);
00120 }
00121
00122 Hep3Vector
00123 TrkCircleTraj::delDirect( double fltLen ) const
00124 {
00125 double delX = -omega() * sin(angle(fltLen));
00126 double delY = omega() * cos(angle(fltLen));
00127 return Hep3Vector(delX, delY, 0.0);
00128 }
00129
00130 double
00131 TrkCircleTraj::distTo1stError(double , double tol, int) const
00132 {
00133 double arg = 2. * tol / fabs(omega());
00134 assert (arg >= 0.);
00135 return sqrt(arg);
00136 }
00137
00138 double
00139 TrkCircleTraj::distTo2ndError(double , double tol, int) const
00140 {
00141
00142 return pow(6.*tol / (omega()*omega()), 0.33333333);
00143 }
00144
00145 void
00146 TrkCircleTraj::getInfo(double flt, HepPoint3D& pos, Hep3Vector& dir,
00147 Hep3Vector& delDir) const
00148 {
00149 double sphi0 = sin(phi0());
00150 double cphi0 = cos(phi0());
00151 double ang = angle(flt);
00152 double cang = cos(ang);
00153 double sang = sin(ang);
00154 double xt = (sang-sphi0)/omega() - d0()*sphi0 + referencePoint().x();
00155 double yt = -(cang-cphi0)/omega() + d0()*cphi0 + referencePoint().y();
00156
00157 pos.setX(xt);
00158 pos.setY(yt);
00159 pos.setZ(referencePoint().z());
00160
00161 dir.setX(cang);
00162 dir.setY(sang);
00163 dir.setZ(0.0);
00164
00165 delDir.setX(-omega()*sang);
00166 delDir.setY( omega()*cang);
00167 delDir.setZ(0.);
00168 }
00169
00170 void
00171 TrkCircleTraj::getInfo( double fltLen, HepPoint3D& pos, Hep3Vector& dir ) const
00172 {
00173 double sphi0 = sin(phi0());
00174 double cphi0 = cos(phi0());
00175 double ang = angle(fltLen);
00176 double cang = cos(ang);
00177 double sang = sin(ang);
00178 double xt = (sang-sphi0)/omega() - d0()*sphi0 + referencePoint().x();
00179 double yt = -(cang-cphi0)/omega() + d0()*cphi0 + referencePoint().y();
00180
00181 pos.setX(xt);
00182 pos.setY(yt);
00183 pos.setZ(referencePoint().z());
00184
00185 dir.setX(cang);
00186 dir.setY(sang);
00187 dir.setZ(0.0);
00188 }
00189
00190 void
00191 TrkCircleTraj::getDFInfo2(double fltLen, DifPoint& pos, DifVector&
00192 dir) const
00193 {
00194
00195
00196
00197 DifNumber phi0Df(phi0(), phi0Index()+1, nCirPrm());
00198 DifNumber d0Df(d0(), d0Index()+1, nCirPrm());
00199 DifNumber omegaDf(omega(), omegaIndex()+1, nCirPrm());
00200 phi0Df.setIndepPar( parameters() );
00201 d0Df.setIndepPar( parameters() );
00202 omegaDf.setIndepPar( parameters() );
00203
00204 DifNumber sinPhi0, cosPhi0;
00205 phi0Df.cosAndSin(cosPhi0, sinPhi0);
00206
00207 DifNumber alphaDf = omegaDf;
00208 alphaDf *= fltLen;
00209 alphaDf += phi0Df;
00210
00211
00212 alphaDf.mod(-Constants::pi, Constants::pi);
00213 DifNumber sinAlpha, cosAlpha;
00214 alphaDf.cosAndSin(cosAlpha, sinAlpha);
00215
00216
00217
00218
00219 DifNumber x = sinAlpha;
00220 x -= sinPhi0;
00221 x /= omegaDf;
00222 x -= (d0Df * sinPhi0);
00223
00224 DifNumber y = -cosAlpha;
00225 y += cosPhi0;
00226 y /= omegaDf;
00227 y += (d0Df * cosPhi0);
00228
00229
00230 static DifNumber zNull(0.);
00231
00232 pos.x = x;
00233 pos.y = y;
00234 pos.z = zNull;
00235
00236 bool lref = (referencePoint().x() != 0. || referencePoint().y() != 0. ||
00237 referencePoint().z() != 0.);
00238 if (lref) {
00239 DifNumber px(referencePoint().x());
00240 DifNumber py(referencePoint().y());
00241 DifNumber pz(referencePoint().z());
00242 pos.x += px;
00243 pos.y += py;
00244 pos.z += pz;
00245 }
00246
00247 dir.x = cosAlpha;
00248 dir.y = sinAlpha;
00249 dir.z = 0.;
00250 }
00251
00252 void
00253 TrkCircleTraj::getDFInfo(double flt, DifPoint& pos, DifVector& dir,
00254 DifVector& delDir) const
00255 {
00256
00257
00258
00259 DifNumber phi0Df(phi0(), phi0Index()+1, nCirPrm());
00260 DifNumber d0Df(d0(), d0Index()+1, nCirPrm());
00261 DifNumber omegaDf(omega(), omegaIndex()+1, nCirPrm());
00262 phi0Df.setIndepPar( parameters() );
00263 d0Df.setIndepPar( parameters() );
00264 omegaDf.setIndepPar( parameters() );
00265
00266 DifNumber sinPhi0, cosPhi0;
00267 phi0Df.cosAndSin(cosPhi0, sinPhi0);
00268
00269 DifNumber alphaDf = omegaDf;
00270 alphaDf *= flt;
00271 alphaDf += phi0Df;
00272
00273
00274 alphaDf.mod(-Constants::pi, Constants::pi);
00275 DifNumber sinAlpha, cosAlpha;
00276 alphaDf.cosAndSin(cosAlpha, sinAlpha);
00277
00278
00279
00280
00281 DifNumber x = sinAlpha;
00282 x -= sinPhi0;
00283 x /= omegaDf;
00284 x -= (d0Df * sinPhi0);
00285
00286 DifNumber y = -cosAlpha;
00287 y += cosPhi0;
00288 y /= omegaDf;
00289 y += (d0Df * cosPhi0);
00290
00291
00292 static DifNumber zNull(0.);
00293
00294 pos.x = x;
00295 pos.y = y;
00296 pos.z = zNull;
00297
00298 bool lref = (referencePoint().x() != 0. || referencePoint().y() != 0. ||
00299 referencePoint().z() != 0.);
00300 if (lref) {
00301 DifNumber px(referencePoint().x());
00302 DifNumber py(referencePoint().y());
00303 DifNumber pz(referencePoint().z());
00304 pos.x += px;
00305 pos.y += py;
00306 pos.z += pz;
00307 }
00308
00309 dir.x = cosAlpha;
00310 dir.y = sinAlpha;
00311 dir.z = 0.;
00312
00313 delDir.x = -omegaDf;
00314 delDir.x *= sinAlpha;
00315
00316 delDir.y = omegaDf;
00317 delDir.y *= cosAlpha;
00318
00319 delDir.z = 0.;
00320 }
00321 HepMatrix
00322 TrkCircleTraj::derivDeflect(double fltlen,deflectDirection idirect) const
00323 {
00324
00325
00326
00327
00328
00329
00330
00331 HepMatrix ddflct(nCirPrm(),1,0);
00332
00333
00334
00335 double omeg = omega();
00336 double arcl = arc(fltlen);
00337 double dx = cos(arcl);
00338 double dy = sin(arcl);
00339 double darc = omeg*d0();
00340
00341
00342
00343 switch (idirect) {
00344 case theta1:
00345 break;
00346 case theta2:
00347 ddflct[d0Index()][0] = -dy/(omeg);
00348 ddflct[phi0Index()][0] = dx/(1+darc);
00349 }
00350
00351 return ddflct;
00352 }
00353
00354 HepMatrix
00355 TrkCircleTraj::derivDisplace(double fltlen,deflectDirection idirect) const
00356 {
00357
00358
00359
00360
00361
00362
00363
00364 HepMatrix ddflct(nCirPrm(),1,0);
00365
00366
00367
00368 double omeg = omega();
00369 double arcl = arc(fltlen);
00370 double dx = cos(arcl);
00371 double dy = sin(arcl);
00372 double darc = omeg*d0();
00373
00374
00375
00376 switch (idirect) {
00377 case theta1:
00378 break;
00379 case theta2:
00380 ddflct[d0Index()][0] = dx;
00381 ddflct[phi0Index()][0] = dy*omeg/(1+darc);
00382 }
00383
00384 return ddflct;
00385 }
00386
00387 HepMatrix
00388 TrkCircleTraj::derivPFract(double fltlen) const
00389 {
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399 HepMatrix dmomfrac(nCirPrm(),1);
00400
00401
00402
00403 double omeg = omega();
00404 double arcl = arc(fltlen);
00405 double dx = cos(arcl);
00406 double dy = sin(arcl);
00407 double darc = omeg*d0();
00408
00409
00410
00411 dmomfrac[omegaIndex()][0] = -omeg;
00412
00413 dmomfrac[d0Index()][0] = -(1-dx)/omeg;
00414
00415 dmomfrac[phi0Index()][0] = dy/(1+darc);
00416
00417 return dmomfrac;
00418 }
00419
00420 double
00421 TrkCircleTraj::curvature(double ) const
00422 {
00423
00424
00425
00426 return fabs(omega());
00427 }
00428
00429 double
00430 TrkCircleTraj::phi0() const
00431 {
00432 return BesAngle(parameters()->parameter()[phi0Index()]).rad();
00433 }
00434
00435 void
00436 TrkCircleTraj::paramFunc(const HepPoint3D& oldpoint,const HepPoint3D& newpoint,
00437 const HepVector& oldvec,const HepSymMatrix& oldcov,
00438 HepVector& newvec,HepSymMatrix& newcov,
00439 double fltlen)
00440 {
00441
00442 HepVector parvec(oldvec);
00443
00444 newvec = parvec;
00445
00446 double delx = newpoint.x()-oldpoint.x();
00447 double dely = newpoint.y()-oldpoint.y();
00448
00449 double rad = 1./parvec[omegaIndex()];
00450 double rad2 = rad*rad;
00451 double delta = rad + parvec[d0Index()];
00452 double cos0 = cos(parvec[phi0Index()]);
00453 double sin0 = sin(parvec[phi0Index()]);
00454 double perp = delx*sin0-dely*cos0;
00455 double para = delx*cos0+dely*sin0;
00456 double oldphi = parvec[phi0Index()] + fltlen*parvec[omegaIndex()] ;
00457
00458 double newdelta2 = delta*delta + delx*delx + dely*dely +
00459 2.0*delta*perp;
00460
00461 double newdelta = delta>0 ? sqrt(newdelta2) : -sqrt(newdelta2);
00462 double invdelta = 1.0/newdelta;
00463 double invdelta2 = 1.0/newdelta2;
00464
00465 newvec[d0Index()] = newdelta - rad;
00466
00467 double newphi = atan2(sin0+delx/delta,cos0-dely/delta);
00468 while(fabs(newphi - oldphi)>M_PI)
00469 if(newphi > oldphi)
00470 newphi -= M_2PI;
00471 else
00472 newphi += M_2PI;
00473 newvec[phi0Index()] = newphi;
00474
00475
00476 HepMatrix covrot(nCirPrm(),nCirPrm(),0);
00477
00478
00479 covrot[omegaIndex()][omegaIndex()] = 1.0;
00480
00481 covrot[d0Index()][omegaIndex()] = rad2*(1.0 - invdelta*(delta + perp));
00482 covrot[d0Index()][d0Index()] = invdelta*(delta + perp);
00483 covrot[d0Index()][phi0Index()] = delta*para*invdelta;
00484
00485 covrot[phi0Index()][omegaIndex()] = rad2*para*invdelta2;
00486 covrot[phi0Index()][d0Index()] = -para*invdelta2;
00487 covrot[phi0Index()][phi0Index()] = delta*(delta + perp)*invdelta2;
00488
00489
00490 newcov = oldcov.similarity(covrot);
00491
00492 }
00493
00494 void
00495 TrkCircleTraj::invertParams(TrkParams* params, std::vector<bool>& flags) const
00496 {
00497
00498
00499
00500 for (unsigned iparam = 0; iparam < NCIRPAR; iparam++) {
00501 switch ( iparam ) {
00502 case d0Ind:
00503 case omegaInd:
00504 params->parameter()[iparam] *= -1.0;
00505 flags[iparam] = true;
00506 break;
00507 case phi0Ind:
00508 params->parameter()[iparam] =
00509 BesAngle(params->parameter()[iparam] + Constants::pi);
00510 flags[iparam] = false;
00511 }
00512 }
00513 }
00514
00515
00516
00517
00518
00519
00520
00521 void
00522 TrkCircleTraj::visitAccept(TrkVisitor* vis) const
00523 {
00524
00525 vis->trkVisitCircleTraj(this);
00526 }
00527
00528 double
00529 TrkCircleTraj::angle(const double& f) const
00530 {
00531 return BesAngle(phi0() + arc(f));
00532 }