00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "TrkBase/TrkHelixUtils.h"
00016 #include "TrkBase/NeutParams.h"
00017 #include "CLHEP/Geometry/Point3D.h"
00018 #include "CLHEP/Matrix/Matrix.h"
00019 #include "CLHEP/Matrix/SymMatrix.h"
00020 #include "CLHEP/Vector/ThreeVector.h"
00021 #include <math.h>
00022 #include "TrkBase/TrkExchangePar.h"
00023 #include "BField/BField.h"
00024 #include "MdcGeom/BesAngle.h"
00025 #include "MdcRecoUtil/BesPointErr.h"
00026 #include "MdcRecoUtil/BesVectorErr.h"
00027 #include "MdcRecoUtil/DifNumber.h"
00028 #include "MdcRecoUtil/DifArray.h"
00029 #include "MdcGeom/Constants.h"
00030 const double pi = Constants::pi;
00031
00032
00033 HepMatrix TrkHelixUtils::jacobianExtrapolate(const TrkExchangePar& par,
00034 double fltNew) {
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055 HepMatrix transform(5, 5, 0);
00056
00057 double tanDip = par.tanDip();
00058 double omega = par.omega();
00059
00060 double darc = fltNew / sqrt(1. + tanDip * tanDip);
00061 double dphi = omega * darc;
00062 double cosDphi = cos(dphi);
00063 double sinDphi = sin(dphi);
00064
00065
00066
00067 transform[0][0] = cosDphi;
00068 transform[0][1] = sinDphi / omega;
00069 transform[0][2] = (1.0-cosDphi) / (omega * omega);
00070
00071
00072
00073 transform[1][0] = -sinDphi * omega;
00074 transform[1][1] = cosDphi;
00075 transform[1][2] = sinDphi / omega;
00076
00077
00078
00079 transform[2][2] = 1.0;
00080
00081
00082
00083 transform[3][0] = -tanDip * sinDphi;
00084 transform[3][1] = -tanDip * (1.0-cosDphi) / omega;
00085 transform[3][2] = -tanDip * (dphi-sinDphi) / (omega*omega);
00086 transform[3][3] = 1.;
00087 transform[3][4] = darc;
00088
00089
00090
00091 transform[4][4] = 1.;
00092
00093 return transform;
00094 }
00095
00096
00097
00098 HepSymMatrix TrkHelixUtils::extrapolateCov(TrkExchangePar& par,
00099 double fltNew) {
00100
00101
00102 return par.covariance().similarity(jacobianExtrapolate(par, fltNew));
00103 }
00104
00105
00106 TrkExchangePar TrkHelixUtils::helixFromMom(const HepPoint3D& pos,
00107 const Hep3Vector& pmom, double sign, const BField& fieldmap) {
00108
00109
00110
00111
00112
00113
00114
00115 register double phip,gamma,rho,pt;
00116 static HepVector pars(5);
00117
00118 double px = pmom.x();
00119 double py = pmom.y();
00120 pt=sqrt(px*px+py*py);
00121
00122 if (pt < 1.e-7) pt = 1.e-7;
00123 if (fabs(px) < 1.e-7) px = (px<0.0) ? -1.e-7 : 1.e-7;
00124
00125 double Bval = fieldmap.bFieldNominal();
00126
00127 pars(3)=-BField::cmTeslaToGeVc*Bval*sign/pt;
00128 pars(5)=pmom.z()/pt;
00129
00130 rho=1./pars(3);
00131 phip=atan2(py,px);
00132 double cosphip=cos(phip); double sinphip=sin(phip);
00133
00134 gamma=atan((pos.x()*cosphip+pos.y()*sinphip)/
00135 (pos.x()*sinphip-pos.y()*cosphip-rho));
00136
00137
00138 pars(2)=BesAngle(phip+gamma).rad();
00139 pars(1)=(rho+pos.y()*cosphip-pos.x()*sinphip)/cos(gamma)-rho;
00140 pars(4)=pos.z()+rho*gamma*pars(5);
00141
00142 return TrkExchangePar(pars);
00143 }
00144
00145
00146 TrkExchangePar TrkHelixUtils::helixFromMomErr(const BesPointErr& pos,
00147 const BesVectorErr& pmom,const HepMatrix& cxp, double sign,
00148 const BField& fieldmap) {
00149
00150
00151 DifNumber xDF(pos.x(),1,6), yDF(pos.y(),2,6), zDF(pos.z(),3,6);
00152 DifNumber pxDF(pmom.x(), 4, 6);
00153 DifNumber pyDF(pmom.y(), 5, 6);
00154 DifNumber pzDF(pmom.z(), 6, 6);
00155
00156 static DifNumber phip, cosphip, sinphip, gamma;
00157 static DifArray pars(5,6);
00158
00159 DifNumber invpt = pxDF;
00160 invpt *= pxDF;
00161 invpt += pyDF*pyDF;
00162 invpt = sqrt(invpt);
00163
00164 if (invpt < 1.e-7) invpt = 1.e-7;
00165 if (fabs(pxDF) < 1.e-7) pxDF = pxDF<0?-1e-7:1e-7;
00166 invpt = 1./invpt;
00167
00168
00169 double Bval = fieldmap.bFieldNominal();
00170
00171
00172
00173
00174
00175 pars(3) = invpt;
00176 pars(3) *= sign;
00177 pars(3) *= Bval;
00178 pars(3) *= -BField::cmTeslaToGeVc;
00179
00180
00181 pars(5) = pzDF;
00182 pars(5) *= invpt;
00183
00184 DifNumber rho = 1./pars[2];
00185 phip=atan2(pyDF,pxDF);
00186 phip.cosAndSin(cosphip,sinphip);
00187
00188
00189 pars(1) = rho;
00190 pars(1) += yDF*cosphip;
00191 pars(1) -= xDF*sinphip;
00192
00193 gamma=atan((xDF*cosphip+yDF*sinphip)/ -pars(1));
00194
00195 pars(1) /= cos(gamma);
00196 pars(1) -= rho;
00197
00198
00199 pars(2) = phip;
00200 pars(2) += gamma;
00201
00202
00203
00204
00205 pars(4) = pars[4];
00206 pars(4) *= gamma;
00207 pars(4) *= rho;
00208 pars(4) += zDF;
00209
00210
00211
00212
00213 static HepSymMatrix posandmomErr(6);
00214 static HepVector parsVec(5);
00215
00216 int i;
00217 for (i = 1; i <= 3; ++i) {
00218 int j;
00219 for (j = 1; j <= i; ++j) {
00220
00221 posandmomErr.fast(i,j) = pos.covMatrix().fast(i,j);
00222 posandmomErr.fast(i+3,j+3) = pmom.covMatrix().fast(i,j);
00223 }
00224 for (j = 1; j <= 3; ++j) {
00225 posandmomErr.fast(j+3,i) = cxp(i,j);
00226 }
00227 }
00228 for (i = 1; i <= 5; ++i) {
00229
00230
00231 parsVec(i) = pars(i).number();
00232 }
00233
00234
00235 return TrkExchangePar(parsVec,posandmomErr.similarity(pars.jacobian()) );
00236 }
00237
00238 NeutParams TrkHelixUtils::lineFromMomErr(const BesPointErr& pos,
00239 const BesVectorErr& pmom,const HepMatrix& cxp, double sign,
00240 const BField& fieldmap) {
00241
00242
00243 DifNumber xDF(pos.x(),1,6), yDF(pos.y(),2,6), zDF(pos.z(),3,6);
00244 DifNumber pxDF(pmom.x(), 4, 6);
00245 DifNumber pyDF(pmom.y(), 5, 6);
00246 DifNumber pzDF(pmom.z(), 6, 6);
00247
00248 static DifArray pars(6,6);
00249
00250 DifNumber pt = pxDF;
00251 pt *= pxDF;
00252 pt += pyDF*pyDF;
00253
00254 if (pt < 1.e-14) pt = 1.e-14;
00255 if (fabs(pxDF) < 1.e-7) pxDF = pxDF<0?-1e-7:1e-7;
00256
00257
00258 pars(3) = pzDF;
00259 pars(3) *= pzDF;
00260 pars(3) += pt;
00261 pars(3) = sqrt(pars(3));
00262
00263 pt = sqrt(pt);
00264 DifNumber invpt = 1./pt;
00265
00266
00267
00268
00269 DifNumber pvxDF=pxDF;
00270 pvxDF *= invpt;
00271
00272
00273 DifNumber pvyDF=pyDF;
00274 pvyDF *= invpt;
00275
00276
00277
00278 pars(5) = pzDF;
00279 pars(5) *= invpt;
00280
00281 pars(2) = atan2(pyDF,pxDF);
00282
00283
00284 pars(1) = yDF;
00285 pars(1) *= pvxDF;
00286 pars(1) -= xDF*pvyDF;
00287
00288
00289 pars(4) = xDF;
00290 pars(4) *= pvxDF;
00291 pars(4) += yDF*pvyDF;
00292
00293
00294
00295 pars(6) = pars(4);
00296 pars(6) *= pars(3);
00297 pars(6) *= invpt;
00298
00299
00300 pars(4) *= -pars(5);
00301 pars(4) += zDF;
00302
00303
00304
00305
00306
00307
00308 static HepSymMatrix posandmomErr(6);
00309 static HepVector parsVec(6);
00310
00311 int i;
00312 for (i = 1; i <= 3; ++i) {
00313 int j;
00314 for (j = 1; j <= i; ++j) {
00315
00316 posandmomErr.fast(i,j) = pos.covMatrix().fast(i,j);
00317 posandmomErr.fast(i+3,j+3) = pmom.covMatrix().fast(i,j);
00318 }
00319 for (j = 1; j <= 3; ++j) {
00320 posandmomErr.fast(j+3,i) = cxp(i,j);
00321 }
00322 }
00323 for (i = 1; i <= 6; ++i) {
00324
00325
00326 parsVec(i) = pars(i).number();
00327 }
00328
00329 return NeutParams(parsVec,posandmomErr.similarity(pars.jacobian()) );
00330 }
00331
00332
00333 double TrkHelixUtils::fltToRad(const TrkExchangePar& hel, double rad) {
00334
00335 double d0 = hel.d0();
00336 double omega = hel.omega();
00337 double tanDip = hel.tanDip();
00338
00339
00340
00341 if( fabs(omega) < 0.0001 ) omega = (omega<0.0) ? -0.0001 : 0.0001 ;
00342
00343 double stuff = ( rad*rad - d0*d0 ) / (1 + omega * d0);
00344 if (stuff <= 0.0) return 0.;
00345
00346 if (omega == 0.) return sqrt(stuff);
00347 double sinAngle = 0.5 * omega * sqrt(stuff);
00348 double dist2d = 0;
00349 if (sinAngle < -1.0 || sinAngle > 1.0) {
00350 dist2d = Constants::pi / fabs(omega);
00351 } else {
00352 dist2d = 2. * asin(sinAngle) / omega;
00353 }
00354
00355 return dist2d * sqrt(1. + tanDip*tanDip);
00356 }