Skip to content

Commit

Permalink
Showing 11 changed files with 199 additions and 128 deletions.
25 changes: 13 additions & 12 deletions physics/discrete_traject0ry_body.hpp
Original file line number Diff line number Diff line change
@@ -184,9 +184,9 @@ DiscreteTraject0ry<Frame>::AttachSegments(
// NOTE(phl): This check might be too strict, we might want to allow LT as the
// time comparison, and to adjust the first point of trajectory as needed.
// We'll see if the clients need that.
CHECK_EQ(rbegin()->first, trajectory.begin()->first)
CHECK_EQ(rbegin()->time, trajectory.begin()->time)
<< "Mismatching times when attaching segments";
CHECK_EQ(rbegin()->second, trajectory.begin()->second)
CHECK_EQ(rbegin()->degrees_of_freedom, trajectory.begin()->degrees_of_freedom)
<< "Mismatching degrees of freedom when attaching segments";

if (empty()) {
@@ -417,10 +417,10 @@ absl::Status DiscreteTraject0ry<Frame>::ValidateConsistency() const {
auto it1 = segments_->cbegin();
auto it2 = segment_by_left_endpoint_.cbegin();
for (; it1 != segments_->cend(); ++it1, ++it2, ++i) {
if (it1->begin()->first != it2->first) {
if (it1->begin()->time != it2->first) {
return absl::InternalError(
absl::StrCat("Times mismatch ",
DebugString(it1->begin()->first),
DebugString(it1->begin()->time),
" and ",
DebugString(it2->first),
" for segment #",
@@ -442,20 +442,21 @@ absl::Status DiscreteTraject0ry<Frame>::ValidateConsistency() const {
// times match. We must look at the endpoints of the timeline explicitly.
auto const timeline_rbegin = --sit->timeline_end();
auto const timeline_begin = std::next(sit)->timeline_begin();
if (timeline_rbegin->first != timeline_begin->first) {
if (timeline_rbegin->time != timeline_begin->time) {
return absl::InternalError(
absl::StrCat("Duplicated time mismatch ",
DebugString(timeline_rbegin->first),
DebugString(timeline_rbegin->time),
" and ",
DebugString(timeline_begin->first),
DebugString(timeline_begin->time),
" for segment #",
i));
} else if (timeline_rbegin->second != timeline_begin->second) {
} else if (timeline_rbegin->degrees_of_freedom !=
timeline_begin->degrees_of_freedom) {
return absl::InternalError(
absl::StrCat("Duplicated degrees of freedom mismatch ",
DebugString(timeline_rbegin->second),
DebugString(timeline_rbegin->degrees_of_freedom),
" and ",
DebugString(timeline_begin->second),
DebugString(timeline_begin->degrees_of_freedom),
" for segment #",
i));
}
@@ -514,7 +515,7 @@ DiscreteTraject0ry<Frame>::ReadFromPreΖήνωνMessage(
std::vector<SegmentIterator*> const& tracked,
value_type const& fork_point,
DiscreteTraject0ry& trajectory) {
CHECK_EQ(fork_point.first, Instant::ReadFromMessage(message.fork_time()))
CHECK_EQ(fork_point.time, Instant::ReadFromMessage(message.fork_time()))
<< "Cannot read trajectory with a fork not at end of segment";
CHECK_EQ(1, message.trajectories_size())
<< "Cannot read trajectory with multiple forks";
@@ -618,7 +619,7 @@ void DiscreteTraject0ry<Frame>::ReadFromPreΖήνωνMessage(
}

// Finally, set the time-to-segment map.
trajectory.segment_by_left_endpoint_.emplace(sit->begin()->first, sit);
trajectory.segment_by_left_endpoint_.emplace(sit->begin()->time, sit);
}

} // namespace internal_discrete_traject0ry
68 changes: 35 additions & 33 deletions physics/discrete_traject0ry_test.cpp
Original file line number Diff line number Diff line change
@@ -132,8 +132,8 @@ TEST_F(DiscreteTraject0ryTest, Make) {

TEST_F(DiscreteTraject0ryTest, BackFront) {
auto const trajectory = MakeTrajectory();
EXPECT_EQ(t0_, trajectory.front().first);
EXPECT_EQ(t0_ + 14 * Second, trajectory.back().first);
EXPECT_EQ(t0_, trajectory.front().time);
EXPECT_EQ(t0_ + 14 * Second, trajectory.back().time);
}

TEST_F(DiscreteTraject0ryTest, IterateForward) {
@@ -164,7 +164,7 @@ TEST_F(DiscreteTraject0ryTest, IterateBackward) {
auto const trajectory = MakeTrajectory();
std::vector<Instant> times;
for (auto it = trajectory.rbegin(); it != trajectory.rend(); ++it) {
times.push_back(it->first);
times.push_back(it->time);
}
EXPECT_THAT(times,
ElementsAre(t0_ + 14 * Second,
@@ -317,8 +317,8 @@ TEST_F(DiscreteTraject0ryTest, Segments) {
std::vector<Instant> begin;
std::vector<Instant> rbegin;
for (auto const& sit : trajectory.segments()) {
begin.push_back(sit.begin()->first);
rbegin.push_back(sit.rbegin()->first);
begin.push_back(sit.begin()->time);
rbegin.push_back(sit.rbegin()->time);
}
EXPECT_THAT(
begin,
@@ -333,8 +333,8 @@ TEST_F(DiscreteTraject0ryTest, RSegments) {
std::vector<Instant> begin;
std::vector<Instant> rbegin;
for (auto const& sit : trajectory.rsegments()) {
begin.push_back(sit.begin()->first);
rbegin.push_back(sit.rbegin()->first);
begin.push_back(sit.begin()->time);
rbegin.push_back(sit.rbegin()->time);
}
EXPECT_THAT(
begin,
@@ -351,10 +351,10 @@ TEST_F(DiscreteTraject0ryTest, DetachSegments) {
auto trajectory2 = trajectory1.DetachSegments(second_segment);
EXPECT_EQ(1, trajectory1.segments().size());
EXPECT_EQ(2, trajectory2.segments().size());
EXPECT_EQ(t0_, trajectory1.begin()->first);
EXPECT_EQ(t0_ + 4 * Second, trajectory1.rbegin()->first);
EXPECT_EQ(t0_ + 4 * Second, trajectory2.begin()->first);
EXPECT_EQ(t0_ + 14 * Second, trajectory2.rbegin()->first);
EXPECT_EQ(t0_, trajectory1.begin()->time);
EXPECT_EQ(t0_ + 4 * Second, trajectory1.rbegin()->time);
EXPECT_EQ(t0_ + 4 * Second, trajectory2.begin()->time);
EXPECT_EQ(t0_ + 14 * Second, trajectory2.rbegin()->time);

// Check that the trajectories are minimally usable (in particular, as far as
// the time-to-segment mapping is concerned).
@@ -409,8 +409,8 @@ TEST_F(DiscreteTraject0ryTest, AttachSegments) {
1 * Metre / Second})));
trajectory1.AttachSegments(std::move(trajectory2));
EXPECT_EQ(6, trajectory1.segments().size());
EXPECT_EQ(t0_, trajectory1.begin()->first);
EXPECT_EQ(t0_ + 28 * Second, trajectory1.rbegin()->first);
EXPECT_EQ(t0_, trajectory1.begin()->time);
EXPECT_EQ(t0_ + 28 * Second, trajectory1.rbegin()->time);

// Check that the trajectories are minimally usable (in particular, as far as
// the time-to-segment mapping is concerned).
@@ -449,46 +449,46 @@ TEST_F(DiscreteTraject0ryTest, DeleteSegments) {
auto const second_segment = std::next(first_segment);
trajectory.DeleteSegments(second_segment);
EXPECT_EQ(1, trajectory.segments().size());
EXPECT_EQ(t0_, trajectory.begin()->first);
EXPECT_EQ(t0_ + 4 * Second, trajectory.rbegin()->first);
EXPECT_EQ(t0_, trajectory.begin()->time);
EXPECT_EQ(t0_ + 4 * Second, trajectory.rbegin()->time);
}

TEST_F(DiscreteTraject0ryTest, ForgetAfter) {
auto trajectory = MakeTrajectory();

trajectory.ForgetAfter(t0_ + 12 * Second);
EXPECT_EQ(3, trajectory.segments().size());
EXPECT_EQ(t0_, trajectory.begin()->first);
EXPECT_EQ(t0_ + 11 * Second, trajectory.rbegin()->first);
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()->first);
EXPECT_EQ(t0_ + 6 * Second, trajectory.rbegin()->first);
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()->first);
EXPECT_EQ(t0_ + 4 * Second, trajectory.rbegin()->first);
EXPECT_EQ(t0_, trajectory.begin()->time);
EXPECT_EQ(t0_ + 4 * Second, trajectory.rbegin()->time);
}

TEST_F(DiscreteTraject0ryTest, ForgetBefore) {
auto trajectory = MakeTrajectory();

trajectory.ForgetBefore(t0_ + 3 * Second);
EXPECT_EQ(3, trajectory.segments().size());
EXPECT_EQ(t0_ + 3 * Second, trajectory.begin()->first);
EXPECT_EQ(t0_ + 14 * Second, trajectory.rbegin()->first);
EXPECT_EQ(t0_ + 3 * Second, trajectory.begin()->time);
EXPECT_EQ(t0_ + 14 * Second, trajectory.rbegin()->time);

trajectory.ForgetBefore(t0_ + 6.1 * Second);
EXPECT_EQ(2, trajectory.segments().size());
EXPECT_EQ(t0_ + 7 * Second, trajectory.begin()->first);
EXPECT_EQ(t0_ + 14 * Second, trajectory.rbegin()->first);
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(t0_ + 9 * Second, trajectory.begin()->first);
EXPECT_EQ(t0_ + 14 * Second, trajectory.rbegin()->first);
EXPECT_EQ(t0_ + 9 * Second, trajectory.begin()->time);
EXPECT_EQ(t0_ + 14 * Second, trajectory.rbegin()->time);
}

TEST_F(DiscreteTraject0ryTest, TMinTMaxEvaluate) {
@@ -531,14 +531,16 @@ TEST_F(DiscreteTraject0ryTest, SerializationRoundTrip) {
message1, /*tracked=*/{&deserialized_second_segment});

// Check that the tracked segment was properly retrieved.
EXPECT_EQ(t0_ + 4 * Second, deserialized_second_segment->begin()->first);
EXPECT_EQ(t0_ + 9 * Second, deserialized_second_segment->rbegin()->first);
EXPECT_EQ(t0_ + 4 * Second, deserialized_second_segment->begin()->time);
EXPECT_EQ(t0_ + 9 * Second, deserialized_second_segment->rbegin()->time);

// Check that the exact points are exact.
EXPECT_EQ(deserialized_trajectory.lower_bound(t0_ + 2 * Second)->second,
trajectory.lower_bound(t0_ + 2 * Second)->second);
EXPECT_EQ(deserialized_trajectory.lower_bound(t0_ + 3 * Second)->second,
trajectory.lower_bound(t0_ + 3 * Second)->second);
EXPECT_EQ(
deserialized_trajectory.lower_bound(t0_ + 2 * Second)->degrees_of_freedom,
trajectory.lower_bound(t0_ + 2 * Second)->degrees_of_freedom);
EXPECT_EQ(
deserialized_trajectory.lower_bound(t0_ + 3 * Second)->degrees_of_freedom,
trajectory.lower_bound(t0_ + 3 * Second)->degrees_of_freedom);

serialization::DiscreteTrajectory message2;
deserialized_trajectory.WriteToMessage(
10 changes: 5 additions & 5 deletions physics/discrete_trajectory_iterator_body.hpp
Original file line number Diff line number Diff line change
@@ -15,7 +15,7 @@ DiscreteTrajectoryIterator<Frame>&
DiscreteTrajectoryIterator<Frame>::operator++() {
CHECK(!is_at_end(point_));
auto& point = iterator(point_);
Instant const previous_time = point->first;
Instant const previous_time = point->time;
do {
if (point == --segment_->timeline_end()) {
++segment_;
@@ -28,7 +28,7 @@ DiscreteTrajectoryIterator<Frame>::operator++() {
} else {
++point;
}
} while (point->first == previous_time);
} while (point->time == previous_time);
return *this;
}

@@ -44,7 +44,7 @@ DiscreteTrajectoryIterator<Frame>::operator--() {
}
auto& point = iterator(point_);
std::optional<Instant> const previous_time =
point_is_at_end ? std::nullopt : std::make_optional(point->first);
point_is_at_end ? std::nullopt : std::make_optional(point->time);
do {
if (point == segment_->timeline_begin()) {
CHECK(!segment_.is_begin());
@@ -53,7 +53,7 @@ DiscreteTrajectoryIterator<Frame>::operator--() {
} else {
--point;
}
} while (point->first == previous_time);
} while (point->time == previous_time);
return *this;
}

@@ -95,7 +95,7 @@ bool DiscreteTrajectoryIterator<Frame>::operator==(
} else if (is_at_end(other.point_)) {
return false;
} else {
return iterator(point_)->first == iterator(other.point_)->first;
return iterator(point_)->time == iterator(other.point_)->time;
}
}

42 changes: 21 additions & 21 deletions physics/discrete_trajectory_iterator_test.cpp
Original file line number Diff line number Diff line change
@@ -97,26 +97,26 @@ class DiscreteTrajectoryIteratorTest : public ::testing::Test {
TEST_F(DiscreteTrajectoryIteratorTest, ForwardOneSegment) {
auto segment = segments_->begin();
auto iterator = MakeBegin(segment);
EXPECT_EQ(t0_ + 2 * Second, iterator->first);
EXPECT_EQ(t0_ + 2 * Second, iterator->time);
auto const current = ++iterator;
EXPECT_EQ(t0_ + 3 * Second, iterator->first);
EXPECT_EQ(t0_ + 3 * Second, current->first);
EXPECT_EQ(t0_ + 3 * Second, iterator->time);
EXPECT_EQ(t0_ + 3 * Second, current->time);
auto const previous = iterator++;
EXPECT_EQ(t0_ + 5 * Second, iterator->first);
EXPECT_EQ(t0_ + 3 * Second, previous->first);
EXPECT_EQ(t0_ + 5 * Second, iterator->time);
EXPECT_EQ(t0_ + 3 * Second, previous->time);
}

TEST_F(DiscreteTrajectoryIteratorTest, BackwardOneSegment) {
auto segment = --segments_->end();
auto iterator = MakeEnd(segment);
--iterator;
EXPECT_EQ(t0_ + 23 * Second, (*iterator).first);
EXPECT_EQ(t0_ + 23 * Second, (*iterator).time);
auto const current = --iterator;
EXPECT_EQ(t0_ + 19 * Second, (*iterator).first);
EXPECT_EQ(t0_ + 19 * Second, (*current).first);
EXPECT_EQ(t0_ + 19 * Second, (*iterator).time);
EXPECT_EQ(t0_ + 19 * Second, (*current).time);
auto const previous = iterator--;
EXPECT_EQ(t0_ + 17 * Second, (*iterator).first);
EXPECT_EQ(t0_ + 19 * Second, (*previous).first);
EXPECT_EQ(t0_ + 17 * Second, (*iterator).time);
EXPECT_EQ(t0_ + 19 * Second, (*previous).time);
}

TEST_F(DiscreteTrajectoryIteratorTest, ForwardAcrossSegments) {
@@ -125,11 +125,11 @@ TEST_F(DiscreteTrajectoryIteratorTest, ForwardAcrossSegments) {
for (int i = 0; i < 4; ++i) {
++iterator;
}
EXPECT_EQ(t0_ + 11 * Second, iterator->first);
EXPECT_EQ(t0_ + 11 * Second, iterator->time);
++iterator;
EXPECT_EQ(t0_ + 13 * Second, iterator->first);
EXPECT_EQ(t0_ + 13 * Second, iterator->time);
++iterator;
EXPECT_EQ(t0_ + 17 * Second, iterator->first);
EXPECT_EQ(t0_ + 17 * Second, iterator->time);
}

TEST_F(DiscreteTrajectoryIteratorTest, BackwardAcrossSegments) {
@@ -138,11 +138,11 @@ TEST_F(DiscreteTrajectoryIteratorTest, BackwardAcrossSegments) {
for (int i = 0; i < 3; ++i) {
--iterator;
}
EXPECT_EQ(t0_ + 17 * Second, (*iterator).first);
EXPECT_EQ(t0_ + 17 * Second, (*iterator).time);
--iterator;
EXPECT_EQ(t0_ + 13 * Second, (*iterator).first);
EXPECT_EQ(t0_ + 13 * Second, (*iterator).time);
--iterator;
EXPECT_EQ(t0_ + 11 * Second, (*iterator).first);
EXPECT_EQ(t0_ + 11 * Second, (*iterator).time);
}

TEST_F(DiscreteTrajectoryIteratorTest, Equality) {
@@ -152,17 +152,17 @@ TEST_F(DiscreteTrajectoryIteratorTest, Equality) {
for (int i = 0; i < 3; ++i) {
--it1;
}
EXPECT_EQ(t0_ + 17 * Second, (*it1).first);
EXPECT_EQ(t0_ + 17 * Second, (*it1).time);
--it1;
EXPECT_EQ(t0_ + 13 * Second, (*it1).first);
EXPECT_EQ(t0_ + 13 * Second, (*it1).time);

auto it2 = MakeBegin(segments_->begin());
for (int i = 0; i < 4; ++i) {
++it2;
}
EXPECT_EQ(t0_ + 11 * Second, it2->first);
EXPECT_EQ(t0_ + 11 * Second, it2->time);
++it2;
EXPECT_EQ(t0_ + 13 * Second, it2->first);
EXPECT_EQ(t0_ + 13 * Second, it2->time);

EXPECT_EQ(it1, it2);
EXPECT_NE(it1, MakeBegin(segments_->begin()));
@@ -209,7 +209,7 @@ TEST_F(DiscreteTrajectoryIteratorTest, EmptyLastSegment) {
auto segments = MakeSegments(2);
auto segment = segments->begin();
Append(segment, t0_, unmoving_origin_);
EXPECT_EQ(t0_, segment->rbegin()->first);
EXPECT_EQ(t0_, segment->rbegin()->time);
}

} // namespace physics
46 changes: 25 additions & 21 deletions physics/discrete_trajectory_segment_body.hpp
Original file line number Diff line number Diff line change
@@ -127,20 +127,20 @@ DiscreteTrajectorySegment<Frame>::upper_bound(Instant const& t) const {

template<typename Frame>
Instant DiscreteTrajectorySegment<Frame>::t_min() const {
return empty() ? InfiniteFuture : timeline_.cbegin()->first;
return empty() ? InfiniteFuture : timeline_.cbegin()->time;
}

template<typename Frame>
Instant DiscreteTrajectorySegment<Frame>::t_max() const {
return empty() ? InfinitePast : timeline_.crbegin()->first;
return empty() ? InfinitePast : timeline_.crbegin()->time;
}

template<typename Frame>
Position<Frame> DiscreteTrajectorySegment<Frame>::EvaluatePosition(
Instant const& t) const {
auto const it = timeline_.lower_bound(t);
if (it->first == t) {
return it->second.position();
if (it->time == t) {
return it->degrees_of_freedom.position();
}
CHECK_LT(t_min(), t);
CHECK_GT(t_max(), t);
@@ -151,8 +151,8 @@ template<typename Frame>
Velocity<Frame> DiscreteTrajectorySegment<Frame>::EvaluateVelocity(
Instant const& t) const {
auto const it = timeline_.lower_bound(t);
if (it->first == t) {
return it->second.velocity();
if (it->time == t) {
return it->degrees_of_freedom.velocity();
}
CHECK_LT(t_min(), t);
CHECK_GT(t_max(), t);
@@ -164,8 +164,8 @@ DegreesOfFreedom<Frame>
DiscreteTrajectorySegment<Frame>::EvaluateDegreesOfFreedom(
Instant const& t) const {
auto const it = timeline_.lower_bound(t);
if (it->first == t) {
return it->second;
if (it->time == t) {
return it->degrees_of_freedom;
}
CHECK_LT(t_min(), t);
CHECK_GT(t_max(), t);
@@ -207,7 +207,7 @@ void DiscreteTrajectorySegment<Frame>::WriteToMessage(
// guarantee that serialization is reproducible.
auto time_comparator = [](value_type const* const left,
value_type const* const right) {
return left->first < right->first;
return left->time < right->time;
};
absl::btree_set<value_type const*,
decltype(time_comparator)> exact_set(time_comparator);
@@ -345,7 +345,7 @@ DiscreteTrajectorySegment<Frame>::ReadFromMessage(
if (auto it = exact.find(time); it == exact.cend()) {
segment.Append(time, DegreesOfFreedom<Frame>(q, p));
} else {
segment.Append(time, it->second);
segment.Append(time, it->degrees_of_freedom);
}
}

@@ -372,18 +372,18 @@ template<typename Frame>
absl::Status DiscreteTrajectorySegment<Frame>::Append(
Instant const& t,
DegreesOfFreedom<Frame> const& degrees_of_freedom) {
if (!timeline_.empty() && timeline_.cbegin()->first == t) {
if (!timeline_.empty() && timeline_.cbegin()->time == t) {
LOG(WARNING) << "Append at existing time " << t << ", time range = ["
<< timeline_.cbegin()->first << ", "
<< timeline_.crbegin()->first << "]";
<< timeline_.cbegin()->time << ", "
<< timeline_.crbegin()->time << "]";
return absl::OkStatus();
}
auto it = timeline_.emplace_hint(timeline_.cend(),
t,
degrees_of_freedom);
CHECK(++it == timeline_.end())
<< "Append out of order at " << t << ", last time is "
<< timeline_.crbegin()->first;
<< timeline_.crbegin()->time;

if (downsampling_parameters_.has_value()) {
return DownsampleIfNeeded();
@@ -440,10 +440,10 @@ void DiscreteTrajectorySegment<Frame>::SetStartOfDenseTimeline(

template<typename Frame>
void DiscreteTrajectorySegment<Frame>::SetForkPoint(value_type const& point) {
auto const it =
timeline_.emplace_hint(timeline_.begin(), point.first, point.second);
auto const it = timeline_.emplace_hint(
timeline_.begin(), point.time, point.degrees_of_freedom);
CHECK(it == timeline_.begin())
<< "Inconsistent fork point at time " << point.first;
<< "Inconsistent fork point at time " << point.time;
}

template<typename Frame>
@@ -465,9 +465,13 @@ absl::Status DiscreteTrajectorySegment<Frame>::DownsampleIfNeeded() {
absl::StatusOr<std::list<typename ConstIterators::const_iterator>>
right_endpoints = FitHermiteSpline<Instant, Position<Frame>>(
dense_iterators,
[](auto&& it) -> auto&& { return it->first; },
[](auto&& it) -> auto&& { return it->second.position(); },
[](auto&& it) -> auto&& { return it->second.velocity(); },
[](auto&& it) -> auto&& { return it->time; },
[](auto&& it) -> auto&& {
return it->degrees_of_freedom.position();
},
[](auto&& it) -> auto&& {
return it->degrees_of_freedom.velocity();
},
downsampling_parameters_->tolerance);
if (!right_endpoints.ok()) {
// Note that the actual appending took place; the propagated status only
@@ -486,7 +490,7 @@ absl::Status DiscreteTrajectorySegment<Frame>::DownsampleIfNeeded() {
std::vector<Instant> right_endpoints_times;
right_endpoints_times.reserve(right_endpoints.value().size());
for (auto const& it_in_dense_iterators : right_endpoints.value()) {
right_endpoints_times.push_back((*it_in_dense_iterators)->first);
right_endpoints_times.push_back((*it_in_dense_iterators)->time);
}

// Poke holes in the timeline at the places given by
63 changes: 33 additions & 30 deletions physics/discrete_trajectory_segment_test.cpp
Original file line number Diff line number Diff line change
@@ -106,35 +106,35 @@ class DiscreteTrajectorySegmentTest : public ::testing::Test {
};

TEST_F(DiscreteTrajectorySegmentTest, BackFront) {
EXPECT_EQ(t0_ + 2 * Second, segment_->front().first);
EXPECT_EQ(t0_ + 11 * Second, segment_->back().first);
EXPECT_EQ(t0_ + 2 * Second, segment_->front().time);
EXPECT_EQ(t0_ + 11 * Second, segment_->back().time);
}

TEST_F(DiscreteTrajectorySegmentTest, Extremities) {
{
auto const it = segment_->begin();
EXPECT_EQ(t0_ + 2 * Second, it->first);
EXPECT_EQ(t0_ + 2 * Second, it->time);
}
{
auto it = segment_->end();
--it;
EXPECT_EQ(t0_ + 11 * Second, it->first);
EXPECT_EQ(t0_ + 11 * Second, it->time);
}
{
auto const it = segment_->rbegin();
EXPECT_EQ(t0_ + 11 * Second, it->first);
EXPECT_EQ(t0_ + 11 * Second, it->time);
}
{
auto it = segment_->rend();
--it;
EXPECT_EQ(t0_ + 2 * Second, it->first);
EXPECT_EQ(t0_ + 2 * Second, it->time);
}
}

TEST_F(DiscreteTrajectorySegmentTest, Find) {
{
auto const it = segment_->find(t0_ + 5 * Second);
EXPECT_EQ(t0_ + 5 * Second, it->first);
EXPECT_EQ(t0_ + 5 * Second, it->time);
}
{
auto const it = segment_->find(t0_ + 6 * Second);
@@ -145,23 +145,23 @@ TEST_F(DiscreteTrajectorySegmentTest, Find) {
TEST_F(DiscreteTrajectorySegmentTest, LowerBoundUpperBound) {
{
auto const it = segment_->lower_bound(t0_ + 5 * Second);
EXPECT_EQ(t0_ + 5 * Second, it->first);
EXPECT_EQ(t0_ + 5 * Second, it->time);
}
{
auto const it = segment_->lower_bound(t0_ + 6 * Second);
EXPECT_EQ(t0_ + 7 * Second, it->first);
EXPECT_EQ(t0_ + 7 * Second, it->time);
}
{
auto const it = segment_->lower_bound(t0_ + 12 * Second);
EXPECT_TRUE(it == segment_->end());
}
{
auto const it = segment_->upper_bound(t0_ + 5 * Second);
EXPECT_EQ(t0_ + 7 * Second, it->first);
EXPECT_EQ(t0_ + 7 * Second, it->time);
}
{
auto const it = segment_->upper_bound(t0_ + 6 * Second);
EXPECT_EQ(t0_ + 7 * Second, it->first);
EXPECT_EQ(t0_ + 7 * Second, it->time);
}
{
auto const it = segment_->upper_bound(t0_ + 11 * Second);
@@ -176,32 +176,32 @@ TEST_F(DiscreteTrajectorySegmentTest, EmptySize) {

TEST_F(DiscreteTrajectorySegmentTest, ForgetAfterExisting) {
ForgetAfter(t0_ + 5 * Second);
EXPECT_EQ(t0_ + 3 * Second, segment_->rbegin()->first);
EXPECT_EQ(t0_ + 3 * Second, segment_->rbegin()->time);
}

TEST_F(DiscreteTrajectorySegmentTest, ForgetAfterNonexisting) {
ForgetAfter(t0_ + 6 * Second);
EXPECT_EQ(t0_ + 5 * Second, segment_->rbegin()->first);
EXPECT_EQ(t0_ + 5 * Second, segment_->rbegin()->time);
}

TEST_F(DiscreteTrajectorySegmentTest, ForgetAfterPastTheEnd) {
ForgetAfter(t0_ + 29 * Second);
EXPECT_EQ(t0_ + 11 * Second, segment_->rbegin()->first);
EXPECT_EQ(t0_ + 11 * Second, segment_->rbegin()->time);
}

TEST_F(DiscreteTrajectorySegmentTest, ForgetBeforeExisting) {
ForgetBefore(t0_ + 7 * Second);
EXPECT_EQ(t0_ + 7 * Second, segment_->begin()->first);
EXPECT_EQ(t0_ + 7 * Second, segment_->begin()->time);
}

TEST_F(DiscreteTrajectorySegmentTest, ForgetBeforeNonexisting) {
ForgetBefore(t0_ + 6 * Second);
EXPECT_EQ(t0_ + 7 * Second, segment_->begin()->first);
EXPECT_EQ(t0_ + 7 * Second, segment_->begin()->time);
}

TEST_F(DiscreteTrajectorySegmentTest, ForgetBeforeTheBeginning) {
ForgetBefore(t0_ + 1 * Second);
EXPECT_EQ(t0_ + 2 * Second, segment_->begin()->first);
EXPECT_EQ(t0_ + 2 * Second, segment_->begin()->time);
}

TEST_F(DiscreteTrajectorySegmentTest, Evaluate) {
@@ -294,7 +294,7 @@ TEST_F(DiscreteTrajectorySegmentTest, DownsamplingForgetAfter) {
/*to=*/forgotten_circle);

// Forget one of the trajectories in the middle, and append new points.
Instant const restart_time = forgotten_circle.lower_bound(t2)->first;
Instant const restart_time = forgotten_circle.lower_bound(t2)->time;
ForgetAfter(t2, forgotten_circle);
AppendTrajectoryTimeline(
NewCircularTrajectoryTimeline<World>(ω, r, Δt, restart_time, t3),
@@ -308,10 +308,10 @@ TEST_F(DiscreteTrajectorySegmentTest, DownsamplingForgetAfter) {
// Check that the two trajectories are identical.
for (auto const [t, degrees_of_freedom] : forgotten_circle) {
position_errors.push_back(
(circle.find(t)->second.position() -
(circle.find(t)->degrees_of_freedom.position() -
degrees_of_freedom.position()).Norm());
velocity_errors.push_back(
(circle.find(t)->second.velocity() -
(circle.find(t)->degrees_of_freedom.velocity() -
degrees_of_freedom.velocity()).Norm());
}
EXPECT_THAT(*std::max_element(position_errors.begin(), position_errors.end()),
@@ -366,14 +366,18 @@ TEST_F(DiscreteTrajectorySegmentTest, SerializationWithDownsampling) {
AbsoluteErrorFrom(degrees_of_freedom1.velocity(),
Lt(1.1 * Milli(Metre) / Second)));
}
EXPECT_NE(deserialized_circle.lower_bound(t0_ + 1 * Second)->second,
circle.lower_bound(t0_ + 1 * Second)->second);
EXPECT_EQ(deserialized_circle.lower_bound(t0_ + 2 * Second)->second,
circle.lower_bound(t0_ + 2 * Second)->second);
EXPECT_EQ(deserialized_circle.lower_bound(t0_ + 3 * Second)->second,
circle.lower_bound(t0_ + 3 * Second)->second);
EXPECT_NE(deserialized_circle.lower_bound(t0_ + 4 * Second)->second,
circle.lower_bound(t0_ + 4 * Second)->second);
EXPECT_NE(
deserialized_circle.lower_bound(t0_ + 1 * Second)->degrees_of_freedom,
circle.lower_bound(t0_ + 1 * Second)->degrees_of_freedom);
EXPECT_EQ(
deserialized_circle.lower_bound(t0_ + 2 * Second)->degrees_of_freedom,
circle.lower_bound(t0_ + 2 * Second)->degrees_of_freedom);
EXPECT_EQ(
deserialized_circle.lower_bound(t0_ + 3 * Second)->degrees_of_freedom,
circle.lower_bound(t0_ + 3 * Second)->degrees_of_freedom);
EXPECT_NE(
deserialized_circle.lower_bound(t0_ + 4 * Second)->degrees_of_freedom,
circle.lower_bound(t0_ + 4 * Second)->degrees_of_freedom);

// Appending may result in different downsampling because the positions differ
// a bit.
@@ -390,8 +394,7 @@ TEST_F(DiscreteTrajectorySegmentTest, SerializationWithDownsampling) {
EXPECT_THAT(circle.size(), Eq(77));
EXPECT_THAT(deserialized_circle.size(), Eq(78));
for (Instant t = t0_;
t <= std::min(circle.rbegin()->first,
deserialized_circle.rbegin()->first);
t <= std::min(circle.rbegin()->time, deserialized_circle.rbegin()->time);
t += Δt) {
EXPECT_THAT(
deserialized_circle.EvaluatePosition(t),
28 changes: 26 additions & 2 deletions physics/discrete_trajectory_types.hpp
Original file line number Diff line number Diff line change
@@ -2,7 +2,7 @@

#include <list>

#include "absl/container/btree_map.h"
#include "absl/container/btree_set.h"
#include "base/macros.hpp"
#include "geometry/named_quantities.hpp"
#include "physics/degrees_of_freedom.hpp"
@@ -31,12 +31,36 @@ struct DownsamplingParameters {
Length tolerance;
};

template<typename Frame>
struct value_type {
value_type(Instant const& time,
DegreesOfFreedom<Frame> const& degrees_of_freedom);
Instant time;
DegreesOfFreedom<Frame> degrees_of_freedom;
};

struct Earlier {
using is_transparent = void;

Earlier() = default;

template<typename Frame>
bool operator()(value_type<Frame> const& left,
value_type<Frame> const& right) const;
template<typename Frame>
bool operator()(Instant const& left, value_type<Frame> const& right) const;
template<typename Frame>
bool operator()(value_type<Frame> const& left, Instant const& right) const;
};

template<typename Frame>
using Segments = std::list<DiscreteTrajectorySegment<Frame>>;

template<typename Frame>
using Timeline = absl::btree_map<Instant, DegreesOfFreedom<Frame>>;
using Timeline = absl::btree_set<value_type<Frame>, Earlier>;

} // namespace internal_discrete_trajectory_types
} // namespace physics
} // namespace principia

#include "physics/discrete_trajectory_types_body.hpp"
33 changes: 33 additions & 0 deletions physics/discrete_trajectory_types_body.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
#pragma once
#include "physics/discrete_trajectory_types.hpp"

namespace principia {
namespace physics {
namespace internal_discrete_trajectory_types {

template<typename Frame>
value_type<Frame>::value_type(Instant const& time,
DegreesOfFreedom<Frame> const& degrees_of_freedom)
: time(time), degrees_of_freedom(degrees_of_freedom) {}

template<typename Frame>
bool Earlier::operator()(value_type<Frame> const& left,
value_type<Frame> const& right) const {
return left.time < right.time;
}

template<typename Frame>
bool Earlier::operator()(Instant const& left,
value_type<Frame> const& right) const {
return left < right.time;
}

template<typename Frame>
bool Earlier::operator()(value_type<Frame> const& left,
Instant const& right) const {
return left.time < right;
}

} // namespace internal_discrete_trajectory_types
} // namespace physics
} // namespace principia
1 change: 1 addition & 0 deletions physics/physics.vcxproj
Original file line number Diff line number Diff line change
@@ -42,6 +42,7 @@
<ClInclude Include="discrete_trajectory_segment_range.hpp" />
<ClInclude Include="discrete_trajectory_segment_range_body.hpp" />
<ClInclude Include="discrete_trajectory_types.hpp" />
<ClInclude Include="discrete_trajectory_types_body.hpp" />
<ClInclude Include="mechanical_system.hpp" />
<ClInclude Include="mechanical_system_body.hpp" />
<ClInclude Include="continuous_trajectory_body.hpp" />
3 changes: 3 additions & 0 deletions physics/physics.vcxproj.filters
Original file line number Diff line number Diff line change
@@ -224,6 +224,9 @@
<ClInclude Include="discrete_traject0ry_body.hpp">
<Filter>Source Files</Filter>
</ClInclude>
<ClInclude Include="discrete_trajectory_types_body.hpp">
<Filter>Source Files</Filter>
</ClInclude>
</ItemGroup>
<ItemGroup>
<ClCompile Include="degrees_of_freedom_test.cpp">
8 changes: 4 additions & 4 deletions testing_utilities/discrete_trajectory_factories_test.cpp
Original file line number Diff line number Diff line change
@@ -64,9 +64,9 @@ TEST_F(DiscreteTrajectoryFactoriesTest, NewLinearTrajectoryTimeline) {
1));
EXPECT_THAT(velocity.Norm(), AlmostEquals(Sqrt(77) * Metre / Second, 0, 0));
}
EXPECT_THAT(timeline.begin()->first,
EXPECT_THAT(timeline.begin()->time,
AlmostEquals(Instant() + 4 * Second, 0));
EXPECT_THAT(timeline.rbegin()->first,
EXPECT_THAT(timeline.rbegin()->time,
AlmostEquals(Instant() + 41.9 * Second, 46));
EXPECT_EQ(380, timeline.size());
}
@@ -88,9 +88,9 @@ TEST_F(DiscreteTrajectoryFactoriesTest, NewCircularTrajectoryTimeline) {
EXPECT_THAT(InnerProduct(position - World::origin, velocity),
VanishesBefore(1 * Metre * Metre / Second, 0, 8));
}
EXPECT_THAT(timeline.begin()->first,
EXPECT_THAT(timeline.begin()->time,
AlmostEquals(Instant() + 4 * Second, 0));
EXPECT_THAT(timeline.rbegin()->first,
EXPECT_THAT(timeline.rbegin()->time,
AlmostEquals(Instant() + 41.9 * Second, 46));
EXPECT_EQ(380, timeline.size());
}

0 comments on commit a26020a

Please sign in to comment.