From c05c6f0384b258a174854039d25e0b1765b44d5e Mon Sep 17 00:00:00 2001 From: pleroy Date: Sat, 9 Oct 2021 18:58:53 +0200 Subject: [PATCH 1/7] A test that works. --- physics/discrete_trajectory2.hpp | 6 + physics/discrete_trajectory_iterator.hpp | 22 ++-- physics/discrete_trajectory_segment.hpp | 38 ++++--- physics/discrete_trajectory_segment_body.hpp | 107 ++++++++++++++++-- .../discrete_trajectory_segment_iterator.hpp | 13 ++- physics/discrete_trajectory_segment_test.cpp | 67 +++++++++++ physics/physics.vcxproj | 1 + physics/physics.vcxproj.filters | 3 + 8 files changed, 225 insertions(+), 32 deletions(-) create mode 100644 physics/discrete_trajectory_segment_test.cpp diff --git a/physics/discrete_trajectory2.hpp b/physics/discrete_trajectory2.hpp index 5b4e2c086a..59c25ea3f9 100644 --- a/physics/discrete_trajectory2.hpp +++ b/physics/discrete_trajectory2.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -34,6 +35,11 @@ using physics::DegreesOfFreedom; template class DiscreteTrajectory2 : public Trajectory { public: + using key_type = + typename internal_discrete_trajectory_types::Timeline::key_type; + using value_type = + typename internal_discrete_trajectory_types::Timeline::value_type; + using iterator = DiscreteTrajectoryIterator; using reverse_iterator = std::reverse_iterator; using SegmentIterator = DiscreteTrajectorySegmentIterator; diff --git a/physics/discrete_trajectory_iterator.hpp b/physics/discrete_trajectory_iterator.hpp index c355b7f392..6e1ab59130 100644 --- a/physics/discrete_trajectory_iterator.hpp +++ b/physics/discrete_trajectory_iterator.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include "absl/container/btree_map.h" @@ -11,8 +12,9 @@ namespace principia { namespace physics { -template -class DiscreteTrajectory; +FORWARD_DECLARE_FROM(discrete_trajectory_segment, + TEMPLATE(typename Frame) class, + DiscreteTrajectorySegment); namespace internal_discrete_trajectory_iterator { @@ -22,6 +24,12 @@ using physics::DegreesOfFreedom; template class DiscreteTrajectoryIterator { public: + using difference_type = std::int64_t; + using value_type = + typename internal_discrete_trajectory_types::Timeline::value_type; + using pointer = value_type const*; + using reference = value_type const&; + DiscreteTrajectoryIterator() = default; DiscreteTrajectoryIterator& operator++(); @@ -29,12 +37,8 @@ class DiscreteTrajectoryIterator { DiscreteTrajectoryIterator operator++(int); DiscreteTrajectoryIterator operator--(int); - typename - internal_discrete_trajectory_types::Timeline::value_type const& - operator*() const; - typename - internal_discrete_trajectory_types::Timeline::value_type const* - operator->() const; + reference operator*() const; + pointer operator->() const; bool operator==(DiscreteTrajectoryIterator const& other) const; bool operator!=(DiscreteTrajectoryIterator const& other) const; @@ -68,6 +72,8 @@ class DiscreteTrajectoryIterator { // times. std::optional previous_time_; + template + friend class DiscreteTrajectorySegment; friend class DiscreteTrajectoryIteratorTest; }; diff --git a/physics/discrete_trajectory_segment.hpp b/physics/discrete_trajectory_segment.hpp index 31a625fc78..a1b0dae2f0 100644 --- a/physics/discrete_trajectory_segment.hpp +++ b/physics/discrete_trajectory_segment.hpp @@ -1,9 +1,11 @@ #pragma once #include +#include #include "absl/container/btree_map.h" #include "absl/container/btree_set.h" +#include "absl/status/status.h" #include "geometry/named_quantities.hpp" #include "physics/degrees_of_freedom.hpp" #include "physics/discrete_trajectory_iterator.hpp" @@ -14,6 +16,7 @@ namespace principia { namespace physics { class DiscreteTrajectoryIteratorTest; +class DiscreteTrajectorySegmentTest; namespace internal_discrete_trajectory_segment { @@ -22,8 +25,20 @@ using physics::DegreesOfFreedom; template class DiscreteTrajectorySegment { + using Timeline = internal_discrete_trajectory_types::Timeline; + public: + using key_type = typename Timeline::key_type; + using value_type = typename Timeline::value_type; + + using iterator = DiscreteTrajectoryIterator; + using reverse_iterator = std::reverse_iterator; + + // TODO(phl): Decide which constructors should be public. DiscreteTrajectorySegment() = default; + explicit DiscreteTrajectorySegment( + DiscreteTrajectorySegmentIterator self); + virtual ~DiscreteTrajectorySegment() = default; // Moveable. @@ -33,27 +48,23 @@ class DiscreteTrajectorySegment { DiscreteTrajectorySegment& operator=(const DiscreteTrajectorySegment&) = delete; - DiscreteTrajectoryIterator begin() const; - DiscreteTrajectoryIterator end() const; + iterator begin() const; + iterator end() const; - DiscreteTrajectoryIterator rbegin() const; - DiscreteTrajectoryIterator rend() const; + reverse_iterator rbegin() const; + reverse_iterator rend() const; - DiscreteTrajectoryIterator find(Instant const& t) const; + iterator find(Instant const& t) const; - DiscreteTrajectoryIterator lower_bound(Instant const& t) const; - DiscreteTrajectoryIterator upper_bound(Instant const& t) const; + iterator lower_bound(Instant const& t) const; + iterator upper_bound(Instant const& t) const; bool empty() const; virtual std::int64_t size() const; - protected: - // For mocking. - using Timeline = internal_discrete_trajectory_types::Timeline; - private: - void Append(Instant const& t, - DegreesOfFreedom const& degrees_of_freedom); + absl::Status Append(Instant const& t, + DegreesOfFreedom const& degrees_of_freedom); void ForgetAfter(Instant const& t); void ForgetAfter(typename Timeline::const_iterator begin); @@ -73,6 +84,7 @@ class DiscreteTrajectorySegment { friend class internal_discrete_trajectory_iterator:: DiscreteTrajectoryIterator; friend class DiscreteTrajectoryIteratorTest; + friend class DiscreteTrajectorySegmentTest; }; } // namespace internal_discrete_trajectory_segment diff --git a/physics/discrete_trajectory_segment_body.hpp b/physics/discrete_trajectory_segment_body.hpp index 50fdb2f48d..7fccf67f51 100644 --- a/physics/discrete_trajectory_segment_body.hpp +++ b/physics/discrete_trajectory_segment_body.hpp @@ -1,27 +1,118 @@ #pragma once -#include "glog/logging.h" #include "physics/discrete_trajectory_segment.hpp" +#include "glog/logging.h" + namespace principia { namespace physics { namespace internal_discrete_trajectory_segment { template -typename DiscreteTrajectorySegment::Timeline::const_iterator -DiscreteTrajectorySegment::timeline_begin() const { - LOG(FATAL) << "NYI"; +DiscreteTrajectorySegment::DiscreteTrajectorySegment( + DiscreteTrajectorySegmentIterator const self) + : self_(self) {} + +template +typename DiscreteTrajectorySegment::iterator +DiscreteTrajectorySegment::begin() const { + return iterator(self_, timeline_.begin()); } template -typename DiscreteTrajectorySegment::Timeline::const_iterator -DiscreteTrajectorySegment::timeline_end() const { - LOG(FATAL) << "NYI"; +typename DiscreteTrajectorySegment::iterator +DiscreteTrajectorySegment::end() const { + // TODO(phl): or begin of next segment? + return iterator(self_, timeline_.end()); +} + +template +typename DiscreteTrajectorySegment::reverse_iterator +DiscreteTrajectorySegment::rbegin() const { + return reverse_iterator(end()); +} + +template +typename DiscreteTrajectorySegment::reverse_iterator +DiscreteTrajectorySegment::rend() const { + return reverse_iterator(begin()); +} + +template +typename DiscreteTrajectorySegment::iterator +DiscreteTrajectorySegment::find(Instant const& t) const { + return iterator(self_, timeline_.find(t)); +} + +template +typename DiscreteTrajectorySegment::iterator +DiscreteTrajectorySegment::lower_bound(Instant const& t) const { + return iterator(self_, timeline_.lower_bound(t)); +} + +template +typename DiscreteTrajectorySegment::iterator +DiscreteTrajectorySegment::upper_bound(Instant const& t) const { + return iterator(self_, timeline_.upper_bound(t)); +} + +template +bool DiscreteTrajectorySegment::empty() const { + return timeline_.empty(); } template std::int64_t DiscreteTrajectorySegment::size() const { - LOG(FATAL) << "NYI"; + // NOTE(phl): This assumes that there are no repeated times *within* a + // segment. This is enforced by Append. + return timeline_.size(); +} + +template +absl::Status DiscreteTrajectorySegment::Append( + Instant const& t, + DegreesOfFreedom const& degrees_of_freedom) { + if (!timeline_.empty() && timeline_.cbegin()->first == t) { + LOG(WARNING) << "Append at existing time " << t << ", time range = [" + << timeline_.cbegin()->first << ", " + << timeline_.crbegin()->first << "]"; + 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; + + // TODO(phl): Downsampling. + return absl::OkStatus(); +} + +template +void DiscreteTrajectorySegment::ForgetAfter(Instant const& t) {} + +template +void DiscreteTrajectorySegment::ForgetAfter( + typename Timeline::const_iterator const begin) {} + +template +void DiscreteTrajectorySegment::ForgetBefore(Instant const& t) {} + +template +void DiscreteTrajectorySegment::ForgetBefore( + typename Timeline::const_iterator const end) {} + +template +typename DiscreteTrajectorySegment::Timeline::const_iterator +DiscreteTrajectorySegment::timeline_begin() const { + return timeline_.cbegin(); +} + +template +typename DiscreteTrajectorySegment::Timeline::const_iterator +DiscreteTrajectorySegment::timeline_end() const { + return timeline_.cend(); } } // namespace internal_discrete_trajectory_segment diff --git a/physics/discrete_trajectory_segment_iterator.hpp b/physics/discrete_trajectory_segment_iterator.hpp index ba7d7edcd0..cea24254a3 100644 --- a/physics/discrete_trajectory_segment_iterator.hpp +++ b/physics/discrete_trajectory_segment_iterator.hpp @@ -17,6 +17,7 @@ FORWARD_DECLARE_FROM(discrete_trajectory_segment, class DiscreteTrajectoryIteratorTest; class DiscreteTrajectorySegmentIteratorTest; +class DiscreteTrajectorySegmentTest; namespace internal_discrete_trajectory_segment_iterator { @@ -25,6 +26,11 @@ using base::not_null; template class DiscreteTrajectorySegmentIterator { public: + using difference_type = std::int64_t; + using value_type = DiscreteTrajectorySegment; + using pointer = value_type const*; + using reference = value_type const&; + DiscreteTrajectorySegmentIterator() = default; DiscreteTrajectorySegmentIterator& operator++(); @@ -32,8 +38,8 @@ class DiscreteTrajectorySegmentIterator { DiscreteTrajectorySegmentIterator operator++(int); DiscreteTrajectorySegmentIterator operator--(int); - DiscreteTrajectorySegment const& operator*() const; - DiscreteTrajectorySegment const* operator->() const; + reference operator*() const; + pointer operator->() const; bool operator==(DiscreteTrajectorySegmentIterator const& other) const; bool operator!=(DiscreteTrajectorySegmentIterator const& other) const; @@ -51,10 +57,11 @@ class DiscreteTrajectorySegmentIterator { Segments const* segments_ = nullptr; typename Segments::const_iterator iterator_; - template + template friend class DiscreteTrajectoryIterator; friend class DiscreteTrajectoryIteratorTest; friend class DiscreteTrajectorySegmentIteratorTest; + friend class DiscreteTrajectorySegmentTest; }; } // namespace internal_discrete_trajectory_segment_iterator diff --git a/physics/discrete_trajectory_segment_test.cpp b/physics/discrete_trajectory_segment_test.cpp new file mode 100644 index 0000000000..e610e125bb --- /dev/null +++ b/physics/discrete_trajectory_segment_test.cpp @@ -0,0 +1,67 @@ +#include "physics/discrete_trajectory_segment.hpp" + +#include + +#include "base/not_null.hpp" +#include "geometry/frame.hpp" +#include "geometry/named_quantities.hpp" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +#include "physics/discrete_trajectory_types.hpp" +#include "quantities/si.hpp" + +namespace principia { +namespace physics { + +using base::check_not_null; +using geometry::Frame; +using geometry::Instant; +using quantities::si::Second; + +class DiscreteTrajectorySegmentTest : public ::testing::Test { + protected: + using World = Frame; + + DiscreteTrajectorySegmentTest() : segments_(1) { + auto const it = segments_.begin(); + *it = std::make_unique>( + DiscreteTrajectorySegmentIterator(check_not_null(&segments_), + it)); + segment_ = segments_.cbegin()->get(); + + segment_->Append(t0_ + 2 * Second, unmoving_origin_); + segment_->Append(t0_ + 3 * Second, unmoving_origin_); + segment_->Append(t0_ + 5 * Second, unmoving_origin_); + segment_->Append(t0_ + 7 * Second, unmoving_origin_); + segment_->Append(t0_ + 11 * Second, unmoving_origin_); + } + + DiscreteTrajectorySegment* segment_; + internal_discrete_trajectory_types::Segments segments_; + Instant const t0_; + DegreesOfFreedom unmoving_origin_{World::origin, World::unmoving}; +}; + +TEST_F(DiscreteTrajectorySegmentTest, Extremities) { + { + auto it = segment_->begin(); + EXPECT_EQ(t0_ + 2 * Second, it->first); + } + { + auto it = segment_->end(); + --it; + EXPECT_EQ(t0_ + 11 * Second, it->first); + } + { + auto it = segment_->rbegin(); + EXPECT_EQ(t0_ + 11 * Second, it->first); + } + { + auto it = segment_->rend(); + --it; + EXPECT_EQ(t0_ + 2 * Second, it->first); + } +} + +} // namespace physics +} // namespace principia diff --git a/physics/physics.vcxproj b/physics/physics.vcxproj index 3f2d3133d1..97daf8c584 100644 --- a/physics/physics.vcxproj +++ b/physics/physics.vcxproj @@ -106,6 +106,7 @@ + diff --git a/physics/physics.vcxproj.filters b/physics/physics.vcxproj.filters index 0c57968abd..14ebba50a8 100644 --- a/physics/physics.vcxproj.filters +++ b/physics/physics.vcxproj.filters @@ -331,5 +331,8 @@ Test Files + + Test Files + \ No newline at end of file From ef0fb82572a39448286f8fec882cf9a9df9bfe90 Mon Sep 17 00:00:00 2001 From: pleroy Date: Sat, 9 Oct 2021 19:48:53 +0200 Subject: [PATCH 2/7] Another test passing. --- physics/discrete_trajectory_iterator.hpp | 3 ++- physics/discrete_trajectory_iterator_body.hpp | 22 ++++++++++++------- physics/discrete_trajectory_segment_test.cpp | 15 +++++++++++-- 3 files changed, 29 insertions(+), 11 deletions(-) diff --git a/physics/discrete_trajectory_iterator.hpp b/physics/discrete_trajectory_iterator.hpp index 6e1ab59130..60357432fb 100644 --- a/physics/discrete_trajectory_iterator.hpp +++ b/physics/discrete_trajectory_iterator.hpp @@ -53,7 +53,8 @@ class DiscreteTrajectoryIterator { DiscreteTrajectoryIterator(DiscreteTrajectorySegmentIterator segment, OptionalTimelineConstIterator point); - static bool is_at_end(OptionalTimelineConstIterator point); + static bool is_at_segment_end(DiscreteTrajectoryIterator it); + static bool is_at_trajectory_end(OptionalTimelineConstIterator point); static typename Timeline::const_iterator& iterator( OptionalTimelineConstIterator& point); diff --git a/physics/discrete_trajectory_iterator_body.hpp b/physics/discrete_trajectory_iterator_body.hpp index 17ac56bf9b..eb7c1ef54e 100644 --- a/physics/discrete_trajectory_iterator_body.hpp +++ b/physics/discrete_trajectory_iterator_body.hpp @@ -13,7 +13,7 @@ using geometry::Instant; template DiscreteTrajectoryIterator& DiscreteTrajectoryIterator::operator++() { - CHECK(!is_at_end(point_)); + CHECK(!is_at_trajectory_end(point_)); auto& point = iterator(point_); for (;;) { if (point == --segment_->timeline_end()) { @@ -38,7 +38,7 @@ DiscreteTrajectoryIterator::operator++() { template DiscreteTrajectoryIterator& DiscreteTrajectoryIterator::operator--() { - if (is_at_end(point_)) { + if (is_at_trajectory_end(point_)) { segment_ = --segment_.end(); point_ = --segment_->timeline_end(); } else { @@ -79,23 +79,23 @@ DiscreteTrajectoryIterator::operator--(int) { // NOLINT template typename internal_discrete_trajectory_types::Timeline::value_type const& DiscreteTrajectoryIterator::operator*() const { - CHECK(!is_at_end(point_)); + CHECK(!is_at_segment_end(*this)); return *iterator(point_); } template typename internal_discrete_trajectory_types::Timeline::value_type const* DiscreteTrajectoryIterator::operator->() const { - CHECK(!is_at_end(point_)); + CHECK(!is_at_segment_end(*this)); return &*iterator(point_); } template bool DiscreteTrajectoryIterator::operator==( DiscreteTrajectoryIterator const& other) const { - if (is_at_end(point_)) { - return segment_ == other.segment_ && is_at_end(other.point_); - } else if (is_at_end(other.point_)) { + if (is_at_segment_end(*this)) { + return segment_ == other.segment_ && is_at_segment_end(other); + } else if (is_at_segment_end(other)) { return false; } else { return iterator(point_)->first == iterator(other.point_)->first; @@ -116,7 +116,13 @@ DiscreteTrajectoryIterator::DiscreteTrajectoryIterator( point_(point) {} template -bool DiscreteTrajectoryIterator::is_at_end( +bool DiscreteTrajectoryIterator::is_at_segment_end( + DiscreteTrajectoryIterator const it) { + return !it.point_.has_value() || it.point_ == it.segment_->timeline_end(); +} + +template +bool DiscreteTrajectoryIterator::is_at_trajectory_end( OptionalTimelineConstIterator const point) { return !point.has_value(); } diff --git a/physics/discrete_trajectory_segment_test.cpp b/physics/discrete_trajectory_segment_test.cpp index e610e125bb..c9b07a3605 100644 --- a/physics/discrete_trajectory_segment_test.cpp +++ b/physics/discrete_trajectory_segment_test.cpp @@ -44,7 +44,7 @@ class DiscreteTrajectorySegmentTest : public ::testing::Test { TEST_F(DiscreteTrajectorySegmentTest, Extremities) { { - auto it = segment_->begin(); + auto const it = segment_->begin(); EXPECT_EQ(t0_ + 2 * Second, it->first); } { @@ -53,7 +53,7 @@ TEST_F(DiscreteTrajectorySegmentTest, Extremities) { EXPECT_EQ(t0_ + 11 * Second, it->first); } { - auto it = segment_->rbegin(); + auto const it = segment_->rbegin(); EXPECT_EQ(t0_ + 11 * Second, it->first); } { @@ -63,5 +63,16 @@ TEST_F(DiscreteTrajectorySegmentTest, Extremities) { } } +TEST_F(DiscreteTrajectorySegmentTest, Find) { + { + auto const it = segment_->find(t0_ + 5 * Second); + EXPECT_EQ(t0_ + 5 * Second, it->first); + } + { + auto const it = segment_->find(t0_ + 6 * Second); + EXPECT_TRUE(it == segment_->end()); + } +} + } // namespace physics } // namespace principia From 2921eb5025eca72538e2857265f29e70469365f6 Mon Sep 17 00:00:00 2001 From: pleroy Date: Sun, 10 Oct 2021 00:25:13 +0200 Subject: [PATCH 3/7] Lower/upper bound. --- physics/discrete_trajectory_segment_test.cpp | 27 ++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/physics/discrete_trajectory_segment_test.cpp b/physics/discrete_trajectory_segment_test.cpp index c9b07a3605..090b96e569 100644 --- a/physics/discrete_trajectory_segment_test.cpp +++ b/physics/discrete_trajectory_segment_test.cpp @@ -74,5 +74,32 @@ TEST_F(DiscreteTrajectorySegmentTest, Find) { } } +TEST_F(DiscreteTrajectorySegmentTest, LowerBoundUpperBound) { + { + auto const it = segment_->lower_bound(t0_ + 5 * Second); + EXPECT_EQ(t0_ + 5 * Second, it->first); + } + { + auto const it = segment_->lower_bound(t0_ + 6 * Second); + EXPECT_EQ(t0_ + 7 * Second, it->first); + } + { + 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); + } + { + auto const it = segment_->upper_bound(t0_ + 6 * Second); + EXPECT_EQ(t0_ + 7 * Second, it->first); + } + { + auto const it = segment_->upper_bound(t0_ + 11 * Second); + EXPECT_TRUE(it == segment_->end()); + } +} + } // namespace physics } // namespace principia From 4f0a84ebb7e22b34df9be56e396acea7f1bfda11 Mon Sep 17 00:00:00 2001 From: pleroy Date: Sun, 10 Oct 2021 00:26:34 +0200 Subject: [PATCH 4/7] Empty and size. --- physics/discrete_trajectory_segment_test.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/physics/discrete_trajectory_segment_test.cpp b/physics/discrete_trajectory_segment_test.cpp index 090b96e569..e997f04461 100644 --- a/physics/discrete_trajectory_segment_test.cpp +++ b/physics/discrete_trajectory_segment_test.cpp @@ -101,5 +101,10 @@ TEST_F(DiscreteTrajectorySegmentTest, LowerBoundUpperBound) { } } +TEST_F(DiscreteTrajectorySegmentTest, EmptySize) { + EXPECT_FALSE(segment_->empty()); + EXPECT_EQ(5, segment_->size()); +} + } // namespace physics } // namespace principia From 45fb21e3f5183805737cf1f3c8977da9d3c103c4 Mon Sep 17 00:00:00 2001 From: pleroy Date: Sun, 10 Oct 2021 00:59:56 +0200 Subject: [PATCH 5/7] Normalize end iterator. --- physics/discrete_trajectory_iterator.hpp | 3 +- physics/discrete_trajectory_iterator_body.hpp | 22 +++++-------- physics/discrete_trajectory_segment.hpp | 1 + physics/discrete_trajectory_segment_body.hpp | 31 ++++++++++++++++--- 4 files changed, 36 insertions(+), 21 deletions(-) diff --git a/physics/discrete_trajectory_iterator.hpp b/physics/discrete_trajectory_iterator.hpp index 60357432fb..6e1ab59130 100644 --- a/physics/discrete_trajectory_iterator.hpp +++ b/physics/discrete_trajectory_iterator.hpp @@ -53,8 +53,7 @@ class DiscreteTrajectoryIterator { DiscreteTrajectoryIterator(DiscreteTrajectorySegmentIterator segment, OptionalTimelineConstIterator point); - static bool is_at_segment_end(DiscreteTrajectoryIterator it); - static bool is_at_trajectory_end(OptionalTimelineConstIterator point); + static bool is_at_end(OptionalTimelineConstIterator point); static typename Timeline::const_iterator& iterator( OptionalTimelineConstIterator& point); diff --git a/physics/discrete_trajectory_iterator_body.hpp b/physics/discrete_trajectory_iterator_body.hpp index eb7c1ef54e..17ac56bf9b 100644 --- a/physics/discrete_trajectory_iterator_body.hpp +++ b/physics/discrete_trajectory_iterator_body.hpp @@ -13,7 +13,7 @@ using geometry::Instant; template DiscreteTrajectoryIterator& DiscreteTrajectoryIterator::operator++() { - CHECK(!is_at_trajectory_end(point_)); + CHECK(!is_at_end(point_)); auto& point = iterator(point_); for (;;) { if (point == --segment_->timeline_end()) { @@ -38,7 +38,7 @@ DiscreteTrajectoryIterator::operator++() { template DiscreteTrajectoryIterator& DiscreteTrajectoryIterator::operator--() { - if (is_at_trajectory_end(point_)) { + if (is_at_end(point_)) { segment_ = --segment_.end(); point_ = --segment_->timeline_end(); } else { @@ -79,23 +79,23 @@ DiscreteTrajectoryIterator::operator--(int) { // NOLINT template typename internal_discrete_trajectory_types::Timeline::value_type const& DiscreteTrajectoryIterator::operator*() const { - CHECK(!is_at_segment_end(*this)); + CHECK(!is_at_end(point_)); return *iterator(point_); } template typename internal_discrete_trajectory_types::Timeline::value_type const* DiscreteTrajectoryIterator::operator->() const { - CHECK(!is_at_segment_end(*this)); + CHECK(!is_at_end(point_)); return &*iterator(point_); } template bool DiscreteTrajectoryIterator::operator==( DiscreteTrajectoryIterator const& other) const { - if (is_at_segment_end(*this)) { - return segment_ == other.segment_ && is_at_segment_end(other); - } else if (is_at_segment_end(other)) { + if (is_at_end(point_)) { + return segment_ == other.segment_ && is_at_end(other.point_); + } else if (is_at_end(other.point_)) { return false; } else { return iterator(point_)->first == iterator(other.point_)->first; @@ -116,13 +116,7 @@ DiscreteTrajectoryIterator::DiscreteTrajectoryIterator( point_(point) {} template -bool DiscreteTrajectoryIterator::is_at_segment_end( - DiscreteTrajectoryIterator const it) { - return !it.point_.has_value() || it.point_ == it.segment_->timeline_end(); -} - -template -bool DiscreteTrajectoryIterator::is_at_trajectory_end( +bool DiscreteTrajectoryIterator::is_at_end( OptionalTimelineConstIterator const point) { return !point.has_value(); } diff --git a/physics/discrete_trajectory_segment.hpp b/physics/discrete_trajectory_segment.hpp index a1b0dae2f0..9bd0c46633 100644 --- a/physics/discrete_trajectory_segment.hpp +++ b/physics/discrete_trajectory_segment.hpp @@ -59,6 +59,7 @@ class DiscreteTrajectorySegment { iterator lower_bound(Instant const& t) const; iterator upper_bound(Instant const& t) const; + // TODO(phl): We probably don't want empty segments. bool empty() const; virtual std::int64_t size() const; diff --git a/physics/discrete_trajectory_segment_body.hpp b/physics/discrete_trajectory_segment_body.hpp index 7fccf67f51..849f6ad37c 100644 --- a/physics/discrete_trajectory_segment_body.hpp +++ b/physics/discrete_trajectory_segment_body.hpp @@ -22,8 +22,14 @@ DiscreteTrajectorySegment::begin() const { template typename DiscreteTrajectorySegment::iterator DiscreteTrajectorySegment::end() const { - // TODO(phl): or begin of next segment? - return iterator(self_, timeline_.end()); + // TODO(phl): We probably don't want empty segments. + if (timeline_.empty()) { + return iterator(self_, timeline_.begin()); + } else { + // The decrement/increment ensures that we normalize the end iterator to the + // next segment or to the end of the trajectory. + return ++iterator(self_, --timeline_.end()); + } } template @@ -41,19 +47,34 @@ DiscreteTrajectorySegment::rend() const { template typename DiscreteTrajectorySegment::iterator DiscreteTrajectorySegment::find(Instant const& t) const { - return iterator(self_, timeline_.find(t)); + auto const it = timeline_.find(t); + if (it == timeline_.end()) { + return end(); + } else { + return iterator(self_, it); + } } template typename DiscreteTrajectorySegment::iterator DiscreteTrajectorySegment::lower_bound(Instant const& t) const { - return iterator(self_, timeline_.lower_bound(t)); + auto const it = timeline_.lower_bound(t); + if (it == timeline_.end()) { + return end(); + } else { + return iterator(self_, it); + } } template typename DiscreteTrajectorySegment::iterator DiscreteTrajectorySegment::upper_bound(Instant const& t) const { - return iterator(self_, timeline_.upper_bound(t)); + auto const it = timeline_.upper_bound(t); + if (it == timeline_.end()) { + return end(); + } else { + return iterator(self_, it); + } } template From 152f0b6f81d37c1df0aaa554aff5b3690b6b8940 Mon Sep 17 00:00:00 2001 From: pleroy Date: Sun, 10 Oct 2021 01:04:45 +0200 Subject: [PATCH 6/7] Readying for review. --- physics/discrete_trajectory_iterator.hpp | 4 ++-- physics/discrete_trajectory_segment_iterator.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/physics/discrete_trajectory_iterator.hpp b/physics/discrete_trajectory_iterator.hpp index 6e1ab59130..c76bb5e77b 100644 --- a/physics/discrete_trajectory_iterator.hpp +++ b/physics/discrete_trajectory_iterator.hpp @@ -25,10 +25,10 @@ template class DiscreteTrajectoryIterator { public: using difference_type = std::int64_t; - using value_type = - typename internal_discrete_trajectory_types::Timeline::value_type; using pointer = value_type const*; using reference = value_type const&; + using value_type = + typename internal_discrete_trajectory_types::Timeline::value_type; DiscreteTrajectoryIterator() = default; diff --git a/physics/discrete_trajectory_segment_iterator.hpp b/physics/discrete_trajectory_segment_iterator.hpp index cea24254a3..87b0ee4c3b 100644 --- a/physics/discrete_trajectory_segment_iterator.hpp +++ b/physics/discrete_trajectory_segment_iterator.hpp @@ -27,9 +27,9 @@ template class DiscreteTrajectorySegmentIterator { public: using difference_type = std::int64_t; - using value_type = DiscreteTrajectorySegment; using pointer = value_type const*; using reference = value_type const&; + using value_type = DiscreteTrajectorySegment; DiscreteTrajectorySegmentIterator() = default; From b89257ade426bdb7525bf31fb43f513f08510ceb Mon Sep 17 00:00:00 2001 From: pleroy Date: Sun, 10 Oct 2021 12:02:39 +0200 Subject: [PATCH 7/7] Fix compilation errors. --- physics/discrete_trajectory_iterator.hpp | 4 ++-- physics/discrete_trajectory_segment_iterator.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/physics/discrete_trajectory_iterator.hpp b/physics/discrete_trajectory_iterator.hpp index c76bb5e77b..6e1ab59130 100644 --- a/physics/discrete_trajectory_iterator.hpp +++ b/physics/discrete_trajectory_iterator.hpp @@ -25,10 +25,10 @@ template class DiscreteTrajectoryIterator { public: using difference_type = std::int64_t; - using pointer = value_type const*; - using reference = value_type const&; using value_type = typename internal_discrete_trajectory_types::Timeline::value_type; + using pointer = value_type const*; + using reference = value_type const&; DiscreteTrajectoryIterator() = default; diff --git a/physics/discrete_trajectory_segment_iterator.hpp b/physics/discrete_trajectory_segment_iterator.hpp index 87b0ee4c3b..cea24254a3 100644 --- a/physics/discrete_trajectory_segment_iterator.hpp +++ b/physics/discrete_trajectory_segment_iterator.hpp @@ -27,9 +27,9 @@ template class DiscreteTrajectorySegmentIterator { public: using difference_type = std::int64_t; + using value_type = DiscreteTrajectorySegment; using pointer = value_type const*; using reference = value_type const&; - using value_type = DiscreteTrajectorySegment; DiscreteTrajectorySegmentIterator() = default;