DiscreteTrajectorySegment, part 1: iterators

physics/discrete_trajectory2.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <iterator>
 #include <list>
 #include <memory>
 #include <vector>
@@ -34,6 +35,11 @@ using physics::DegreesOfFreedom;
 template<typename Frame>
 class DiscreteTrajectory2 : public Trajectory<Frame> {
  public:
+  using key_type =
+      typename internal_discrete_trajectory_types::Timeline<Frame>::key_type;
+  using value_type =
+      typename internal_discrete_trajectory_types::Timeline<Frame>::value_type;
+
   using iterator = DiscreteTrajectoryIterator<Frame>;
   using reverse_iterator = std::reverse_iterator<iterator>;
   using SegmentIterator = DiscreteTrajectorySegmentIterator<Frame>;

physics/discrete_trajectory_iterator.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <cstdint>
 #include <optional>
 
 #include "absl/container/btree_map.h"
@@ -11,8 +12,9 @@
 namespace principia {
 namespace physics {
 
-template<typename Frame>
-class DiscreteTrajectory;
+FORWARD_DECLARE_FROM(discrete_trajectory_segment,
+                     TEMPLATE(typename Frame) class,
+                     DiscreteTrajectorySegment);
 
 namespace internal_discrete_trajectory_iterator {
 
@@ -22,19 +24,21 @@ using physics::DegreesOfFreedom;
 template<typename Frame>
 class DiscreteTrajectoryIterator {
  public:
+  using difference_type = std::int64_t;
+  using value_type =
+      typename internal_discrete_trajectory_types::Timeline<Frame>::value_type;
+  using pointer = value_type const*;
+  using reference = value_type const&;
+
   DiscreteTrajectoryIterator() = default;
 
   DiscreteTrajectoryIterator& operator++();
   DiscreteTrajectoryIterator& operator--();
   DiscreteTrajectoryIterator operator++(int);
   DiscreteTrajectoryIterator operator--(int);
 
-  typename
-  internal_discrete_trajectory_types::Timeline<Frame>::value_type const&
-  operator*() const;
-  typename
-  internal_discrete_trajectory_types::Timeline<Frame>::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<Instant> previous_time_;
 
+  template<typename F>
+  friend class DiscreteTrajectorySegment;
   friend class DiscreteTrajectoryIteratorTest;
 };
 

physics/discrete_trajectory_segment.hpp

@@ -1,9 +1,11 @@
 #pragma once
 
 #include <cstdint>
+#include <iterator>
 
 #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<typename Frame>
 class DiscreteTrajectorySegment {
+  using Timeline = internal_discrete_trajectory_types::Timeline<Frame>;
+
  public:
+  using key_type = typename Timeline::key_type;
+  using value_type = typename Timeline::value_type;
+
+  using iterator = DiscreteTrajectoryIterator<Frame>;
+  using reverse_iterator = std::reverse_iterator<iterator>;
+
+  // TODO(phl): Decide which constructors should be public.
   DiscreteTrajectorySegment() = default;
+  explicit DiscreteTrajectorySegment(
+      DiscreteTrajectorySegmentIterator<Frame> self);
+
   virtual ~DiscreteTrajectorySegment() = default;
 
   // Moveable.
@@ -33,27 +48,24 @@ class DiscreteTrajectorySegment {
   DiscreteTrajectorySegment& operator=(const DiscreteTrajectorySegment&) =
       delete;
 
-  DiscreteTrajectoryIterator<Frame> begin() const;
-  DiscreteTrajectoryIterator<Frame> end() const;
+  iterator begin() const;
+  iterator end() const;
 
-  DiscreteTrajectoryIterator<Frame> rbegin() const;
-  DiscreteTrajectoryIterator<Frame> rend() const;
+  reverse_iterator rbegin() const;
+  reverse_iterator rend() const;
 
-  DiscreteTrajectoryIterator<Frame> find(Instant const& t) const;
+  iterator find(Instant const& t) const;
 
-  DiscreteTrajectoryIterator<Frame> lower_bound(Instant const& t) const;
-  DiscreteTrajectoryIterator<Frame> upper_bound(Instant const& t) const;
+  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;
 
- protected:
-  // For mocking.
-  using Timeline = internal_discrete_trajectory_types::Timeline<Frame>;
-
  private:
-  void Append(Instant const& t,
-              DegreesOfFreedom<Frame> const& degrees_of_freedom);
+  absl::Status Append(Instant const& t,
+                      DegreesOfFreedom<Frame> const& degrees_of_freedom);
 
   void ForgetAfter(Instant const& t);
   void ForgetAfter(typename Timeline::const_iterator begin);
@@ -73,6 +85,7 @@ class DiscreteTrajectorySegment {
   friend class internal_discrete_trajectory_iterator::
       DiscreteTrajectoryIterator;
   friend class DiscreteTrajectoryIteratorTest;
+  friend class DiscreteTrajectorySegmentTest;
 };
 
 }  // namespace internal_discrete_trajectory_segment

physics/discrete_trajectory_segment_body.hpp

@@ -1,27 +1,139 @@
 #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 Frame>
-typename DiscreteTrajectorySegment<Frame>::Timeline::const_iterator
-DiscreteTrajectorySegment<Frame>::timeline_begin() const {
-  LOG(FATAL) << "NYI";
+DiscreteTrajectorySegment<Frame>::DiscreteTrajectorySegment(
+    DiscreteTrajectorySegmentIterator<Frame> const self)
+    : self_(self) {}
+
+template<typename Frame>
+typename DiscreteTrajectorySegment<Frame>::iterator
+DiscreteTrajectorySegment<Frame>::begin() const {
+  return iterator(self_, timeline_.begin());
 }
 
 template<typename Frame>
-typename DiscreteTrajectorySegment<Frame>::Timeline::const_iterator
-DiscreteTrajectorySegment<Frame>::timeline_end() const {
-  LOG(FATAL) << "NYI";
+typename DiscreteTrajectorySegment<Frame>::iterator
+DiscreteTrajectorySegment<Frame>::end() const {
+  // 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<typename Frame>
+typename DiscreteTrajectorySegment<Frame>::reverse_iterator
+DiscreteTrajectorySegment<Frame>::rbegin() const {
+  return reverse_iterator(end());
+}
+
+template<typename Frame>
+typename DiscreteTrajectorySegment<Frame>::reverse_iterator
+DiscreteTrajectorySegment<Frame>::rend() const {
+  return reverse_iterator(begin());
+}
+
+template<typename Frame>
+typename DiscreteTrajectorySegment<Frame>::iterator
+DiscreteTrajectorySegment<Frame>::find(Instant const& t) const {
+  auto const it = timeline_.find(t);
+  if (it == timeline_.end()) {
+    return end();
+  } else {
+    return iterator(self_, it);
+  }
+}
+
+template<typename Frame>
+typename DiscreteTrajectorySegment<Frame>::iterator
+DiscreteTrajectorySegment<Frame>::lower_bound(Instant const& t) const {
+  auto const it = timeline_.lower_bound(t);
+  if (it == timeline_.end()) {
+    return end();
+  } else {
+    return iterator(self_, it);
+  }
+}
+
+template<typename Frame>
+typename DiscreteTrajectorySegment<Frame>::iterator
+DiscreteTrajectorySegment<Frame>::upper_bound(Instant const& t) const {
+  auto const it = timeline_.upper_bound(t);
+  if (it == timeline_.end()) {
+    return end();
+  } else {
+    return iterator(self_, it);
+  }
+}
+
+template<typename Frame>
+bool DiscreteTrajectorySegment<Frame>::empty() const {
+  return timeline_.empty();
 }
 
 template<typename Frame>
 std::int64_t DiscreteTrajectorySegment<Frame>::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<typename Frame>
+absl::Status DiscreteTrajectorySegment<Frame>::Append(
+    Instant const& t,
+    DegreesOfFreedom<Frame> 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<typename Frame>
+void DiscreteTrajectorySegment<Frame>::ForgetAfter(Instant const& t) {}
+
+template<typename Frame>
+void DiscreteTrajectorySegment<Frame>::ForgetAfter(
+    typename Timeline::const_iterator const begin) {}
+
+template<typename Frame>
+void DiscreteTrajectorySegment<Frame>::ForgetBefore(Instant const& t) {}
+
+template<typename Frame>
+void DiscreteTrajectorySegment<Frame>::ForgetBefore(
+    typename Timeline::const_iterator const end) {}
+
+template<typename Frame>
+typename DiscreteTrajectorySegment<Frame>::Timeline::const_iterator
+DiscreteTrajectorySegment<Frame>::timeline_begin() const {
+  return timeline_.cbegin();
+}
+
+template<typename Frame>
+typename DiscreteTrajectorySegment<Frame>::Timeline::const_iterator
+DiscreteTrajectorySegment<Frame>::timeline_end() const {
+  return timeline_.cend();
 }
 
 }  // namespace internal_discrete_trajectory_segment

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,15 +26,20 @@ using base::not_null;
 template<typename Frame>
 class DiscreteTrajectorySegmentIterator {
  public:
+  using difference_type = std::int64_t;
+  using value_type = DiscreteTrajectorySegment<Frame>;
+  using pointer = value_type const*;
+  using reference = value_type const&;
+
   DiscreteTrajectorySegmentIterator() = default;
 
   DiscreteTrajectorySegmentIterator& operator++();
   DiscreteTrajectorySegmentIterator& operator--();
   DiscreteTrajectorySegmentIterator operator++(int);
   DiscreteTrajectorySegmentIterator operator--(int);
 
-  DiscreteTrajectorySegment<Frame> const& operator*() const;
-  DiscreteTrajectorySegment<Frame> 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<typename Frame>
+  template<typename F>
   friend class DiscreteTrajectoryIterator;
   friend class DiscreteTrajectoryIteratorTest;
   friend class DiscreteTrajectorySegmentIteratorTest;
+  friend class DiscreteTrajectorySegmentTest;
 };
 
 }  // namespace internal_discrete_trajectory_segment_iterator