00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include "MdcGeom/Constants.h"
00022 #include "MdcGeom/BesAngle.h"
00023 #include <math.h>
00024 #include "MdcxReco/MdcxHel.h"
00025 #include "MdcxReco/MdcxHit.h"
00026 #include "MdcxReco/MdcxParameters.h"
00027 using std::cout;
00028 using std::endl;
00029
00030
00031
00032 MdcxHel::MdcxHel( ){ }
00033
00034 MdcxHel::MdcxHel
00035 (double D0, double Phi0, double Omega, double Z0, double Tanl,
00036 double T0, int Code, int Mode, double X, double Y) :
00037 d0(D0), phi0(Phi0), omega(Omega), z0(Z0), tanl(Tanl), t0(T0),
00038 xref(X), yref(Y), code(Code), mode(Mode), omin(0.000005) {
00039
00040 ominfl = (fabs(omega) < omin) ? 0 : 1;
00041 double m_2pi = 2.0*M_PI;
00042 if (phi0 > M_PI) phi0 -= m_2pi;
00043 if (phi0 < -M_PI) phi0 += m_2pi;
00044 cphi0 = cos(phi0);
00045 sphi0 = sin(phi0);
00046 x0 = X0();
00047 y0 = Y0();
00048 xc = Xc();
00049 yc = Yc();
00050 decode(code, qd0, qphi0, qomega, qz0, qtanl, qt0, nfree);
00051 turnflag = 1;
00052
00053 }
00054
00055 MdcxHel::~MdcxHel( ){ }
00056
00057
00058
00059 double MdcxHel::Xc()const {
00060 if(ominfl) {
00061
00062 return (x0 - sphi0/omega);
00063 } else {
00064 return 999999999.9;
00065 }
00066 }
00067
00068 double MdcxHel::Yc()const {
00069 if(ominfl) {
00070
00071 return (y0 + cphi0/omega);
00072 } else {
00073 return 999999999.9;
00074 }
00075 }
00076
00077 double MdcxHel::X0()const{
00078 return (xref - sphi0*d0);
00079 }
00080
00081 double MdcxHel::Y0()const{
00082 return (yref + cphi0*d0);
00083 }
00084
00085 double MdcxHel::Xh(double l)const{
00086 if(ominfl){
00087 double phit=phi0+omega*l;
00088 return (xc+sin(phit)/omega);
00089 }else{
00090 return (x0+cphi0*l-0.5*l*l*omega*sphi0);
00091 }
00092 }
00093
00094 double MdcxHel::Yh(double l)const{
00095 if(ominfl){
00096 double phit=phi0+omega*l;
00097 return (yc-cos(phit)/omega);
00098 }else{
00099 return (y0+sphi0*l+0.5*l*l*omega*cphi0);
00100 }
00101 }
00102
00103 double MdcxHel::Zh(double l)const{
00104 return (z0+tanl*l);
00105 }
00106
00107 double MdcxHel::Px(double l) const {
00108 if(ominfl) {
00109 double phit = phi0 + omega*l;
00110 return 0.003*cos(phit)/fabs(omega);
00111 } else {
00112 return 1000.0*cphi0;
00113 }
00114 }
00115
00116 double MdcxHel::Py(double l) const {
00117 if(ominfl) {
00118 double phit = phi0+omega*l;
00119 return 0.003*sin(phit)/fabs(omega);
00120 } else {
00121 return 1000.0*sphi0;
00122 }
00123 }
00124
00125 double MdcxHel::Pz(double l) const {
00126 if(ominfl) {
00127 return 0.003*tanl/fabs(omega);
00128 }
00129 else{
00130 return 1000.0*tanl;
00131 }
00132 }
00133
00134 double MdcxHel::Ptot(double l)const{
00135 if(ominfl) {
00136 return 0.003*sqrt(1.0+tanl*tanl)/fabs(omega);
00137 } else {
00138 return 1000.0*sqrt(1.0+tanl*tanl);
00139 }
00140 }
00141
00142 double MdcxHel::Lmax() const {
00143 double lmax = MdcxParameters::maxTrkLength;
00144 if (ominfl) {
00145 double rmax = 1.0/fabs(omega);
00146 double dmax = fabs(d0) + 2.0*rmax;
00147 if (dmax > MdcxParameters::maxMdcRadius) lmax = M_PI*rmax;
00148 }
00149 return lmax;
00150 }
00151
00152
00153
00154 void MdcxHel::SetMode(int n) { mode = n; }
00155
00156 void MdcxHel::SetRef(double x, double y) {
00157 xref = x;
00158 yref = y;
00159 }
00160
00161 void MdcxHel::SetOmega(int Qomega) {
00162 nfree = nfree + deltaq(qomega, Qomega);
00163 code = code + deltaq(qomega, Qomega)*100;
00164 qomega = Qomega;
00165 }
00166 void MdcxHel::SetPhi0(int Qphi0) {
00167 nfree = nfree + deltaq(qphi0, Qphi0);
00168 code = code + deltaq(qphi0, Qphi0)*10;
00169 qphi0 = Qphi0;
00170 }
00171 void MdcxHel::SetD0(int Qd0) {
00172 nfree = nfree + deltaq(qd0, Qd0);
00173 code = code + deltaq(qd0, Qd0);
00174 qd0 = Qd0;
00175 }
00176 void MdcxHel::SetTanl(int Qtanl) {
00177 nfree = nfree + deltaq(qtanl, Qtanl);
00178 code = code + deltaq(qtanl, Qtanl)*10000;
00179 qtanl = Qtanl;
00180 }
00181 void MdcxHel::SetZ0(int Qz0) {
00182 nfree = nfree + deltaq(qz0, Qz0);
00183 code = code + deltaq(qz0, Qz0)*1000;
00184 qz0 = Qz0;
00185 }
00186 void MdcxHel::SetT0(int Qt0) {
00187 nfree = nfree + deltaq(qt0, Qt0);
00188 code = code + deltaq(qt0, Qt0)*100000;
00189 qt0 = Qt0;
00190 }
00191
00192
00193 MdcxHel& MdcxHel::operator=(const MdcxHel& rhs) {
00194 copy(rhs);
00195 return *this;
00196 }
00197
00198
00199 void MdcxHel::decode(const int code,int& i1,int& i2,
00200 int& i3,int& i4,int& i5,int& i6,int& n)
00201 {
00202 int temp = code;
00203 temp=temp/1000000; temp=code-1000000*temp;
00204 i6=temp/100000; temp=temp-100000*i6;
00205 i5=temp/10000; temp=temp-10000*i5;
00206 i4=temp/1000; temp=temp-1000*i4;
00207 i3=temp/100; temp=temp-100*i3;
00208 i2=temp/10; i1=temp-10*i2;
00209 n = 0;
00210 if(i6 == 1) n++; else i6 = 0;
00211 if(i5 == 1) n++; else i5 = 0;
00212 if(i4 == 1) n++; else i4 = 0;
00213 if(i3 == 1) n++; else i3 = 0;
00214 if(i2 == 1) n++; else i2 = 0;
00215 if(i1 == 1) n++; else i1 = 0;
00216 }
00217
00218
00219 void MdcxHel::copy(const MdcxHel& rhs)
00220 {
00221
00222 omega=rhs.Omega(); phi0=rhs.Phi0(); d0=rhs.D0(); t0=rhs.T0();
00223 tanl=rhs.Tanl(); z0=rhs.Z0();
00224 cphi0=rhs.CosPhi0(); sphi0=rhs.SinPhi0();
00225 x0=rhs.X0(); y0=rhs.Y0(); xc=rhs.Xc(); yc=rhs.Yc();
00226 xref=rhs.Xref(); yref=rhs.Yref();
00227 qomega=rhs.Qomega(); qphi0=rhs.Qphi0(); qd0=rhs.Qd0(); qt0=rhs.Qt0();
00228 qtanl=rhs.Qtanl(); qz0=rhs.Qz0();
00229 mode=rhs.Mode(); nfree=rhs.Nfree();
00230 code=rhs.Code(); ominfl=rhs.Ominfl(); omin=rhs.Omin();
00231 turnflag=rhs.GetTurnFlag();
00232 }
00233
00234 double MdcxHel::Doca(const MdcxHit& h) {
00235
00236 return Doca( h.wx(), h.wy(), h.wz(), h.x(), h.y() );
00237 }
00238
00239 double MdcxHel::Doca( double wx, double wy, double wz,
00240 double xi, double yi, double zi )
00241 {
00242 double m_2pi = 2.0*M_PI;
00243
00244
00245 Hep3Vector ivec(xi, yi, zi);
00246 wvec = Hep3Vector(wx, wy, wz);
00247
00248
00249 double zd, xd = xi, yd = yi;
00250
00251 double lnew,t1,t2,dphi,dlen=1000.0;
00252 len = 0.0;
00253 int itry = 2;
00254
00255
00256 double circut, circum = 10000.;
00257 if (ominfl) circum = m_2pi/fabs(omega);
00258 circut = 0.50 * circum;
00259 while (itry) {
00260 if (ominfl) {
00262 t1 = -xc + xd; t2 = yc - yd; phi = atan2(t1, t2);
00263 if (omega < 0.0) phi += M_PI;
00264 if (phi > M_PI) phi -= m_2pi;
00265 dphi = phi - phi0;
00266 if (omega < 0.0){
00267 if (dphi > 0.0) dphi -= m_2pi;
00268 if (dphi < -m_2pi) dphi += m_2pi;
00269 }else{
00270 if (dphi < 0.0) dphi += m_2pi;
00271 if (dphi > m_2pi) dphi -= m_2pi;
00272 }
00273 lnew = dphi/omega;
00274
00275
00276 if ((lnew>circut)&&(turnflag)) lnew -= circum;
00277
00278 zh = Zh(lnew);
00279 xd=xi+(zh-zi)*wx/wz; yd=yi+(zh-zi)*wy/wz; zd=zh;
00280
00281
00282 dlen=fabs(lnew-len); len=lnew;
00283
00284
00285
00286 if (fabs(zh) > MdcxParameters::maxMdcZLen) break;
00287 if ( (0.0==wx) && (0.0==wy) )break; if (dlen < 0.000001)break; itry--;
00288 } else {
00289 len = (xi-xref)*cphi0 + (yi-yref)*sphi0;
00290 zh = z0 + tanl*len;
00291 phi = phi0;
00292 break;
00293 }
00294 }
00295
00296 xh = Xh(len); yh = Yh(len);
00297 Hep3Vector hvec(xh, yh, zh);
00298
00299 double lamb = atan(tanl);
00300 cosl = cos(lamb); sinl = sin(lamb);
00301 tx = cosl*cos(phi); ty = cosl*sin(phi); tz = sinl;
00302 tvec = Hep3Vector(tx, ty, tz);
00303 Hep3Vector vvec = wvec.cross(tvec);
00304 vhat = vvec.unit(); vx = vhat.x(); vy = vhat.y(); vz = vhat.z();
00305
00306 dvec = ivec - hvec;
00307 double doca = dvec*vhat;
00308
00309 double f1 = dvec*tvec; double f2 = wvec*tvec; double f3 = dvec*wvec;
00310 f0 = (f1 - f2*f3) / (1.0 - f2*f2);
00311 samb = (doca > 0.0) ? -1 : +1;
00312 double wirephi = atan2(yd, xd);
00313 eang = BesAngle(phi-wirephi);
00314 wamb = (fabs(eang) < Constants::pi/2) ? samb : -samb;
00315
00316
00317 if (fabs(zh) > MdcxParameters::maxMdcZLen) doca = 1000.0;
00318
00319
00320
00321 return doca;
00322 }
00323
00324 std::vector<float> MdcxHel::derivatives(const MdcxHit& hit)
00325 {
00326 double doca = Doca(hit);
00327 std::vector<float> temp(nfree+1);
00328 temp[0] = doca;
00329 double fac = 1.0;
00330 if((mode==0) && (doca<0.0)) fac = -fac;
00331 if(mode == 0) temp[0] = fabs(temp[0]);
00332
00333 int bump = 0;
00334 if (qd0) temp[++bump] = (-vx*sphi0 + vy*cphi0) * fac;
00335 if (qphi0) {
00336
00337 double dddp0 = -(yh-y0+f0*ty)*vx + (xh-x0+f0*tx)*vy;
00338 dddp0 *= (1.0 + d0*omega);
00339 temp[++bump] = dddp0*fac;
00340 }
00341 if (qomega) {
00342 double dddom;
00343 if (ominfl) {
00344 dddom = ((len*cos(phi)-xh+x0)*vx + (len*sin(phi)-yh+y0)*vy)/omega;
00345 dddom += f0*len*cosl*(-sin(phi)*vx+cos(phi)*vy);
00346 } else {
00347 dddom = 0.5*len*len*(-vx*sphi0+vy*cphi0);
00348 }
00349 temp[++bump] = dddom * fac;
00350 }
00351 if (qz0) temp[++bump] = vz * fac;
00352 if (qtanl) temp[++bump] = (vz *len) * fac;
00353 if (qt0) temp[++bump] = -hit.v();
00354 return temp;
00355 }
00356
00357 void MdcxHel::print() const {
00358 cout << "MdcxHel(";
00359 cout << d0<<",";
00360 cout << phi0<<",";
00361 cout << omega<<",";
00362 cout << z0<<",";
00363 cout << tanl<<")"<<endl;
00364 cout << " t0 = " << t0 ;
00365 cout << " nfree = " << nfree ;
00366 cout << " (x0,y0) " << x0<<","<<y0;
00367 cout << " (xc,yc) " << xc<<","<<yc;
00368 cout << " (xref,yref) " << xref<<","<<yref;
00369 cout << " code = " << code;
00370 cout << " mode = " << mode;
00371 cout << " ominfl = " << ominfl;
00372 cout << " " << endl;
00373 }
00374
00375 void MdcxHel::flip() {
00376 double m_2pi = 2.0*M_PI;
00377 if (ominfl) {
00378 if ( (fabs(d0) + 2.0/fabs(omega)) > 80.0 ) return;
00379 double lturn = m_2pi/fabs(omega);
00380 double zturn = Zh(lturn);
00381
00382 if (fabs(zturn) < fabs(z0)) {
00383 z0 = zturn;
00384 tanl = -tanl;
00385 omega = -omega;
00386 d0 = -d0;
00387 phi0 = phi0 - M_PI;
00388 if (phi0 < -M_PI) phi0 += m_2pi;
00389 cphi0 = cos(phi0);
00390 sphi0 = sin(phi0);
00391 x0 = X0();
00392 y0 = Y0();
00393 }
00394 }
00395 }
00396