00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "TrkBase/TrkPocaBase.h"
00016 #include "TrkBase/TrkPoca.h"
00017 #include "CLHEP/Vector/ThreeVector.h"
00018 #include "CLHEP/Geometry/Point3D.h"
00019 #include "MdcGeom/Trajectory.h"
00020 #include <math.h>
00021 #include <assert.h>
00022 #include <iomanip>
00023 #include <algorithm>
00024
00025 TrkPocaBase::TrkPocaBase(double f1, double f2, double prec)
00026 : _precision(prec), _flt1(f1), _flt2(f2),_status(TrkErrCode::fail)
00027 {
00028 assert(prec > 0.);
00029 }
00030
00031 void
00032 TrkPocaBase::minimize(const Trajectory& traj1, double f1,
00033 const Trajectory& traj2, double f2)
00034 {
00035
00036 const int maxnOscillStep = 5 ;
00037 const int maxnDivergingStep = 5 ;
00038
00039
00040 _status = TrkErrCode::succeed;
00041 _flt1 = f1;
00042 _flt2 = f2;
00043
00044 static HepPoint3D newPos1, newPos2 ;
00045 double delta(0), prevdelta(0) ;
00046 int nOscillStep(0) ;
00047 int nDivergingStep(0) ;
00048 bool finished = false ;
00049 int istep(0) ;
00050
00051 for (istep=0; istep < _maxTry && !finished; ++istep) {
00052 double prevflt1 = flt1();
00053 double prevflt2 = flt2() ;
00054 double prevprevdelta = prevdelta ;
00055 prevdelta = delta;
00056
00057 stepTowardPoca(traj1, traj2);
00058 if( status().failure() ) {
00059
00060 finished=true ;
00061 } else {
00062 newPos1 = traj1.position(flt1());
00063 newPos2 = traj2.position(flt2());
00064 delta = (newPos1 - newPos2).mag();
00065 double step1 = flt1() - prevflt1;
00066 double step2 = flt2() - prevflt2;
00067 int pathDir1 = (step1 > 0.) ? 1 : -1;
00068 int pathDir2 = (step2 > 0.) ? 1 : -1;
00069
00070
00071 double distToErr1 = traj1.distTo1stError(prevflt1, precision(), pathDir1);
00072 double distToErr2 = traj2.distTo1stError(prevflt2, precision(), pathDir2);
00073
00074
00075 finished =
00076 (fabs(step1) < distToErr1 && fabs(step2) < distToErr2 ) ||
00077 (status().success() == 3) ;
00078
00079
00080 if( !finished && istep>2 && delta > prevdelta) {
00081 if( prevdelta > prevprevdelta) {
00082
00083 if(++nDivergingStep>maxnDivergingStep) {
00084 _status.setFailure(2) ;
00085 finished = true ;
00086 }
00087 } else {
00088 nDivergingStep=0;
00089
00090 if(++nOscillStep>maxnOscillStep) {
00091
00092
00093 _flt1 = prevflt1 ;
00094 _flt2 = prevflt2 ;
00095 _status.setSuccess(21, "Oscillating poca.") ;
00096 finished = true ;
00097 } else {
00098
00099
00100
00101 _flt1 = prevflt1 + 0.5*step1 ;
00102 _flt2 = prevflt2 + 0.5*step2 ;
00103 newPos1 = traj1.position(_flt1) ;
00104 newPos2 = traj2.position(_flt2) ;
00105 delta = (newPos1 - newPos2).mag() ;
00106 }
00107 }
00108 }
00109 }
00110 }
00111 if(!finished) _status.setSuccess(2) ;
00112 }
00113
00114 TrkPocaBase::TrkPocaBase(double f1, double prec)
00115 : _precision(prec), _flt1(f1), _flt2(0), _status(TrkErrCode::fail)
00116 {
00117 }
00118
00119 void
00120 TrkPocaBase::minimize(const Trajectory& traj, double f1,
00121 const HepPoint3D& pt )
00122 {
00123 _status=TrkErrCode::succeed;
00124 _flt1 = f1;
00125 int pathDir = 1;
00126
00127 int nTinyStep = 0;
00128 int nOscills = 0;
00129 double fltLast = 0., fltBeforeLast = 0.;
00130 for (int i = 0; i < _maxTry; i++) {
00131 fltLast = flt1();
00132 stepToPointPoca(traj, pt);
00133 if (status().failure()) return;
00134 double step = flt1() - fltLast;
00135 pathDir = (step > 0.) ? 1 : -1;
00136
00137 double distToErr = traj.distTo1stError(fltLast, precision(), pathDir);
00138 bool mustStep = (fabs(step) > distToErr && step != 0.);
00139
00140 if (fabs(step) < 0.5 * precision()) {
00141 nTinyStep++;
00142 } else {
00143 nTinyStep = 0;
00144 if (i > 1) {
00145 if (fabs(step) >= fabs(fltBeforeLast-fltLast) &&
00146 fabs(fltBeforeLast-flt1()) <= fabs(step)) {
00147 nOscills++;
00148 double halfway = (fltBeforeLast + fltLast) / 2.;
00149 if ((traj.position(flt1()) - pt).mag() >
00150 (traj.position(halfway) - pt).mag()) _flt1 = halfway;
00151 }
00152 }
00153 }
00154 if (nTinyStep > 3) mustStep = false;
00155 if (nOscills > 2) {
00156 mustStep = false;
00157 #ifdef MDCPATREC_WARNING
00158 std::cout<<" ErrMsg(warning) "<< "Alleged oscillation detected. "
00159 << step << " " << fltLast-fltBeforeLast
00160 << " " << i << " " << std::endl;
00161 #endif
00162 }
00163 if (!mustStep) return;
00164 fltBeforeLast = fltLast;
00165 }
00166
00167 _status.setFailure(2);
00168 }
00169
00170
00171 TrkPocaBase::~TrkPocaBase()
00172 {}
00173
00174
00175 void
00176 TrkPocaBase::stepTowardPoca(const Trajectory& traj1, const Trajectory& traj2)
00177 {
00178
00179
00180
00181 static Hep3Vector dir1, dir2;
00182 static Hep3Vector delDir1, delDir2;
00183 static HepPoint3D pos1, pos2;
00184
00185 traj1.getInfo(flt1(), pos1, dir1, delDir1);
00186 traj2.getInfo(flt2(), pos2, dir2, delDir2);
00187 Hep3Vector delta = ((CLHEP::Hep3Vector)pos1) - ((CLHEP::Hep3Vector)pos2);
00188 double ua = -delta.dot(dir1);
00189 double ub = delta.dot(dir2);
00190 double caa = dir1.mag2() + delta.dot(delDir1);
00191 double cbb = dir2.mag2() - delta.dot(delDir2);
00192 double cab = -dir1.dot(dir2);
00193 double det = caa * cbb - cab * cab;
00194
00195 if(det<0) {
00196
00197 caa = dir1.mag2() ;
00198 cbb = dir2.mag2() ;
00199 det = caa * cbb - cab * cab;
00200 }
00201
00202 if ( det < 1.e-8) {
00203
00204 _status.setSuccess(3);
00205 return;
00206 }
00207
00208 double df1 = (ua * cbb - ub * cab)/det;
00209 int pathDir1 = (df1 > 0) ? 1 : -1;
00210 double df2 = (ub * caa - ua * cab)/det;
00211 int pathDir2 = (df2 > 0) ? 1 : -1;
00212
00213
00214
00215
00216
00217
00218 double distToErr1 = traj1.distTo2ndError(flt1(), _extrapToler, pathDir1);
00219 double distToErr2 = traj2.distTo2ndError(flt2(), _extrapToler, pathDir2);
00220
00221
00222 const double smudge = 1.01 ;
00223 if( fabs(df1) > smudge*distToErr1 ) {
00224
00225 df1 = smudge*distToErr1 * pathDir1 ;
00226
00227 df2 = (ub - df1*cab)/cbb ;
00228 }
00229
00230 if( fabs(df2) > smudge*distToErr2 ) {
00231
00232 df2 = smudge*distToErr2 * pathDir2 ;
00233
00234 df1 = (ua - df2*cab)/cbb ;
00235
00236 if( fabs(df1) > smudge*distToErr1 ) {
00237 df1 = smudge*distToErr1 * pathDir1 ;
00238 }
00239 }
00240
00241 _flt1 += df1;
00242 _flt2 += df2;
00243
00244
00245 if (fabs(flt1()) > _maxDist && fabs(flt2()) > _maxDist)
00246 _status.setSuccess(3) ;
00247 }
00248
00249 void
00250 TrkPocaBase::stepToPointPoca(const Trajectory& traj, const HepPoint3D& pt)
00251 {
00252
00253 static Hep3Vector dir, delDir;
00254 static HepPoint3D trajPos;
00255
00256 traj.getInfo(flt1(), trajPos, dir, delDir);
00257 Hep3Vector delta = ((CLHEP::Hep3Vector)trajPos) - ((CLHEP::Hep3Vector)pt);
00258 double denom = 1. + delta.dot(delDir);
00259 if (fabs(denom)*_maxDist < 1. ) {
00260 _status.setFailure(11, "TrkPoca::ambiguous tight looper.");
00261 return;
00262 }
00263 double df = -delta.dot(dir) / fabs(denom);
00264 int pathDir = (df > 0.) ? 1 : -1;
00265
00266
00267 double distToErr = traj.distTo2ndError(flt1(), _extrapToler, pathDir);
00268 if (fabs(df)>distToErr) df = (df>0?distToErr:-distToErr);
00269
00270 df += 0.001 * pathDir * precision();
00271 _flt1 += df;
00272 }
00273
00274 double TrkPocaBase::_maxDist = 1.e7;
00275
00276 int TrkPocaBase::_maxTry = 500;
00277
00278 double TrkPocaBase::_extrapToler = 2.;