diff --git a/General/TimeRange.hh b/General/TimeRange.hh index 2967f327..77cd7ce2 100644 --- a/General/TimeRange.hh +++ b/General/TimeRange.hh @@ -40,6 +40,10 @@ namespace KinKal { bool beyond(double time, TimeDir tdir) const { return tdir == TimeDir::forwards ? time > range_[1] : time < range_[0]; } + // return the minimum time to this range (0 if in range) + double minTimeTo(double t) const { + return inRange(t) ? 0.0 : std::min(fabs(t-begin()),fabs(t-end())); + } // augment another range void combine(TimeRange const& other ) { range_[0] = std::min(begin(),other.begin()); diff --git a/Trajectory/PiecewiseTrajectory.hh b/Trajectory/PiecewiseTrajectory.hh index 5963ab5e..ab1e1640 100644 --- a/Trajectory/PiecewiseTrajectory.hh +++ b/Trajectory/PiecewiseTrajectory.hh @@ -242,18 +242,35 @@ namespace KinKal { } template double PiecewiseTrajectory::t0() const { - // find a self-consistent t0 + // find the maximally self-consistent t0. This is NOT guaranteed to be in range or unique. if(pieces_.empty())throw std::length_error("Empty PiecewiseTrajectory!"); + // first try using the traj itself to find the right piece. This works for most cases. Start with the earliest time. double t0 = pieces_.front()->t0(); + // if the front piece t0 precedes its range the search will fail. In this case try from the back + if(t0 < pieces_.front()->range().begin()) t0 = pieces_.back()->t0(); + // Check for the same problem at the back; in that case, start with the midpoint + if(t0 > pieces_.back()->range().end())t0 = this->range().mid(); auto index = nearestIndex(t0); int oldindex = -1; unsigned niter(0); - while(static_cast(index) != oldindex && niter < 10){ + static const unsigned maxniter(10); // almost all trajs converge in at most 2 iterations + while(static_cast(index) != oldindex && niter < maxniter){ ++niter; t0 = pieces_[index]->t0(); oldindex = index; index = nearestIndex(t0); } + if(!pieces_[index]->range().inRange(t0)){ + // no self-consistent segment found. Find the segment with t0 closest to its range + double mindt = std::numeric_limits::max(); + for(auto const& piece :pieces_ ){ + double dt = piece->range().minTimeTo(piece->t0()); + if(dt < mindt){ + mindt = dt; + t0 = piece->t0(); + } + } + } return t0; }