Support for empty segments

physics/discrete_traject0ry.hpp

@@ -88,8 +88,12 @@ class DiscreteTraject0ry : public Trajectory<Frame> {
   SegmentIterator AttachSegments(DiscreteTraject0ry&& trajectory);
   void DeleteSegments(SegmentIterator begin);
 
+  // Deletes the point at times in [t, end[.  Drops empty segment.
   void ForgetAfter(Instant const& t);
   void ForgetAfter(iterator it);
+
+  // Deletes the points at times in [begin, t[.  Preserves empty segments and
+  // doesn't invalidate any segment iterator.
   void ForgetBefore(Instant const& t);
   void ForgetBefore(iterator it);
 

physics/discrete_traject0ry_body.hpp

@@ -242,7 +242,9 @@ template<typename Frame>
 void DiscreteTraject0ry<Frame>::ForgetBefore(Instant const& t) {
   auto const sit = FindSegment(t);
   sit->ForgetBefore(t);
-  segments_->erase(segments_->begin(), sit);
+  for (auto s = segments_->begin(); s != sit; ++s) {
+    s->ForgetBefore(t);
+  }
 }
 
 template<typename Frame>

physics/discrete_traject0ry_test.cpp

@@ -481,14 +481,43 @@ TEST_F(DiscreteTraject0ryTest, ForgetBefore) {
   EXPECT_EQ(t0_ + 14 * Second, trajectory.rbegin()->time);
 
   trajectory.ForgetBefore(t0_ + 6.1 * Second);
-  EXPECT_EQ(2, trajectory.segments().size());
+  EXPECT_EQ(3, trajectory.segments().size());
   EXPECT_EQ(t0_ + 7 * Second, trajectory.begin()->time);
   EXPECT_EQ(t0_ + 14 * Second, trajectory.rbegin()->time);
 
   trajectory.ForgetBefore(t0_ + 9 * Second);
-  EXPECT_EQ(1, trajectory.segments().size());
+  EXPECT_EQ(3, trajectory.segments().size());
   EXPECT_EQ(t0_ + 9 * Second, trajectory.begin()->time);
   EXPECT_EQ(t0_ + 14 * Second, trajectory.rbegin()->time);
+
+  // The trajectory now has empty segments, so let's check that we can properly
+  // iterate over those.
+  {
+    std::vector<Instant> times;
+    for (auto const& [t, _] : trajectory) {
+      times.push_back(t);
+    }
+    EXPECT_THAT(times,
+                ElementsAre(t0_ + 9 * Second,
+                            t0_ + 10 * Second,
+                            t0_ + 11 * Second,
+                            t0_ + 12 * Second,
+                            t0_ + 13 * Second,
+                            t0_ + 14 * Second));
+  }
+  {
+    std::vector<Instant> times;
+    for (auto it = trajectory.rbegin(); it != trajectory.rend(); ++it) {
+      times.push_back(it->time);
+    }
+    EXPECT_THAT(times,
+                ElementsAre(t0_ + 14 * Second,
+                            t0_ + 13 * Second,
+                            t0_ + 12 * Second,
+                            t0_ + 11 * Second,
+                            t0_ + 10 * Second,
+                            t0_ + 9 * Second));
+  }
 }
 
 TEST_F(DiscreteTraject0ryTest, TMinTMaxEvaluate) {

physics/discrete_trajectory_iterator_body.hpp

@@ -18,8 +18,11 @@ DiscreteTrajectoryIterator<Frame>::operator++() {
   Instant const previous_time = point->time;
   do {
     if (point == --segment_->timeline_end()) {
-      ++segment_;
-      if (segment_.is_end() || segment_->timeline_empty()) {
+      do {
+        ++segment_;
+      } while (!segment_.is_end() && segment_->timeline_empty());
+
+      if (segment_.is_end()) {
         point_.reset();
         break;
       } else {
@@ -111,8 +114,15 @@ DiscreteTrajectoryIterator<Frame>::DiscreteTrajectoryIterator(
     OptionalTimelineConstIterator const point)
     : segment_(segment),
       point_(point) {
-  if (segment_.is_end() || segment_->timeline_empty()) {
+  bool incremented_segment = false;
+  while (!segment_.is_end() && segment_->timeline_empty()) {
+    ++segment_;
+    incremented_segment = true;
+  }
+  if (segment_.is_end()) {
     point_.reset();
+  } else if (incremented_segment) {
+    point_ = segment_->timeline_begin();
   }
 }
 

physics/discrete_trajectory_segment_body.hpp

@@ -58,9 +58,8 @@ DiscreteTrajectorySegment<Frame>::begin() const {
 template<typename Frame>
 typename DiscreteTrajectorySegment<Frame>::iterator
 DiscreteTrajectorySegment<Frame>::end() const {
-  // TODO(phl): We probably don't want empty segments.
   if (timeline_.empty()) {
-    return iterator(self_, timeline_.begin());
+    return iterator(self_, timeline_.end());
   } else {
     // The decrement/increment ensures that we normalize the end iterator to the
     // next segment or to the end of the trajectory.