00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "TrkBase/TrkPocaXY.h"
00015 #include "TrkBase/TrkPoca.h"
00016 #include "TrkBase/TrkExchangePar.h"
00017 #include "TrkBase/HelixTraj.h"
00018 #include "TrkBase/TrkLineTraj.h"
00019 #include "CLHEP/Vector/ThreeVector.h"
00020 using std::cout;
00021 using std::endl;
00022
00023 TrkPocaXY::TrkPocaXY(const Trajectory& traj, double flt,
00024 const HepPoint3D& pt, double prec)
00025 : TrkPocaBase(flt,prec) , _docaxy(-9999.0) {
00026
00027 Hep3Vector zaxis(0.0,0.0,1.0);
00028
00029
00030 TrkLineTraj zline(pt, zaxis, 2000.0);
00031
00032 double zlen(pt.z());
00033 TrkPoca zlinepoca(traj,flt,zline,zlen,prec);
00034
00035 _status = zlinepoca.status();
00036 if(status().success()){
00037 _flt1 = zlinepoca.flt1();
00038 _flt2 = zlinepoca.flt2();
00039 _docaxy = zlinepoca.doca();
00040 }
00041 }
00042
00043 TrkPocaXY::TrkPocaXY(const Trajectory& traj1,double flt1,
00044 const Trajectory& traj2,double flt2,
00045 double prec)
00046 : TrkPocaBase(flt1,flt2,prec) , _docaxy(-9999.0) {
00047
00048
00049
00050
00051
00052 _flt1 = flt1;
00053 _flt2 = flt2;
00054 double delta1(10*prec);
00055 double delta2(10*prec);
00056 unsigned niter(0);
00057 static const unsigned maxiter(20);
00058 while(niter < maxiter &&
00059 ( delta1 > prec || delta2 > prec ) ){
00060
00061 HepPoint3D pos1 = traj1.position(_flt1);
00062 HepPoint3D pos1b;
00063 if(niter == 0) pos1b = pos1;
00064 HepPoint3D pos2 = traj2.position(_flt2);
00065 Hep3Vector dir1 = traj1.direction(_flt1);
00066 Hep3Vector dir2 = traj2.direction(_flt2);
00067 Hep3Vector dd1 = traj1.delDirect(_flt1);
00068 Hep3Vector dd2 = traj2.delDirect(_flt2);
00069 double curv1 = traj1.curvature(_flt1);
00070 double curv2 = traj2.curvature(_flt2);
00071 double m1, m2, q1, q2;
00072 double r1, r2, xc1,xc2,yc1,yc2,sr1,sr2;
00073 if(curv1 == 0) {
00074
00075 if(dir1[0] == 0){
00076 m1 = 1.0e12;
00077 }else{
00078 m1 = dir1[1]/dir1[0];
00079 }
00080 q1 = pos1.y()-pos1.x()*m1;
00081 }else{
00082
00083 r1 = (1- dir1[2]*dir1[2])/curv1;
00084 sr1=r1;
00085 if(dir1[0]*dd1[1] - dir1[1]*dd1[0] < 0) sr1 = -r1;
00086 double cosphi1 = dir1[0]/sqrt(dir1[0]*dir1[0]+dir1[1]*dir1[1]);
00087 double sinphi1 = cosphi1 * dir1[1]/dir1[0];
00088 xc1 = pos1.x() - sr1 * sinphi1;
00089 yc1 = pos1.y() + sr1 * cosphi1;
00090 }
00091 if(curv2 == 0) {
00092 if(dir2[0] == 0){
00093 m2 = 1.0e12;
00094 }else{
00095 m2 = dir2[1]/dir2[0];
00096 }
00097 q2 = pos2.y()-pos2.x()*m2;
00098 }else{
00099 r2 = (1-dir2[2]*dir2[2])/curv2;
00100 sr2 = r2;
00101 if(dir2[0]*dd2[1] - dir2[1]*dd2[0] < 0) sr2 = -r2;
00102 double cosphi2 = dir2[0]/sqrt(dir2[0]*dir2[0]+dir2[1]*dir2[1]);
00103 double sinphi2 = cosphi2 * dir2[1]/dir2[0];
00104 xc2 = pos2.x() - sr2 * sinphi2;
00105 yc2 = pos2.y() + sr2 * cosphi2;
00106 }
00107 double xint, yint, xint1, xint2, yint1, yint2, absdoca;
00108 _docaxy = 0;
00109 _status.setSuccess(3);
00110
00111
00112
00113 if(curv1==0 && curv2==0){
00114
00115 interTwoLines(m1, q1, m2, q2, xint, yint);
00116 }
00117
00118
00119
00120 if(curv1 !=0 && curv2 !=0){
00121
00122 double cdist = sqrt((xc1-xc2)*(xc1-xc2)+(yc1-yc2)*(yc1-yc2));
00123
00124 if (fabs(cdist) < 1.e-12 ) {
00125
00126 _status.setFailure(12,
00127 "TrkPocaXY:: the two circles have the same center...");
00128 return;
00129 }
00130
00131 if ( (fabs(r1-r2) < cdist) && (cdist < r1+r2 ) ) {
00132 interTwoCircles(xc1,yc1,r1,xc2,yc2,r2,xint1,yint1,xint2,yint2);
00133 double dist1 = (xint1-pos1b.x())*(xint1-pos1b.x()) +
00134 (yint1-pos1b.y())*(yint1-pos1b.y());
00135 double dist2 = (xint2-pos1b.x())*(xint2-pos1b.x()) +
00136 (yint2-pos1b.y())*(yint2-pos1b.y());
00137
00138
00139 if(dist1<dist2){
00140 xint = xint1;
00141 yint = yint1;
00142 } else {
00143 xint = xint2;
00144 yint = yint2;
00145 }
00146 }
00147
00148
00149
00150 if ( (fabs(r1-r2) > cdist) ||
00151 ( cdist > (r1+r2) )) {
00152
00153
00154 double m = (yc1-yc2)/(xc1-xc2);
00155 double q = yc1 - m*xc1;
00156
00157
00158
00159 double xint1, yint1, xint2, yint2, zOfDCrossT1;
00160
00161 interLineCircle(m, q, xc1, yc1, r1, xint1, yint1, xint2, yint2);
00162
00163 double xint3, yint3, xint4, yint4 ;
00164 interLineCircle(m, q, xc2, yc2, r2, xint3, yint3, xint4, yint4);
00165 if (fabs(r1-r2) > cdist) {
00166 absdoca = fabs(r1-r2)-cdist;
00167 #ifdef MDCPATREC_DEBUG
00168 cout << "ErrMsg(debugging) doing nested circles in TrkPocaXY " << endl;
00169 #endif
00170
00171 double dist1_3 = pow((xint1-xint3),2.) + pow((yint1-yint3),2.);
00172 double dist2_4 = pow((xint2-xint4),2.) + pow((yint2-yint4),2.);
00173
00174 if(dist1_3<dist2_4) {
00175 xint = 0.5*(xint1+xint3);
00176 yint = 0.5*(yint1+yint3);
00177 zOfDCrossT1 = (xint3-xint1)*dir1[1]-(yint3-yint1)*dir1[0];
00178 } else {
00179 xint = 0.5*(xint2+xint4);
00180 yint = 0.5*(yint2+yint4);
00181 zOfDCrossT1 = (xint4-xint2)*dir1[1]-(yint4-yint2)*dir1[0];
00182 }
00183 _docaxy = absdoca;
00184 if( zOfDCrossT1 > 0) _docaxy = -absdoca;
00185 }
00186 if( cdist > (r1+r2) ) {
00187 absdoca = cdist - (r1+r2);
00188 #ifdef MDCPATREC_DEBUG
00189 cout << "ErrMsg(debugging) doing separated circles in TrkPocaXY"<< endl;
00190 #endif
00191
00192 double dist2_3 = pow((xint2-xint3),2.) + pow((yint2-yint3),2.);
00193 double dist1_4 = pow((xint1-xint4),2.) + pow((yint1-yint4),2.);
00194 if (dist2_3<dist1_4){
00195 xint = 0.5*(xint2+xint3);
00196 yint = 0.5*(yint2+yint3);
00197 zOfDCrossT1 = (xint3-xint1)*dir1[1]-(yint3-yint1)*dir1[0];
00198 } else {
00199 xint = 0.5*(xint1+xint4);
00200 yint = 0.5*(yint1+yint4);
00201 zOfDCrossT1 = (xint4-xint2)*dir1[1]-(yint4-yint2)*dir1[0];
00202 }
00203 _docaxy = absdoca;
00204 if( zOfDCrossT1 > 0) _docaxy = -absdoca;
00205 }
00206 }
00207 }
00208
00209
00210
00211 if((curv1 == 0 && curv2 !=0) || (curv1 != 0 && curv2 == 0)) {
00212
00213 HepVector dirT;
00214 double m,q,r,xc,yc, zOfDCrossT1;
00215 if(curv1==0) {m=m1; q=q1; r=r2; xc=xc2; yc=yc2; dirT=dir2;}
00216 else {m=m2; q=q2; r=r1; xc=xc1; yc=yc1; dirT=dir1;}
00217
00218 double dist = fabs((m*xc-yc+q)/sqrt(1+m*m));
00219 if(dist <= r) {
00220
00221
00222
00223 interLineCircle(m,q,xc,yc,r, xint1, yint1, xint2, yint2);
00224 double dist1 = (xint1-pos1b.x())*(xint1-pos1b.x()) +
00225 (yint1-pos1b.y())*(yint1-pos1b.y());
00226 double dist2 = (xint2-pos1b.x())*(xint2-pos1b.x()) +
00227 (yint2-pos1b.y())*(yint2-pos1b.y());
00228
00229 if(dist1<dist2){
00230 xint = xint1;
00231 yint = yint1;
00232 } else {
00233 xint = xint2;
00234 yint = yint2;
00235 }
00236 } else {
00237 #ifdef MDCPATREC_DEBUG
00238 cout << "ErrMsg(debugging) doing separated line-circle in TrkPocaXY"<<endl;
00239 #endif
00240
00241
00242
00243 double mperp = -1./m;
00244 double qperp = yc - mperp*xc;
00245
00246
00247
00248 interLineCircle(mperp, qperp, xc, yc, r, xint1,yint1,xint2,yint2);
00249
00250 double xint3,yint3;
00251 interTwoLines(m, q, mperp, qperp, xint3, yint3);
00252
00253 double dist1_3 = pow((xint1-xint3),2.) + pow((yint1-yint3),2.);
00254 double dist2_3 = pow((xint2-xint3),2.) + pow((yint2-yint3),2.);
00255 if (dist1_3<dist2_3) {
00256 xint = 0.5*(xint3 + xint1);
00257 yint = 0.5*(yint3 + yint1);
00258 absdoca = sqrt((xint3-xint1)*(xint3-xint1)+
00259 (yint3-yint1)*(yint3-yint1));
00260 zOfDCrossT1 = (xint3-xint1)*dirT[1]-(yint3-yint1)*dirT[0];
00261 }
00262 else {
00263 xint = 0.5*(xint3 + xint2);
00264 yint = 0.5*(yint3 + yint2);
00265 absdoca = sqrt((xint3-xint2)*(xint3-xint2)+
00266 (yint3-yint2)*(yint3-yint2));
00267 zOfDCrossT1 = (xint3-xint2)*dirT[1]-(yint3-yint2)*dirT[0];
00268 }
00269 _docaxy = absdoca;
00270 if( zOfDCrossT1 > 0) _docaxy = -absdoca;
00271 }
00272 }
00273
00274
00275
00276 HepPoint3D intpt( xint, yint, 0.0);
00277 TrkPocaXY poca1(traj1,_flt1,intpt,prec);
00278 TrkPocaXY poca2(traj2,_flt2,intpt,prec);
00279 _status = poca2.status();
00280 if(poca1.status().success() && poca2.status().success()) {
00281 delta1 = fabs(_flt1 - poca1.flt1());
00282 delta2 = fabs(_flt2 - poca2.flt1());
00283 _flt1 = poca1.flt1();
00284 _flt2 = poca2.flt1();
00285 }else{
00286 #ifdef MDCPATREC_WARNING
00287 cout << "ErrMsg(warning)" << " poca fialure " << endl;
00288 #endif
00289 if(poca1.status().failure()) _status=poca1.status();
00290 break;
00291 }
00292 niter++;
00293 }
00294 #ifdef MDCPATREC_DEBUG
00295 cout <<"ErrMsg(debugging)" <<"TrkPocaXY iterations = " << niter-1
00296 << " flight lengths " << _flt1 <<" " << _flt2 << endl
00297 << " deltas " << delta1 <<" " << delta2 << endl;
00298 #endif
00299 if(niter == maxiter){
00300 #ifdef MDCPATREC_ROUTINE
00301 cout << "ErrMsg(routine)" << "TrkPocaXY:: did not converge" << endl;
00302 #endif
00303 _status.setFailure(13, "TrkPocaXY:: did not converge");
00304 }
00305 }
00306
00307 void
00308 TrkPocaXY::interLineCircle(const double& m, const double& q,
00309 const double& xc, const double& yc, const double& r,
00310 double& xint1, double& yint1,
00311 double& xint2, double& yint2)
00312
00313 {
00314
00315 double alpha = 1 + m*m;
00316
00317 double beta = -xc +m*(q-yc);
00318
00319 double gamma = xc*xc + (q-yc)*(q-yc) -r*r;
00320
00321 double Delta = beta*beta - alpha*gamma;
00322
00323 if (Delta < 0) {
00324
00325 _status.setFailure(14,
00326 "TrkPocaXY:: the line and the circle heve no intersections...");
00327 return;
00328
00329 }
00330 else if (fabs(Delta) <1.e-12) {
00331
00332 xint1 = -beta/alpha;
00333 xint2 = xint1;
00334
00335 }
00336 else {
00337
00338 double xPlus = -beta/alpha + sqrt(beta*beta - alpha*gamma)/alpha;
00339 double xMinus = -beta/alpha - sqrt(beta*beta - alpha*gamma)/alpha;
00340
00341 if (xPlus > xMinus) {
00342 xint1 = xMinus;
00343 xint2 = xPlus;
00344 }
00345 else {
00346 xint1 = xPlus;
00347 xint2 = xMinus;
00348 }
00349 }
00350 yint1 = m*xint1 + q;
00351 yint2 = m*xint2 + q;
00352
00353 return;
00354 }
00355
00356 void
00357 TrkPocaXY::interTwoCircles
00358 (const double& xc1, const double& yc1, const double& r1,
00359 const double& xc2, const double& yc2, const double& r2,
00360 double& xint1, double& yint1, double& xint2, double& yint2)
00361
00362 {
00363
00364 double A = (xc1*xc1 + yc1*yc1 - r1*r1) - (xc2*xc2 + yc2*yc2 - r2*r2);
00365
00366 double B = -xc1 + xc2;
00367
00368 double C = -yc1 + yc2;
00369
00370 double alpha = 1 + (B*B)/(C*C);
00371
00372 double beta = -xc1 + B/C*(yc1+A/(2*C));
00373
00374 double gamma = xc1*xc1 + (yc1+A/(2*C))*(yc1+A/(2*C)) - r1*r1;
00375
00376 double Delta = beta*beta - alpha*gamma;
00377
00378 if (Delta < 0) {
00379
00380 _status.setFailure(14, "TrkPocaXY:: the two circles have no intersections..\
00381 .");
00382 return;
00383
00384 }
00385 else if (fabs(Delta) <1.e-12) {
00386
00387 xint1 = -beta/alpha;
00388 xint2 = xint1;
00389
00390 }
00391 else {
00392 double xPlus = -beta/alpha + sqrt(beta*beta - alpha*gamma)/alpha;
00393 double xMinus = -beta/alpha - sqrt(beta*beta - alpha*gamma)/alpha;
00394
00395 if (xPlus > xMinus) {
00396 xint1 = xMinus;
00397 xint2 = xPlus;
00398 }
00399 else {
00400 xint1 = xPlus;
00401 xint2 = xMinus;
00402 }
00403
00404 }
00405
00406 yint1 = -(A+2*B*xint1)/(2*C);
00407 yint2 = -(A+2*B*xint2)/(2*C);
00408
00409
00410 return;
00411 }
00412
00413
00414 void
00415 TrkPocaXY::interTwoLines
00416 (const double& m1, const double& q1, const double& m2, const double& q2,
00417 double& xint, double& yint)
00418
00419 {
00420
00421 if (fabs(m1-m2) < 1.e-12) {
00422
00423
00424
00425 _status.setFailure(13, "TrkPocaXY:: the two lines are parallel...");
00426 return;
00427 }
00428 else {
00429
00430 xint = (q2-q1)/(m1-m2);
00431 yint = m1*xint + q1;
00432 }
00433
00434 return;
00435 }