00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include "MdcTrkRecon/MdcSegInfoSterO.h"
00022 #include "MdcGeom/MdcSuperLayer.h"
00023 #include "TrkBase/TrkRecoTrk.h"
00024 #include "MdcGeom/BesAngle.h"
00025 #include "MdcTrkRecon/MdcSegWorks.h"
00026 #include "MdcTrkRecon/MdcSeg.h"
00027 #include "CLHEP/Alist/AList.h"
00028 #include "MdcTrkRecon/mdcTwoInv.h"
00029 #include "MdcGeom/MdcDetector.h"
00030 #include "TrkBase/TrkExchangePar.h"
00031 #include "TrkBase/TrkHelixUtils.h"
00032 #include "TrkBase/TrkFit.h"
00033 #include "MdcTrkRecon/MdcLine.h"
00034 #include "MdcData/MdcHit.h"
00035 #include "MdcData/MdcHitUse.h"
00036 #include "CLHEP/Geometry/Point3D.h"
00037 #include "CLHEP/Geometry/Vector3D.h"
00038 #include <math.h>
00039 #include "BField/BField.h"
00040 #include "MdcGeom/Constants.h"
00041 using std::endl;
00042 #ifndef ENABLE_BACKWARDS_COMPATIBILITY
00043 typedef HepGeom::Vector3D<double> HepVector3D;
00044 #endif
00045
00046
00047 bool MdcSegInfoSterO::parIsAngle(int i) const {
00048
00049 assert (i >= 0 && i < 2);
00050 return false;
00051 }
00052
00053 int MdcSegInfoSterO::calcStereo(MdcSeg *parentSeg,
00054 const TrkRecoTrk &track, MdcSegWorks &segStuff) {
00055
00056 const TrkFit* tkFit = track.fitResult();
00057 if (tkFit == 0) return 1;
00058 TrkExchangePar par = tkFit->helix(0.0);
00059 double Bz = track.bField().bFieldZ();
00060 return calcStereo(parentSeg, par, segStuff,Bz);
00061 }
00062
00063 int MdcSegInfoSterO::calcStereo(MdcSeg *parentSeg,
00064 const TrkExchangePar &par, MdcSegWorks &segStuff,double Bz) {
00065
00066 _debug = 0;
00067
00068
00069 const MdcSuperLayer *slayer = parentSeg->superlayer();
00070
00071 double kap = 0.5 * par.omega();
00072 double d0 = par.d0();
00073 BesAngle phi0(par.phi0());
00074 BesAngle phiSeg(parentSeg->phi());
00075 double radius = slayer->rEnd();
00076 double dPhiZ = (-1)*slayer->delPhiinv() * slayer->zEnd();
00077 if (_debug >0){
00078 parentSeg->plotSeg();
00079 std::cout << "delPhi "<<slayer->delPhi()
00080 <<" delPhiInv "<<slayer->delPhiinv()
00081 <<" dPhiZ "<<dPhiZ
00082 <<" zEnd() "<<slayer->zEnd()
00083 <<" phiAx "<<segStuff.phiAx
00084 <<" phiSeg "<<phiSeg
00085 <<" phiDiff "<<phiSeg - segStuff.phiAx<< std::endl;
00086 }
00087 bool lStraight = false;
00088 if (fabs(kap)<1.e-9) lStraight = true;
00089
00090 BesAngle phiDiff = phiSeg - segStuff.phiAx;
00091 double zApprox = phiDiff.rad() * -dPhiZ;
00092 if (_debug >0){
00093 std::cout<<"zApp "<<zApprox<<std::endl;
00094 }
00095 double delRad = slayer->stDip() *
00096 (1. - zApprox * zApprox * segStuff.wirLen2inv);
00097 radius -= delRad;
00098 double delPhiArg;
00099 if (lStraight) {
00100 delPhiArg = delRad * d0 / (radius*radius);
00101 }else {
00102 delPhiArg = -delRad * kap;
00103 }
00104
00105
00106
00107 MdcLine span(12);
00108 int hitFit=0;
00109 bool szFaild = false;
00110 double arcTemp=0;
00111
00112 for (int ihit = 0; ihit < parentSeg->nHit(); ihit++){
00113
00114
00115 int szCode = zPosition(*(parentSeg->hit(ihit)), par, &span,hitFit,parentSeg->bunchTime(),Bz);
00116
00117
00118
00119 if (szCode > 0){
00120 span.sigma[hitFit] = 1;
00121 arcTemp += span.x[hitFit];
00122 hitFit ++;
00123 }else{
00124 szFaild = true;
00125 if (_debug >0){
00126 parentSeg->hit(ihit)->mdcHit()->print(std::cout);
00127 std::cout<<"MdcSegInfoSterO hit "<<ihit<<" z calc faild"<<std::endl;
00128 }
00129 }
00130 }
00131 if (hitFit >0) span.fit(hitFit);
00132
00133
00134
00135 segStuff.phiAx += delPhiArg +
00136 0.5 * delPhiArg * segStuff.phiArg * segStuff.phiArg;
00137 phiDiff = phiSeg - segStuff.phiAx;
00138 double z = phiDiff.rad() * -dPhiZ;
00139 if (_debug >0){
00140 std::cout<<"z "<<z<<std::endl;
00141 }
00142 double ct, z0, arc;
00143 if (lStraight) {
00144
00145 double arg = radius*radius - d0*d0;
00146 if (arg <= 0.0) return 1;
00147 double rD0Root = sqrt(arg);
00148 double rD0Rinv = 1. / rD0Root;
00149 double rinv = 1./radius;
00150
00151 double slope = parentSeg->slope();
00152 ct = dPhiZ * (rD0Root * slope + d0 * rinv) * rinv;
00153
00154 arc = TrkHelixUtils::fltToRad(par, radius);
00155 if (arc == 0.0) return 1;
00156 z0 = z - ct * arc;
00157
00158 if (_debug>0){
00159 std::cout << "in MdcSegInfoSterO : z0 "<<z0 <<" "<<_errmat[0]
00160 <<" ct "<<ct<<" "<<_errmat[1]
00161 <<" arc "<<arc<<" "<<_errmat[2]
00162 <<endl;
00163 }
00164
00165 double dctdm = dPhiZ * rD0Root * rinv;
00166 double dctdD = -dPhiZ * rinv * (slope * d0 * rD0Rinv - rinv);
00167 double dzdm = -arc * dPhiZ * rD0Root * rinv;
00168 double dzdphi = dPhiZ;
00169 double dzdphi0 = -dPhiZ;
00170 double dzdD = -dPhiZ + ct * d0 * rD0Rinv - arc * dctdD;
00171
00172 const double *inErr = parentSeg->errmat();
00173
00174 const HepMatrix trackErr = par.covariance();
00175 _errmat[0] = dzdm * dzdm * inErr[2] +
00176 2 * dzdm * dzdphi * inErr[1] +
00177 dzdphi * dzdphi * inErr[0] +
00178 dzdD * dzdD * trackErr(1,1) +
00179 dzdphi0 * dzdphi0 * trackErr(2,2) +
00180 2 * dzdD * dzdphi0 * trackErr(1,2);
00181 if (_errmat[0] < 0.) _errmat[0] = 0.;
00182
00183
00184 _errmat[2] = dctdm * dctdm * inErr[2] +
00185 dctdD * dctdD * trackErr(1,1);
00186 if (_errmat[2] < 0.) _errmat[2] = 0.;
00187
00188
00189 _errmat[1] = dzdm * dctdm * inErr[2] +
00190 dzdphi * dctdm * inErr[1] +
00191 dzdD * dctdD * trackErr(1,1) +
00192 dzdphi0 * dctdD * trackErr(1,2);
00193 }
00194 else {
00195
00196
00197 double arg = 1. - kap*kap * radius*radius;
00198 if (arg < 0.0) return 1;
00199 double rKapRoot = sqrt(arg);
00200 double rKapRinv = 1. / rKapRoot;
00201 double ctFactor = -rKapRoot * -dPhiZ;
00202 double slopeAx = kap * rKapRinv;
00203 double slope = parentSeg->slope();
00204
00205
00206
00207
00208
00209
00210
00211 if (szFaild){
00212
00213 ct = (slopeAx - slope) * ctFactor;
00214 arc = TrkHelixUtils::fltToRad(par, radius);
00215 if (arc == 0.0) return 1;
00216 z0 = z - ct * arc;
00217 }else{
00218
00219 if (!szFaild){
00220 arc = arcTemp/hitFit;
00221 ct = span.slope;
00222 z0 = span.intercept;
00223 }else{
00224 ct = 999;
00225 z0 = 999;
00226 arc = 999;
00227 }
00228 }
00229 if (_debug >0){
00230 std::cout<<"--slayer NO. "<<slayer->index()<<std::endl;
00231 std::cout<<"ori ct "<<(slopeAx - slope) * ctFactor
00232 <<" z0 "<<z - ct * TrkHelixUtils::fltToRad(par, radius)
00233 <<" arc "<<TrkHelixUtils::fltToRad(par, radius)<<std::endl;
00234 std::cout<<"fix ct "<<span.slope<<" z0 "<<span.intercept
00235 <<" arc "<<arcTemp/hitFit<<std::endl;
00236 std::cout<<"-------- "<<std::endl;
00237 }
00238
00239 double dctdm = dPhiZ * rKapRoot;
00240 double dctdkap = -dPhiZ * ( 1 + radius * radius * kap * rKapRinv * slope);
00241 double dzdm = arc * -dPhiZ * rKapRoot;
00242 double dzdphi = dPhiZ;
00243 double dzdkap = dPhiZ * radius * rKapRinv - arc * dctdkap -
00244 ct * ( radius * rKapRinv / kap - arc / kap);
00245 double dzdphi0 = -dPhiZ ;
00246
00247 const double *inErr = parentSeg->errmat();
00248
00249 const HepMatrix trackErr = par.covariance();
00250 _errmat[0] = dzdm * dzdm * inErr[2] +
00251 2 * dzdm * dzdphi * inErr[1] +
00252 dzdphi * dzdphi * inErr[0] +
00253 dzdkap * dzdkap * trackErr(3,3) +
00254 dzdphi0 * dzdphi0 * trackErr(2,2) +
00255 2 * dzdkap * dzdphi0 * trackErr(2,3);
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269 if (_errmat[0] < 0.) _errmat[0] = 0.;
00270
00271
00272 _errmat[2] = dctdm * dctdm * inErr[2] +
00273 dctdkap * dctdkap * trackErr(3,3);
00274 if (_errmat[2] < 0.) _errmat[2] = 0.;
00275
00276
00277 _errmat[1] = dzdm * dctdm * inErr[2] +
00278 dzdphi * dctdm * inErr[1] +
00279 dzdkap * dctdkap * trackErr(3,3) +
00280 dzdphi0 * dctdkap * trackErr(2,3);
00281
00282 }
00283
00284
00285
00286 _par0 = z0;
00287 _par1 = ct;
00288 _arc = arc;
00289
00290 long error = mdcTwoInv(_errmat, _inverr);
00291 if (error) {
00292 std::cout << " ErrMsg(warning)" << "Failed to invert matrix -- MdcSegInfo::calcStereo" <<
00293 endl << _errmat[0] << " " << _errmat[1] << " " << _errmat[2]<< std::endl;
00294 }
00295 return 0;
00296
00297 }
00298
00299 int MdcSegInfoSterO::zPosition(MdcHitUse & hitUse, const TrkExchangePar &par, MdcLine* span,int hitFit, double t0, double Bz) const{
00300
00301 int debug =0;
00302
00303
00304 const MdcHit & h = *(hitUse.mdcHit());
00305
00306 HepPoint3D fp ( h.wire()->getWestPoint()->x(),h.wire()->getWestPoint()->y(),h.wire()->getWestPoint()->z());
00307 HepPoint3D rp ( h.wire()->getEastPoint()->x(),h.wire()->getEastPoint()->y(),h.wire()->getEastPoint()->z());
00308
00309
00310 HepVector3D X = 0.5 * (fp + rp);
00311 if(debug>0){
00312 std::cout<<"---------- "<<std::endl;
00313 h.print(std::cout);
00314
00315 std::cout<<"fp rp "<<fp<<" "<<rp<<std::endl;
00316 std::cout<<"Xmid "<<X<<std::endl;
00317 }
00318 HepVector3D xx = HepVector3D(X.x(), X.y(), 0.);
00319
00320
00321
00322 double d0 = -par.d0();
00323 double phi0 = (par.phi0()-pi/2);
00324 double _charge = -1;
00325 if((-1)*par.omega()/Bz > 0) _charge = 1;
00326
00327 double r;
00328 if (fabs(par.omega())< Constants::epsilon) r = 9999.;
00329 else r = 1/par.omega();
00330
00331 double xc = sin(par.phi0()) *(-d0+r) * -1.;
00332 double yc = -1.*cos(par.phi0()) *(-d0+r) * -1;
00333 HepPoint3D center (xc, yc, 0. );
00334
00335 HepVector3D yy = center - xx;
00336 HepVector3D ww = HepVector3D(yy.x(), yy.y(), 0.);
00337 double wwmag2 = ww.mag2();
00338 double wwmag = sqrt(wwmag2);
00339 int ambig = hitUse.ambig();
00340
00341 double driftdist = fabs( h.driftDist(t0,ambig) );
00342 HepVector3D lr(driftdist/wwmag * ww.x(),
00343 driftdist/wwmag * ww.y(), 0.);
00344 if (debug >0){
00345 std::cout<<"xc "<<xc << " yc "<<yc<< " drfitdist "<<driftdist<<std::endl;
00346 std::cout<<"r1 "<<r <<" d0 "<<d0 <<" phi0 "<<phi0<<std::endl;
00347 std::cout<<"lr "<<lr<<" hit ambig "<<hitUse.ambig()<<" ambig "<<ambig
00348 <<" left "<<h.driftDist(0,1)
00349 <<" right "<<h.driftDist(0,-1)<<std::endl;
00350 }
00351
00352
00353 HepPoint3D ORIGIN = HepPoint3D(0., 0., 0.);
00354
00355
00356
00357 if (ambig == 0) lr = ORIGIN;
00358 if (_charge*Bz < 0){
00359 if (ambig == -1){
00360 lr = - lr;
00361 }
00362 }else{
00363 if (ambig == 1){
00364 lr = - lr;
00365 }
00366 }
00367 X += lr;
00368
00369
00370
00371 HepPoint3D tmp(-9999., -9999., 0.);
00372 HepVector3D x = HepVector3D(X.x(), X.y(), 0.);
00373 HepVector3D w = x - center;
00374
00375 HepVector3D V = h.wire()->zAxis();
00376 HepVector3D v = HepVector3D(V.x(), V.y(), 0.);
00377 double vmag2 = v.mag2();
00378
00379
00380
00381 double wv = w.dot(v);
00382
00383 double d2 = wv * wv - vmag2 * (w.mag2() - r * r);
00384 if (debug >0){
00385 std::cout<<"X_fix "<<X<<" center "<<center<<std::endl;
00386 std::cout<<"V "<<V<<std::endl;
00387 std::cout<<"w "<<w<<" v "<<v<<std::endl;
00388 }
00389
00390
00391 if (d2 < 0.) {
00392
00393 if (debug>0){
00394 std::cout << "in MdcSegInfoSterO !!! stereo: 0. > d2 = " << d2 << " "
00395 << hitUse.ambig() << std::endl;
00396 }
00397 return -1;
00398 }
00399 double d = sqrt(d2);
00400
00401
00402 double l[2];
00403 if (debug >0){
00404 std::cout<<"wv "<<wv<<" d "<<d<<" vmag2 "<<vmag2<<std::endl;
00405 }
00406 l[0] = (- wv + d) / vmag2;
00407 l[1] = (- wv - d) / vmag2;
00408
00409
00410 bool ok[2];
00411 ok[0] = true;
00412 ok[1] = true;
00413 double z[2];
00414 z[0] = X.z() + l[0] * V.z();
00415 z[1] = X.z() + l[1] * V.z();
00416 if (debug >0){
00417 std::cout << "X.z "<<X.z()<<" V.z "<<V.z()<<std::endl;
00418 std::cout << "l0, l1 = " << l[0] << ", " << l[1] << std::endl;
00419 std::cout << "rpz " << rp.z() << " fpz " << fp.z() << std::endl;
00420 std::cout << "z0 " << z[0] << " z1 " << z[1] << std::endl;
00421 }
00422
00423 if (hitUse.ambig() == 0) {
00424 if(debug>0)std::cout<< " ambig = 0 " <<std::endl;
00425 if (z[0] > rp.z()+20. || z[0] < fp.z()-20.) {
00426 ok[0] = false;
00427 }
00428 if (z[1] > rp.z()+20. || z[1] < fp.z()-20.) {
00429 ok[1] = false;
00430 }
00431 } else {
00432 if(debug>0)std::cout<< " ambig != 0 " <<std::endl;
00433
00434 if (fabs(z[0]/rp.z())>1.05 ) { ok[0] = false; }
00435
00436 if (fabs(z[1]/rp.z())>1.05 ) { ok[1] = false; }
00437 }
00438 if ((! ok[0]) && (! ok[1])) {
00439 if(debug>0&&(hitUse.ambig()!=0)&&fabs(z[1]/rp.z())>1.05)std::cout<< " z[1] bad " << std::endl;
00440 if(debug>0&&(hitUse.ambig()!=0)&&fabs(z[0]/rp.z())>1.05)std::cout<< " z[0] bad " << std::endl;
00441
00442 if(debug>0) {
00443 std::cout<< " z[1] bad " << "rpz " << rp.z() << " fpz " << fp.z()
00444 << "z0 " << z[0] << " z1 " << z[1] << std::endl;
00445 std::cout<< " !ok[0] && !ok[1] return -2" <<std::endl;
00446 }
00447 return -2;
00448 }
00449
00450
00451 HepVector3D p[2];
00452 p[0] = x + l[0] * v;
00453 p[1] = x + l[1] * v;
00454
00455 if(debug>0){
00456 std::cout<<__FILE__<<" "<<__LINE__<< " p0 "<<p[0].x()<<" "<<p[0].y()<< std::endl;
00457 std::cout<<__FILE__<<" "<<__LINE__<< " p1 "<<p[1].x()<<" "<<p[1].y()<< std::endl;
00458 std::cout<<__FILE__<<" "<<__LINE__<< " c "<<center.x()<<" "<<center.y()<<" "<<_charge<< std::endl;
00459 std::cout<<__FILE__<<" "<<__LINE__<< " p0 centerx*y "<<center.x() * p[0].y()<<" centery*x"<<center.y() * p[0].x()<< std::endl;
00460 std::cout<<__FILE__<<" "<<__LINE__<< " p1 centerx*y "<<center.x() * p[1].y()<<" centery*x"<<center.y() * p[1].x()<< std::endl;
00461 }
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476 unsigned best = 0;
00477 if (ok[1]) best = 1;
00478
00479
00480
00481 double cosdPhi = - center.dot((p[best] - center).unit()) / center.mag();
00482 double dPhi;
00483 if(fabs(cosdPhi)<=1.0) {
00484 dPhi = acos(cosdPhi);
00485 } else if (cosdPhi>1.0) {
00486 dPhi = 0.0;
00487 } else {
00488 dPhi = pi;
00489 }
00490
00491
00492 tmp.setX(abs(r * dPhi));
00493 tmp.setY(z[best]);
00494
00495
00496
00497 span->y[hitFit] = z[best];
00498
00499
00500
00501 span->x[hitFit] = abs(r * dPhi);
00502 if (hitUse.ambig()<0) driftdist *= -1.;
00503 span->sigma[hitFit] = h.sigma(driftdist,hitUse.ambig());
00504
00505 if (debug >0){
00506 std::cout<<"("<<h.layernumber()<<","<<h.wirenumber()<<") s "<<span->x[hitFit]<<" z "<<span->y[hitFit]<<std::endl;
00507 }
00508 return 1;
00509 }