diff --git a/physics/discrete_trajectory_segment_body.hpp b/physics/discrete_trajectory_segment_body.hpp index 0918b3d3d3..e690b3130e 100644 --- a/physics/discrete_trajectory_segment_body.hpp +++ b/physics/discrete_trajectory_segment_body.hpp @@ -353,11 +353,12 @@ void DiscreteTrajectorySegment::ForgetBefore(Instant const& t) { template void DiscreteTrajectorySegment::ForgetBefore( typename Timeline::const_iterator const end) { - std::int64_t number_of_points_to_remove = + std::int64_t const number_of_points_to_remove = std::distance(timeline_.cbegin(), end); - number_of_dense_points_ = - std::max( - 0, number_of_dense_points_ - number_of_points_to_remove); + std::int64_t const number_of_dense_points_to_remove = std::max( + 0, + number_of_points_to_remove + number_of_dense_points_ - timeline_.size()); + number_of_dense_points_ -= number_of_dense_points_to_remove; timeline_.erase(timeline_.cbegin(), end); } @@ -542,9 +543,10 @@ void DiscreteTrajectorySegment::WriteToMessage( downsampling_parameters_->tolerance.WriteToMessage( serialized_downsampling_parameters->mutable_tolerance()); } - message->set_number_of_dense_points( + message->set_number_of_dense_points(std::min( + timeline_size, std::max( - 0, number_of_dense_points_ - number_of_points_to_skip_at_end)); + 0, number_of_dense_points_ - number_of_points_to_skip_at_end))); // Convert the |exact| vector into a set, and add the extremities. This // ensures that we don't have redundancies. The set is sorted by time to diff --git a/physics/discrete_trajectory_segment_test.cpp b/physics/discrete_trajectory_segment_test.cpp index 3f13c25654..5657b59f6c 100644 --- a/physics/discrete_trajectory_segment_test.cpp +++ b/physics/discrete_trajectory_segment_test.cpp @@ -523,15 +523,17 @@ TEST_F(DiscreteTrajectorySegmentTest, SerializationRange) { serialization::DiscreteTrajectorySegment message1; circle1.WriteToMessage(&message1, - /*begin=*/circle1.upper_bound(t0_ + 1 * Second), - /*end=*/circle1.upper_bound(t0_ + 3 * Second), + /*begin=*/circle1.upper_bound(t0_ + 4.9 * Second), + /*end=*/circle1.upper_bound(t0_ + 4.98 * Second), /*exact=*/{}); + EXPECT_LE(message1.number_of_dense_points(), message1.zfp().timeline_size()); serialization::DiscreteTrajectorySegment message2; - ForgetBefore(circle2.upper_bound(t0_ + 1 * Second)->time, circle2); - ForgetAfter(circle2.upper_bound(t0_ + 3 * Second)->time, circle2); + ForgetBefore(circle2.upper_bound(t0_ + 4.9 * Second)->time, circle2); + ForgetAfter(circle2.upper_bound(t0_ + 4.98 * Second)->time, circle2); circle2.WriteToMessage(&message2, /*exact=*/{}); + EXPECT_LE(message2.number_of_dense_points(), message2.zfp().timeline_size()); // Writing a range of the segment is equivalent to forgetting and writing the // result.