00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013 #include "TrkBase/TrkDifPieceTraj.h"
00014 #include "CLHEP/Matrix/Matrix.h"
00015 #include "CLHEP/Vector/ThreeVector.h"
00016 #include "CLHEP/Geometry/Point3D.h"
00017 #include "MdcRecoUtil/DifPoint.h"
00018 #include "MdcRecoUtil/DifVector.h"
00019 #include "TrkBase/TrkPoca.h"
00020 #include "TrkBase/TrkErrCode.h"
00021 #include <iostream>
00022 #include "MdcRecoUtil/BesCollectionUtils.h"
00023 using namespace std;
00024
00025 static const double STEPEPSILON = 1.0e-6;
00026 static const double _TOL = 1.0e-5;
00027
00028
00029
00030 TrkDifPieceTraj::TrkDifPieceTraj(const TrkSimpTraj& seed,const double lowlim,const double hilim) :
00031 TrkDifTraj(lowlim,hilim), _lastIndex(-1)
00032 {
00033
00034 _localtraj.push_back((TrkSimpTraj*)seed.clone());
00035 assert(lowlim < hilim);
00036 _globalrange.push_back(lowlim);
00037 _globalrange.push_back(hilim);
00038
00039 double locrange[2];
00040 locrange[0] = seed.lowRange();
00041 locrange[1] = hilim-lowlim+locrange[0];
00042 _localtraj.front()->setFlightRange(locrange);
00043 }
00044
00045 TrkDifPieceTraj::TrkDifPieceTraj(TrkSimpTraj* seed,const double lowlim,const double hilim) :
00046 TrkDifTraj(lowlim,hilim), _lastIndex(-1)
00047 {
00048
00049 assert(lowlim < hilim);
00050 _globalrange.push_back(lowlim);
00051 _globalrange.push_back(hilim);
00052
00053 double locrange[2];
00054 locrange[0] = seed->lowRange();
00055 locrange[1] = hilim-lowlim+locrange[0];
00056 seed->setFlightRange(locrange);
00057 _localtraj.push_back(seed);
00058 }
00059
00060
00061 TrkDifPieceTraj::TrkDifPieceTraj(const TrkDifPieceTraj& other) :
00062 TrkDifTraj(other.lowRange(),other.hiRange()),
00063 _globalrange(other._globalrange),
00064 _lastIndex(other._lastIndex)
00065 {
00066
00067
00068
00069
00070 typedef std::deque<TrkSimpTraj *>::const_iterator iter_t;
00071 iter_t end = other._localtraj.end();
00072 for(iter_t i=other._localtraj.begin();i!=end; ++i)
00073 _localtraj.push_back( (TrkSimpTraj*) (*i)->clone());
00074 }
00075
00076 TrkDifPieceTraj::TrkDifPieceTraj(const std::vector<TrkSimpTraj*>& trajs)
00077 {
00078
00079
00080 TrkSimpTraj* prevtraj(0);
00081 for(std::vector<TrkSimpTraj*>::const_iterator itraj=trajs.begin();itraj!=trajs.end();++itraj){
00082 TrkSimpTraj* newtraj = (*itraj)->clone();
00083 assert(newtraj != 0);
00084 if(prevtraj != 0){
00085
00086 TrkErrCode add;
00087 double gap;
00088 if(newtraj->lowRange() > prevtraj->lowRange())
00089 add = append(newtraj,gap);
00090 else
00091 add = prepend(newtraj,gap);
00092 if(add.failure()){
00093 #ifdef MDCPATREC_WARNING
00094 std::cout<<"ErrMsg(warning) "
00095 << "construction from vector of trajs failed"
00096 << add << std::endl;
00097 #endif
00098 delete newtraj;
00099 break;
00100 }
00101 } else {
00102
00103 _localtraj.push_back(newtraj);
00104 _globalrange.push_back(newtraj->lowRange());
00105 _globalrange.push_back(newtraj->hiRange());
00106
00107 flightrange[0] = _globalrange.front();
00108 flightrange[1] = _globalrange.back();
00109 }
00110 prevtraj = _localtraj[itraj-trajs.begin()];
00111 }
00112 }
00113
00114 TrkDifPieceTraj::~TrkDifPieceTraj()
00115 {
00116 std::for_each(_localtraj.begin(),
00117 _localtraj.end(),
00118 bes::Collection::DeleteObject());
00119 }
00120
00121 void
00122 TrkDifPieceTraj::getDFInfo(double flightdist, DifPoint& pos, DifVector& direction,
00123 DifVector& delDirect) const
00124 {
00125
00126
00127
00128 double localflight(0.0);
00129 const TrkSimpTraj* loctraj = localTrajectory(flightdist,localflight);
00130 loctraj->getDFInfo(localflight,pos,direction,delDirect);
00131 }
00132
00133 void
00134 TrkDifPieceTraj::getDFInfo2(double flightdist, DifPoint& pos,
00135 DifVector& direction) const
00136 {
00137
00138
00139
00140 double localflight(0.0);
00141 const TrkSimpTraj* loctraj = localTrajectory(flightdist,localflight);
00142 loctraj->getDFInfo2(localflight,pos,direction);
00143 }
00144
00145 TrkDifPieceTraj&
00146 TrkDifPieceTraj::operator =(const TrkDifPieceTraj& other)
00147 {
00148 if (&other == this) return *this;
00149 flightrange[0] = other.flightrange[0];
00150 flightrange[1] = other.flightrange[1];
00151 _globalrange = other._globalrange;
00152
00153
00154
00155 std::for_each(_localtraj.begin(),_localtraj.end(),bes::Collection::DeleteObject());
00156 _localtraj.clear();
00157 typedef std::deque<TrkSimpTraj*>::const_iterator iter_t;
00158 for(iter_t i=other._localtraj.begin();i!=other._localtraj.end();++i)
00159 _localtraj.push_back((*i)->clone());
00160 return *this;
00161 }
00162
00163
00164
00165
00166 const TrkErrCode&
00167 TrkDifPieceTraj::append(double glen,TrkSimpTraj* nexttraj,double& gap)
00168 {
00169 static TrkErrCode retval;
00170 retval = TrkErrCode(TrkErrCode::succeed);
00171
00172 if(glen >= lowRange()){
00173
00174 int nremoved = resize(glen,trkOut);
00175 if(nremoved > 0) {
00176 #ifdef MDCPATREC_WARNING
00177 std::cout<<"ErrMsg(warning)" << "append removed "
00178 << nremoved << " old trajectories" << std::endl;
00179 #endif
00180 }
00181
00182 HepPoint3D endpoint = position(glen);
00183 HepPoint3D newend = nexttraj->position(nexttraj->lowRange());
00184 gap = newend.distance(endpoint);
00185 double range[2];
00186 range[0] = nexttraj->lowRange();
00187 range[1] = nexttraj->hiRange();
00188 double delta = max(hiRange() - glen,range[1]-range[0]);
00189 if(gap > _TOL){
00190 TrkPoca endpoca(*nexttraj,nexttraj->lowRange(),endpoint,_TOL);
00191 if(endpoca.status().failure()){
00192 retval = TrkErrCode(TrkErrCode::fail,7,"poca failure");
00193 return retval;
00194 }
00195 range[0] = endpoca.flt1();
00196 gap = endpoca.doca();
00197 }
00198
00199 range[1] = range[0] + delta;
00200
00201 nexttraj->setFlightRange(range);
00202
00203 _localtraj.push_back(nexttraj);
00204
00205 _globalrange.push_back(glen+delta);
00206 flightrange[1] = glen+delta;
00207 } else
00208 retval = TrkErrCode(TrkErrCode::fail,6," cannot append before minimum flight range");
00209 return retval;
00210 }
00211
00212 const TrkErrCode&
00213 TrkDifPieceTraj::prepend(double glen,TrkSimpTraj* nexttraj,double& gap)
00214 {
00215 static TrkErrCode retval;
00216 retval = TrkErrCode(TrkErrCode::succeed);
00217
00218 if(glen <= hiRange()){
00219
00220 int nremoved = resize(glen,trkIn);
00221 if(nremoved > 0) {
00222 #ifdef MDCPATREC_WARNING
00223 std::cout<<"ErrMsg(warning) "<< " prepend removed "
00224 << nremoved << " old trajectories" << std::endl;
00225 #endif
00226 }
00227
00228 HepPoint3D endpoint = position(glen);
00229 HepPoint3D newend = nexttraj->position(nexttraj->hiRange());
00230 gap = newend.distance(endpoint);
00231 double range[2];
00232 range[0] = nexttraj->lowRange();
00233 range[1] = nexttraj->hiRange();
00234 double delta = max(glen -lowRange(),range[1]-range[0]);
00235 if(gap > _TOL){
00236 TrkPoca endpoca(*nexttraj,nexttraj->hiRange(),endpoint,_TOL);
00237 if(endpoca.status().failure()){
00238 retval = TrkErrCode(TrkErrCode::fail,7,"poca failure");
00239 return retval;
00240 }
00241 range[1] = endpoca.flt1();
00242 gap = endpoca.doca();
00243 }
00244
00245 range[0] = range[1] - delta;
00246
00247 nexttraj->setFlightRange(range);
00248
00249 _localtraj.push_front(nexttraj);
00250
00251 _globalrange.push_front(glen-delta);
00252 flightrange[0] = glen-delta;
00253 } else
00254 retval = TrkErrCode(TrkErrCode::fail,6," cannot prepend after maximum flight range");
00255 return retval;
00256 }
00257
00258 const TrkErrCode&
00259 TrkDifPieceTraj::append(double glen,const TrkSimpTraj& nexttraj,double& gap)
00260 {
00261 return append(glen,nexttraj.clone(),gap);
00262 }
00263
00264 const TrkErrCode&
00265 TrkDifPieceTraj::prepend(double glen,const TrkSimpTraj& nexttraj,double& gap)
00266 {
00267 return prepend(glen,nexttraj.clone(),gap);
00268 }
00269
00270
00271 const TrkErrCode&
00272 TrkDifPieceTraj::append(double glen,const TrkDifPieceTraj& other,double& gap)
00273 {
00274 static TrkErrCode retval;
00275 retval = TrkErrCode(TrkErrCode::succeed);
00276
00277 double mylen = glen;
00278 HepPoint3D myend = position(mylen);
00279 double otherlen = other.lowRange();
00280 HepPoint3D otherstart = other.position(otherlen);
00281 gap = otherstart.distance(myend);
00282 if(gap > _TOL){
00283 TrkPoca endpoca(other,other.lowRange(),myend,_TOL);
00284 if(endpoca.status().failure()){
00285 retval = TrkErrCode(TrkErrCode::fail,7,"poca failure");
00286 return retval;
00287 }
00288 gap = endpoca.doca();
00289 otherlen = endpoca.flt1();
00290 }
00291
00292 double loclen(0);
00293 double piecegap;
00294
00295 for(int itraj = other.trajIndex(otherlen,loclen);
00296 itraj < other._localtraj.size();itraj++){
00297
00298 retval = append(mylen,*(other._localtraj[itraj]),piecegap);
00299 if(retval.failure())
00300 break;
00301
00302 mylen = hiRange();
00303 }
00304 return retval;
00305 }
00306
00307 const TrkErrCode&
00308 TrkDifPieceTraj::prepend(double glen,const TrkDifPieceTraj& other,double& gap)
00309 {
00310 static TrkErrCode retval;
00311 retval = TrkErrCode(TrkErrCode::succeed);
00312
00313 double mylen = glen;
00314 HepPoint3D mystart = position(mylen);
00315 double otherlen = other.hiRange();
00316 HepPoint3D otherend = other.position(otherlen);
00317 gap = otherend.distance(mystart);
00318 if(gap > _TOL){
00319 TrkPoca endpoca(other,other.hiRange(),mystart,_TOL);
00320 if(endpoca.status().failure()){
00321 retval = TrkErrCode(TrkErrCode::fail,7,"poca failure");
00322 return retval;
00323 }
00324 gap = endpoca.doca();
00325 otherlen = endpoca.flt1();
00326 }
00327
00328 double loclen(0);
00329 double piecegap;
00330
00331 for(int itraj = other.trajIndex(otherlen,loclen);
00332 itraj >= 0;itraj--) {
00333
00334 retval = prepend(mylen,*(other._localtraj[itraj]),piecegap);
00335 if(retval.failure())
00336 break;
00337
00338 mylen = lowRange();
00339 }
00340 return retval;
00341 }
00342
00343
00344 TrkDifPieceTraj&
00345 TrkDifPieceTraj::invert()
00346 {
00347
00348 std::deque<TrkSimpTraj*> trajcopy; trajcopy.swap(_localtraj);
00349 std::deque<double> rangecopy; rangecopy.swap(_globalrange);
00350 assert(_localtraj.size()==0);
00351 assert(_globalrange.size()==0);
00352
00353 _globalrange.push_back(-rangecopy.back());
00354
00355 for(int itraj=trajcopy.size()-1;itraj>=0;itraj--){
00356
00357 _localtraj.push_back(&(trajcopy[itraj]->invert()));
00358
00359 _globalrange.push_back(-rangecopy[itraj]);
00360 }
00361
00362 double range[2];
00363 range[0] = -hiRange();
00364 range[1] = -lowRange();
00365 Trajectory::setFlightRange(range);
00366 return *this;
00367 }
00368
00369
00370 int
00371 TrkDifPieceTraj::trajIndex(const double& flightdist,double& localflight) const
00372 {
00373 int index(0);
00374 if (_localtraj.size() > 1) {
00375 if(validFlightDistance(flightdist)){
00376
00377 if ( _lastIndex >= 0 && _lastIndex < _localtraj.size()-1 &&
00378 flightdist > _globalrange[_lastIndex] && flightdist <
00379 _globalrange[_lastIndex+1] ) {
00380 index = _lastIndex;
00381 } else {
00382
00383
00384
00385 int hirange = _localtraj.size() - 1;
00386 int lorange = 0;
00387 index = hirange/2;
00388 int oldindex = -1;
00389 while(index != oldindex){
00390 oldindex = index;
00391 if(flightdist < _globalrange[index]){
00392 hirange = index;
00393 index -= max(1,(index-lorange)/2);
00394 } else if(flightdist > _globalrange[index+1]){
00395 lorange = index;
00396 index += max(1,(hirange-index)/2);
00397 }
00398 }
00399 }
00400 } else {
00401
00402
00403
00404
00405 index = (flightdist < lowRange()?0:(_localtraj.size()-1));
00406 }
00407 }
00408 localflight = localDist(index,flightdist);
00409
00410 _lastIndex = index;
00411 return index;
00412 }
00413
00414 HepPoint3D
00415 TrkDifPieceTraj::position(double flightdist) const
00416 {
00417
00418
00419
00420 double localflight(0.0);
00421 const TrkSimpTraj* loctraj = localTrajectory(flightdist,localflight);
00422 return loctraj->position(localflight);
00423 }
00424
00425 Hep3Vector
00426 TrkDifPieceTraj::direction(double flightdist) const
00427 {
00428
00429
00430
00431 double localflight(0.0);
00432 const TrkSimpTraj* loctraj = localTrajectory(flightdist,localflight);
00433 return loctraj->direction(localflight);
00434 }
00435
00436 double
00437 TrkDifPieceTraj::curvature(double flightdist) const
00438 {
00439
00440
00441
00442 double localflight(0.0);
00443 const TrkSimpTraj* loctraj = localTrajectory(flightdist,localflight);
00444 return loctraj->curvature(localflight);
00445 }
00446
00447 Hep3Vector
00448 TrkDifPieceTraj::delDirect(double flightdist) const
00449 {
00450
00451
00452
00453 double localflight(0.0);
00454 const TrkSimpTraj* loctraj = localTrajectory(flightdist,localflight);
00455 return loctraj->delDirect(localflight);
00456 }
00457
00458 void
00459 TrkDifPieceTraj::getInfo(double flightdist,
00460 HepPoint3D& point,
00461 Hep3Vector& dir) const
00462 {
00463
00464
00465
00466 double localflight(0.0);
00467 const TrkSimpTraj* loctraj = localTrajectory(flightdist,localflight);
00468 loctraj->getInfo(localflight,point,dir);
00469 }
00470
00471 void
00472 TrkDifPieceTraj::getInfo(double flightdist,
00473 HepPoint3D& point,
00474 Hep3Vector& dir,
00475 Hep3Vector& deldirect) const
00476 {
00477
00478
00479
00480 double localflight(0.0);
00481 const TrkSimpTraj* loctraj = localTrajectory(flightdist,localflight);
00482 loctraj->getInfo(localflight,point,dir,deldirect);
00483 }
00484
00485 double
00486 TrkDifPieceTraj::distTo1stError(double flightdist,double tol,int dir) const
00487 {
00488
00489
00490
00491 double localflight(0.0);
00492 int index = trajIndex(flightdist,localflight);
00493 const TrkSimpTraj* loctraj = _localtraj[index];
00494
00495
00496
00497
00498 double localdist = loctraj->distTo1stError(localflight,tol,dir);
00499
00500
00501 double dist = localdist;
00502 if (dir > 0){
00503 if(index < _localtraj.size()-1)
00504 dist = min(localdist,_globalrange[index+1]-flightdist) + STEPEPSILON;
00505 } else {
00506 if(index > 0)
00507 dist = min(localdist,flightdist - _globalrange[index]) + STEPEPSILON;
00508 }
00509 return dist;
00510 }
00511
00512 double
00513 TrkDifPieceTraj::distTo2ndError(double flightdist,double tol,int dir) const
00514 {
00515
00516
00517
00518 double localflight(0.0);
00519 int index = trajIndex(flightdist,localflight);
00520 const TrkSimpTraj* loctraj = _localtraj[index];
00521
00522
00523
00524
00525 double localdist = loctraj->distTo2ndError(localflight,tol,dir);
00526
00527
00528 double dist = localdist;
00529 if (dir > 0){
00530 if(index < _localtraj.size()-1)
00531 dist = min(localdist,_globalrange[index+1]-flightdist) + STEPEPSILON;
00532 } else {
00533 if(index > 0)
00534 dist = min(localdist,flightdist - _globalrange[index]) + STEPEPSILON;
00535 }
00536 return dist;
00537 }
00538
00539
00540 const TrkSimpTraj*
00541 TrkDifPieceTraj::localTrajectory(double flightdist,double& localflight) const
00542 {
00543
00544
00545
00546 int index = trajIndex(flightdist,localflight);
00547 return _localtraj[index];
00548 }
00549
00550
00551
00552 int
00553 TrkDifPieceTraj::resize(double len,trkDirection tdir)
00554 {
00555 int nremoved(0);
00556 TrkSimpTraj* trajpiece;
00557 double oldend,locdist;
00558 double lrange[2];
00559 int ipiece = trajIndex(len,locdist);
00560 switch(tdir) {
00561 case trkIn:
00562
00563
00564
00565
00566
00567 while(ipiece > 0){
00568 nremoved++;
00569 trajpiece = _localtraj.front(); _localtraj.pop_front();
00570 delete trajpiece;
00571 oldend = _globalrange.front(); _globalrange.pop_front();
00572 ipiece = trajIndex(len,locdist);
00573 }
00574
00575 flightrange[0] = len;
00576 _globalrange[ipiece] = len;
00577
00578 lrange[0] = locdist;
00579 lrange[1] = _localtraj[ipiece]->hiRange();
00580 _localtraj[ipiece]->setFlightRange(lrange);
00581 break;
00582 case trkOut:
00583 while(ipiece < _localtraj.size() - 1){
00584 nremoved++;
00585 trajpiece = _localtraj.back(); _localtraj.pop_back();
00586 delete trajpiece;
00587 oldend = _globalrange.back(); _globalrange.pop_back();
00588 ipiece = trajIndex(len,locdist);
00589 }
00590 flightrange[1] = len;
00591 _globalrange[ipiece+1] = len;
00592 lrange[0] = _localtraj[ipiece]->lowRange();
00593 lrange[1] = locdist;
00594 _localtraj[ipiece]->setFlightRange(lrange);
00595 break;
00596 }
00597 return nremoved;
00598 }
00599
00600
00601
00602 void
00603 TrkDifPieceTraj::print(ostream& os) const
00604 {
00605 os << "TrkDifPieceTraj has " << _localtraj.size() << " pieces "
00606 << ", total flight range of " << hiRange();
00607 }
00608
00609 void
00610 TrkDifPieceTraj::printAll(ostream& os) const
00611 {
00612 os << "TrkDifPieceTraj has " << _localtraj.size() << " pieces "
00613 << ", total flight range from "
00614 << lowRange() << " to " << hiRange() << endl;
00615 for(int ipiece=0;ipiece<_localtraj.size();ipiece++){
00616 TrkSimpTraj* tpiece = _localtraj[ipiece];
00617 double plow = tpiece->lowRange();
00618 double phi = tpiece->hiRange();
00619 os << "Piece " << ipiece << " has global range from "
00620 << _globalrange[ipiece] << " to " << _globalrange[ipiece+1]
00621 << " and local range from " << plow << " to " << phi << endl;
00622 HepPoint3D start = tpiece->position(plow);
00623 HepPoint3D end = tpiece->position(phi);
00624 const HepPoint3D& refp = tpiece->referencePoint();
00625 os << "Piece " << ipiece << " starts at point "
00626 << start.x() <<","<< start.y()<<","<< start.z()
00627 << " and ends at point "
00628 << end.x()<<"," << end.y()<<"," << end.z()
00629 << " and references point "
00630 << refp.x()<<"," << refp.y()<<"," << refp.z()
00631 << endl;
00632 }
00633 }
00634
00635
00636
00637 void
00638 TrkDifPieceTraj::setFlightRange(double newrange[2])
00639 {
00640 double oldend;
00641 double lrange[2];
00642 TrkSimpTraj* trajpiece;
00643
00644
00645
00646 if( newrange[1] > newrange[0] &&
00647 newrange[0] < flightrange[1] &&
00648 newrange[1] > flightrange[0] ) {
00649 resize(newrange[0],trkIn);
00650 resize(newrange[1],trkOut);
00651 } else if(newrange[1] < newrange[0] ){
00652 #ifdef MDCPATREC_ERROR
00653 std::cout<<"ErrMsg(error) "<< "cannot resize -- invalid range" << std::endl;
00654 #endif
00655 } else {
00656
00657
00658
00659
00660
00661 if( newrange[0] > flightrange[1]){
00662 while(_localtraj.size()>1){
00663 trajpiece = _localtraj.front(); _localtraj.pop_front();
00664 delete trajpiece;
00665 oldend = _globalrange.front(); _globalrange.pop_front();
00666 }
00667 } else {
00668 while(_localtraj.size()>1){
00669 trajpiece = _localtraj.back(); _localtraj.pop_back();
00670 delete trajpiece;
00671 oldend = _globalrange.back(); _globalrange.pop_back();
00672 }
00673 }
00674 lrange[0] = localDist(0,newrange[0]);
00675 lrange[1] = localDist(0,newrange[1]);
00676 flightrange[0] = newrange[0];
00677 flightrange[1] = newrange[1];
00678 _globalrange[0] = newrange[0];
00679 _globalrange[1] = newrange[1];
00680 _localtraj[0]->setFlightRange(lrange);
00681 }
00682 }
00683
00684
00685
00686 HepMatrix
00687 TrkDifPieceTraj::derivDeflect(double flightdist,deflectDirection idir) const
00688 {
00689 double localflight(0.0);
00690 const TrkSimpTraj* loctraj = localTrajectory(flightdist,localflight);
00691 return loctraj->derivDeflect(localflight,idir);
00692 }
00693
00694 HepMatrix
00695 TrkDifPieceTraj::derivDisplace(double flightdist,deflectDirection idir) const
00696 {
00697 double localflight(0.0);
00698 const TrkSimpTraj* loctraj = localTrajectory(flightdist,localflight);
00699 return loctraj->derivDisplace(localflight,idir);
00700 }
00701
00702 HepMatrix
00703 TrkDifPieceTraj::derivPFract(double flightdist) const
00704 {
00705 double localflight(0.0);
00706 const TrkSimpTraj* loctraj = localTrajectory(flightdist,localflight);
00707 return loctraj->derivPFract(localflight);
00708 }
00709
00710 const TrkErrCode&
00711 TrkDifPieceTraj::append(TrkSimpTraj* newtraj,double& gap)
00712 {
00713 static TrkErrCode retval;
00714 retval = TrkErrCode(TrkErrCode::succeed);
00715
00716 HepPoint3D end = newtraj->position(newtraj->lowRange());
00717 TrkPoca poca(*this,newtraj->lowRange(),end,_TOL);
00718 if(poca.status().success()){
00719
00720 double local;
00721 int index = trajIndex(poca.flt1(),local);
00722 TrkSimpTraj* oldtraj = _localtraj[index];
00723
00724 if( index == _localtraj.size()-1 &&
00725 poca.flt1() > hiRange()){
00726
00727 double gapstart = globalDist(index,oldtraj->hiRange());
00728 double gapend = poca.flt1();
00729 double gapmid = (gapend+gapstart)/2.0;
00730 HepPoint3D mid = position(gapmid);
00731
00732 double locmid = newtraj->lowRange() - gapend + gapmid;
00733 TrkPoca midpoca(*newtraj,locmid,mid,_TOL);
00734 if(midpoca.status().success()){
00735
00736
00737 TrkSimpTraj* gaptraj = (TrkSimpTraj*)(newtraj->clone());
00738 assert(gaptraj != 0);
00739 double range[2];
00740 range[0] = midpoca.flt1();
00741 range[1] = range[0];
00742 gaptraj->setFlightRange(range);
00743
00744 _localtraj.push_back(gaptraj);
00745 _localtraj.push_back(newtraj);
00746
00747 _globalrange.back() = gapmid;
00748 _globalrange.push_back(gapend);
00749 _globalrange.push_back(gapend+newtraj->range());
00750
00751 flightrange[1] = _globalrange.back();
00752
00753 gap = mid.distance(newtraj->position(midpoca.flt1()));
00754 } else
00755 retval = TrkErrCode(TrkErrCode::fail,7,"poca failure");
00756 } else
00757 retval = TrkErrCode(TrkErrCode::fail,8,"invalid range");
00758 } else
00759 retval = TrkErrCode(TrkErrCode::fail,7,"poca failure");
00760 return retval;
00761 }
00762
00763
00764 const TrkErrCode&
00765 TrkDifPieceTraj::prepend(TrkSimpTraj* newtraj,double& gap)
00766 {
00767 static TrkErrCode retval;
00768 retval = TrkErrCode(TrkErrCode::succeed);
00769
00770 HepPoint3D end = newtraj->position(newtraj->hiRange());
00771 TrkPoca poca(*this,newtraj->hiRange(),end,_TOL);
00772 if(poca.status().success()){
00773
00774 double local;
00775 int index = trajIndex(poca.flt1(),local);
00776 TrkSimpTraj* oldtraj = _localtraj[index];
00777
00778 if( index == 0 && poca.flt1() < lowRange()){
00779
00780 double gapstart = poca.flt1();
00781 double gapend = globalDist(index,oldtraj->hiRange());
00782 double gapmid = (gapend+gapstart)/2.0;
00783 HepPoint3D mid = position(gapmid);
00784
00785 double locmid = newtraj->hiRange() - gapstart + gapmid;
00786 TrkPoca midpoca(*newtraj,locmid,mid,_TOL);
00787 if(midpoca.status().success()){
00788
00789
00790 TrkSimpTraj* gaptraj = (TrkSimpTraj*)(newtraj->clone());
00791 assert(gaptraj != 0);
00792 double range[2];
00793 range[0] = midpoca.flt1();
00794 range[1] = range[0];
00795 gaptraj->setFlightRange(range);
00796
00797 _localtraj.push_front(gaptraj);
00798 _localtraj.push_front(newtraj);
00799
00800 _globalrange.push_front(gapmid);
00801 _globalrange.push_front(gapstart-newtraj->range());
00802
00803 flightrange[0] = _globalrange.front();
00804
00805 gap = mid.distance(newtraj->position(midpoca.flt1()));
00806 } else
00807 retval = TrkErrCode(TrkErrCode::fail,7,"poca failure");
00808 } else
00809 retval = TrkErrCode(TrkErrCode::fail,8,"invalid range");
00810 } else
00811 retval = TrkErrCode(TrkErrCode::fail,7,"poca failure");
00812 return retval;
00813 }
00814
00815 bool
00816 TrkDifPieceTraj::locallyValid(double glen,double tol) const
00817 {
00818 double localflight(0.0);
00819 const TrkSimpTraj* loctraj = localTrajectory(glen,localflight);
00820 return loctraj != 0 &&
00821 localflight-tol <= loctraj->hiRange() &
00822 localflight+tol >= loctraj->lowRange();
00823 }
00824
00825 bool
00826 TrkDifPieceTraj::operator == (const TrkDifPieceTraj& other) const {
00827 bool retval(true);
00828 if(retval) retval = _globalrange == other._globalrange;
00829 if(retval) retval = _localtraj.size() == other._localtraj.size();
00830
00831 std::deque<TrkSimpTraj*>::const_iterator miter = _localtraj.begin();
00832 std::deque<TrkSimpTraj*>::const_iterator oiter = other._localtraj.begin();
00833 while(retval && miter != _localtraj.end() && oiter != other._localtraj.end()){
00834 retval = (*oiter)->parameters()->parameter() == (*miter)->parameters()->parameter() &&
00835 (*oiter)->parameters()->covariance() == (*miter)->parameters()->covariance();
00836 miter++;oiter++;
00837 }
00838 return retval;
00839 }