Fix a bug where ForgetAfter was creating orphaned 1-point segments

physics/discrete_trajectory_body.hpp

@@ -280,8 +280,21 @@ void DiscreteTrajectory<Frame>::ForgetAfter(Instant const& t) {
     clear();
     return;
   }
-  auto const sit = leit->second;
+
+  // |FindSegment| gives us the most recent segment comprising |t|.  If |t| is
+  // exactly at the beginning of a segment, it is possible that there would be
+  // 1-point segments at |t| before that segment.  Let's find them and include
+  // them in the forgetting.
+  auto sit = leit->second;
+  while (sit != segments_->begin()) {
+    auto previous_sit = std::prev(sit);
+    if (previous_sit->empty() || previous_sit->front().time != t) {
+      break;
+    }
+    sit = previous_sit;
+  }
   sit->ForgetAfter(t);
+
   // Here |sit| designates a segment starting at or after |t|.  If |t| is
   // exactly at the beginning of the segment,
   // |DiscreteTrajectorySegment::ForgetAfter| will leave it empty.  In that

physics/discrete_trajectory_test.cpp

@@ -492,29 +492,39 @@ TEST_F(DiscreteTrajectoryTest, DeleteSegments) {
 }
 
 TEST_F(DiscreteTrajectoryTest, ForgetAfter) {
-  auto trajectory = MakeTrajectory();
+  {
+    auto trajectory = MakeTrajectory();
 
-  trajectory.ForgetAfter(trajectory.end());
-  EXPECT_EQ(3, trajectory.segments().size());
+    trajectory.ForgetAfter(trajectory.end());
+    EXPECT_EQ(3, trajectory.segments().size());
 
-  trajectory.ForgetAfter(t0_ + 12 * Second);
-  EXPECT_EQ(3, trajectory.segments().size());
-  EXPECT_EQ(t0_, trajectory.begin()->time);
-  EXPECT_EQ(t0_ + 11 * Second, trajectory.rbegin()->time);
+    trajectory.ForgetAfter(t0_ + 12 * Second);
+    EXPECT_EQ(3, trajectory.segments().size());
+    EXPECT_EQ(t0_, trajectory.begin()->time);
+    EXPECT_EQ(t0_ + 11 * Second, trajectory.rbegin()->time);
 
-  trajectory.ForgetAfter(t0_ + 6.1 * Second);
-  EXPECT_EQ(2, trajectory.segments().size());
-  EXPECT_EQ(t0_, trajectory.begin()->time);
-  EXPECT_EQ(t0_ + 6 * Second, trajectory.rbegin()->time);
+    trajectory.ForgetAfter(t0_ + 6.1 * Second);
+    EXPECT_EQ(2, trajectory.segments().size());
+    EXPECT_EQ(t0_, trajectory.begin()->time);
+    EXPECT_EQ(t0_ + 6 * Second, trajectory.rbegin()->time);
 
-  trajectory.ForgetAfter(t0_ + 4 * Second);
-  EXPECT_EQ(1, trajectory.segments().size());
-  EXPECT_EQ(t0_, trajectory.begin()->time);
-  EXPECT_EQ(t0_ + 4 * Second, trajectory.rbegin()->time);
+    trajectory.ForgetAfter(t0_ + 4 * Second);
+    EXPECT_EQ(1, trajectory.segments().size());
+    EXPECT_EQ(t0_, trajectory.begin()->time);
+    EXPECT_EQ(t0_ + 4 * Second, trajectory.rbegin()->time);
 
-  trajectory.ForgetAfter(t0_);
-  EXPECT_TRUE(trajectory.empty());
-  EXPECT_EQ(1, trajectory.segments().size());
+    trajectory.ForgetAfter(t0_);
+    EXPECT_TRUE(trajectory.empty());
+    EXPECT_EQ(1, trajectory.segments().size());
+  }
+  {
+    // This used to fail because ForgetAfter would leave a 1-point segment at
+    // t0_ + 9 * Second which was not in the time-to-segment map.
+    auto trajectory = MakeTrajectory();
+
+    trajectory.ForgetBefore(t0_ + 9 * Second);
+    trajectory.ForgetAfter(t0_ + 9 * Second);
+  }
 }
 
 TEST_F(DiscreteTrajectoryTest, ForgetBefore) {