diff --git a/physics/discrete_trajectory_body.hpp b/physics/discrete_trajectory_body.hpp
index 153786e66e..321ef98928 100644
--- a/physics/discrete_trajectory_body.hpp
+++ b/physics/discrete_trajectory_body.hpp
@@ -187,7 +187,7 @@ DiscreteTrajectory::NewSegment() {
segment_by_left_endpoint_.end(), last_time, new_segment_sit);
}
- DCHECK_OK(ConsistencyStatus());
+ CHECK_OK(ConsistencyStatus());
return new_self;
}
@@ -205,7 +205,7 @@ DiscreteTrajectory::DetachSegments(SegmentIterator const begin) {
/*to=*/detached,
/*to_segments_begin=*/detached.segments_->begin());
- DCHECK_OK(ConsistencyStatus());
+ CHECK_OK(ConsistencyStatus());
return detached;
}
@@ -245,7 +245,7 @@ DiscreteTrajectory::AttachSegments(DiscreteTrajectory trajectory) {
/*to=*/*this,
/*to_segments_begin=*/end_before_splice);
- DCHECK_OK(ConsistencyStatus());
+ CHECK_OK(ConsistencyStatus());
return SegmentIterator(segments_.get(), end_before_splice);
}
@@ -270,7 +270,7 @@ void DiscreteTrajectory::DeleteSegments(SegmentIterator& begin) {
// Make sure that the client doesn't try to use the invalid iterator.
begin = segments().end();
- DCHECK_OK(ConsistencyStatus());
+ CHECK_OK(ConsistencyStatus());
}
template
@@ -300,7 +300,7 @@ void DiscreteTrajectory::ForgetAfter(Instant const& t) {
segment_by_left_endpoint_.end());
}
- DCHECK_OK(ConsistencyStatus());
+ CHECK_OK(ConsistencyStatus());
}
template
@@ -342,7 +342,7 @@ void DiscreteTrajectory::ForgetBefore(Instant const& t) {
sit);
}
- DCHECK_OK(ConsistencyStatus());
+ CHECK_OK(ConsistencyStatus());
}
template
@@ -396,12 +396,18 @@ void DiscreteTrajectory::Merge(DiscreteTrajectory trajectory) {
// Merge corresponding segments.
sit_t->Merge(std::move(*sit_s));
- // If the left endpoint has changed, remove its entry from the time-to-
- // segment map. Insert a new entry if the segment is not empty.
+ // If the left endpoint of |sit_t| has changed, remove its entry from the
+ // time-to-segment map, if any.
if (left_endpoint.has_value() &&
sit_t->front().time < left_endpoint.value()) {
- segment_by_left_endpoint_.erase(left_endpoint.value());
+ auto const it = segment_by_left_endpoint_.find(left_endpoint.value());
+ if (it != segment_by_left_endpoint_.end() && it->second == sit_t) {
+ segment_by_left_endpoint_.erase(left_endpoint.value());
+ }
}
+ // Insert a new entry in the time-to-segment map if the segment is not
+ // empty. This entry will be overwritten by any future entry at the same
+ // time, thereby enforcing the invariants of the time-to-segment map.
if (!sit_t->empty()) {
segment_by_left_endpoint_.insert_or_assign(sit_t->front().time, sit_t);
}
@@ -431,7 +437,7 @@ void DiscreteTrajectory::Merge(DiscreteTrajectory trajectory) {
}
}
- DCHECK_OK(ConsistencyStatus());
+ CHECK_OK(ConsistencyStatus());
}
template
@@ -726,6 +732,12 @@ absl::Status DiscreteTrajectory::ConsistencyStatus() const {
" and ", DebugString(sit2->front().time)));
}
}
+ if (sit1 != segments_->cend() && !sit1->empty()) {
+ return absl::InternalError(
+ absl::StrCat("Segment at time ", DebugString(sit1->front().time),
+ " missing in the time-to-segment map of size ",
+ segment_by_left_endpoint_.size()));
+ }
}
if (!segments_->empty()) {
int i = 0;
diff --git a/physics/discrete_trajectory_test.cpp b/physics/discrete_trajectory_test.cpp
index 359d5d2260..92d794da74 100644
--- a/physics/discrete_trajectory_test.cpp
+++ b/physics/discrete_trajectory_test.cpp
@@ -628,6 +628,17 @@ TEST_F(DiscreteTrajectoryTest, Merge) {
EXPECT_EQ(t0_ + 9 * Second, sit->front().time);
EXPECT_EQ(t0_ + 14 * Second, sit->back().time);
}
+ {
+ auto trajectory1 = MakeTrajectory();
+ auto trajectory2 = MakeTrajectory();
+
+ trajectory1.ForgetAfter(t0_ + 9 * Second);
+ // This trajectory starts with a 1-point segment. Merge used to fail the
+ // consistency check because the time-to-segment map was losing an entry.
+ trajectory2.ForgetBefore(t0_ + 9 * Second);
+
+ trajectory2.Merge(std::move(trajectory1));
+ }
}
TEST_F(DiscreteTrajectoryTest, TMinTMaxEvaluate) {