Merge pull request #3148 from pleroy/Evaluate

DiscreteTrajectorySegment, part 3: evaluation

Author: pleroy

physics/discrete_trajectory_segment.hpp

@@ -7,10 +7,12 @@
 #include "absl/container/btree_set.h"
 #include "absl/status/status.h"
 #include "geometry/named_quantities.hpp"
+#include "numerics/hermite3.hpp"
 #include "physics/degrees_of_freedom.hpp"
 #include "physics/discrete_trajectory_iterator.hpp"
 #include "physics/discrete_trajectory_segment_iterator.hpp"
 #include "physics/discrete_trajectory_types.hpp"
+#include "physics/trajectory.hpp"
 
 namespace principia {
 
@@ -28,10 +30,13 @@ class DiscreteTrajectorySegmentTest;
 namespace internal_discrete_trajectory_segment {
 
 using geometry::Instant;
+using geometry::Position;
+using geometry::Velocity;
+using numerics::Hermite3;
 using physics::DegreesOfFreedom;
 
 template<typename Frame>
-class DiscreteTrajectorySegment {
+class DiscreteTrajectorySegment : public Trajectory<Frame> {
   using Timeline = internal_discrete_trajectory_types::Timeline<Frame>;
 
  public:
@@ -61,14 +66,22 @@ class DiscreteTrajectorySegment {
   reverse_iterator rbegin() const;
   reverse_iterator rend() const;
 
+  // TODO(phl): We probably don't want empty segments.
+  bool empty() const;
+  virtual std::int64_t size() const;
+
   iterator find(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;
+  Instant t_min() const override;
+  Instant t_max() const override;
+
+  Position<Frame> EvaluatePosition(Instant const& t) const override;
+  Velocity<Frame> EvaluateVelocity(Instant const& t) const override;
+  DegreesOfFreedom<Frame> EvaluateDegreesOfFreedom(
+      Instant const& t) const override;
 
  private:
   absl::Status Append(Instant const& t,
@@ -84,6 +97,11 @@ class DiscreteTrajectorySegment {
   void ForgetBefore(Instant const& t);
   void ForgetBefore(typename Timeline::const_iterator end);
 
+  // Returns the Hermite interpolation for the left-open, right-closed
+  // trajectory segment bounded above by |upper|.
+  Hermite3<Instant, Position<Frame>> GetInterpolation(
+      typename Timeline::const_iterator upper) const;
+
   virtual typename Timeline::const_iterator timeline_begin() const;
   virtual typename Timeline::const_iterator timeline_end() const;
 

physics/discrete_trajectory_segment_body.hpp

@@ -2,12 +2,18 @@
 
 #include "physics/discrete_trajectory_segment.hpp"
 
+#include <iterator>
+
+#include "astronomy/epoch.hpp"
 #include "glog/logging.h"
 
 namespace principia {
 namespace physics {
 namespace internal_discrete_trajectory_segment {
 
+using astronomy::InfiniteFuture;
+using astronomy::InfinitePast;
+
 template<typename Frame>
 DiscreteTrajectorySegment<Frame>::DiscreteTrajectorySegment(
     DiscreteTrajectorySegmentIterator<Frame> const self)
@@ -44,6 +50,18 @@ DiscreteTrajectorySegment<Frame>::rend() const {
   return reverse_iterator(begin());
 }
 
+template<typename Frame>
+bool DiscreteTrajectorySegment<Frame>::empty() const {
+  return timeline_.empty();
+}
+
+template<typename Frame>
+std::int64_t DiscreteTrajectorySegment<Frame>::size() const {
+  // NOTE(phl): This assumes that there are no repeated times *within* a
+  // segment.  This is enforced by Append.
+  return timeline_.size();
+}
+
 template<typename Frame>
 typename DiscreteTrajectorySegment<Frame>::iterator
 DiscreteTrajectorySegment<Frame>::find(Instant const& t) const {
@@ -78,15 +96,51 @@ DiscreteTrajectorySegment<Frame>::upper_bound(Instant const& t) const {
 }
 
 template<typename Frame>
-bool DiscreteTrajectorySegment<Frame>::empty() const {
-  return timeline_.empty();
+Instant DiscreteTrajectorySegment<Frame>::t_min() const {
+  return empty() ? InfiniteFuture : timeline_.cbegin()->first;
 }
 
 template<typename Frame>
-std::int64_t DiscreteTrajectorySegment<Frame>::size() const {
-  // NOTE(phl): This assumes that there are no repeated times *within* a
-  // segment.  This is enforced by Append.
-  return timeline_.size();
+Instant DiscreteTrajectorySegment<Frame>::t_max() const {
+  return empty() ? InfinitePast : timeline_.crbegin()->first;
+}
+
+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();
+  }
+  CHECK_LT(t_min(), t);
+  CHECK_GT(t_max(), t);
+  return GetInterpolation(it).Evaluate(t);
+}
+
+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();
+  }
+  CHECK_LT(t_min(), t);
+  CHECK_GT(t_max(), t);
+  return GetInterpolation(it).EvaluateDerivative(t);
+}
+
+template<typename Frame>
+DegreesOfFreedom<Frame>
+DiscreteTrajectorySegment<Frame>::EvaluateDegreesOfFreedom(
+    Instant const& t) const {
+  auto const it = timeline_.lower_bound(t);
+  if (it->first == t) {
+    return it->second;
+  }
+  CHECK_LT(t_min(), t);
+  CHECK_GT(t_max(), t);
+  auto const interpolation = GetInterpolation(it);
+  return {interpolation.Evaluate(t), interpolation.EvaluateDerivative(t)};
 }
 
 template<typename Frame>
@@ -136,6 +190,22 @@ void DiscreteTrajectorySegment<Frame>::ForgetBefore(
   // TODO(phl): Downsampling.
 }
 
+template<typename Frame>
+Hermite3<Instant, Position<Frame>>
+DiscreteTrajectorySegment<Frame>::GetInterpolation(
+    typename Timeline::const_iterator const upper) const {
+  CHECK(upper != timeline_.cbegin());
+  auto const lower = std::prev(upper);
+  auto const& [lower_time, lower_degrees_of_freedom] = *lower;
+  auto const& [upper_time, upper_degrees_of_freedom] = *upper;
+  return Hermite3<Instant, Position<Frame>>{
+      {lower_time, upper_time},
+      {lower_degrees_of_freedom.position(),
+       upper_degrees_of_freedom.position()},
+      {lower_degrees_of_freedom.velocity(),
+       upper_degrees_of_freedom.velocity()}};
+}
+
 template<typename Frame>
 typename DiscreteTrajectorySegment<Frame>::Timeline::const_iterator
 DiscreteTrajectorySegment<Frame>::timeline_begin() const {

physics/discrete_trajectory_segment_test.cpp

@@ -1,22 +1,41 @@
-#include "physics/discrete_trajectory_segment.hpp"
+#include "physics/discrete_trajectory_segment.hpp"
 
 #include <memory>
+#include <vector>
 
 #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/named_quantities.hpp"
+#include "quantities/quantities.hpp"
 #include "quantities/si.hpp"
+#include "testing_utilities/approximate_quantity.hpp"
+#include "testing_utilities/discrete_trajectory_factories.hpp"
+#include "testing_utilities/is_near.hpp"
 
 namespace principia {
 namespace physics {
 
 using base::check_not_null;
 using geometry::Frame;
 using geometry::Instant;
+using quantities::Abs;
+using quantities::AngularFrequency;
+using quantities::Length;
+using quantities::Speed;
+using quantities::Time;
+using quantities::si::Metre;
+using quantities::si::Milli;
+using quantities::si::Nano;
+using quantities::si::Radian;
 using quantities::si::Second;
+using testing_utilities::IsNear;
+using testing_utilities::NewCircularTrajectorySegment;
+using testing_utilities::operator""_⑴;
+using ::testing::Eq;
 
 class DiscreteTrajectorySegmentTest : public ::testing::Test {
  protected:
@@ -144,5 +163,31 @@ TEST_F(DiscreteTrajectorySegmentTest, ForgetBeforeTheBeginning) {
   EXPECT_EQ(t0_ + 2 * Second, segment_->begin()->first);
 }
 
+TEST_F(DiscreteTrajectorySegmentTest, Evaluate) {
+  AngularFrequency const ω = 3 * Radian / Second;
+  Length const r = 2 * Metre;
+  Time const Δt = 10 * Milli(Second);
+  Instant const t1 = t0_;
+  Instant const t2 = t0_ + 10 * Second;
+  auto circle = NewCircularTrajectorySegment<World>(ω, r, Δt, t1, t2);
+  auto& segment = **circle->cbegin();
+
+  EXPECT_THAT(segment.size(), Eq(1001));
+  std::vector<Length> position_errors;
+  std::vector<Speed> velocity_errors;
+  for (Instant t = segment.t_min();
+       t <= segment.t_max();
+       t += 1 * Milli(Second)) {
+    position_errors.push_back(
+        Abs((segment.EvaluatePosition(t) - World::origin).Norm() - r));
+    velocity_errors.push_back(
+        Abs(segment.EvaluateVelocity(t).Norm() - r * ω / Radian));
+  }
+  EXPECT_THAT(*std::max_element(position_errors.begin(), position_errors.end()),
+              IsNear(4.2_⑴ * Nano(Metre)));
+  EXPECT_THAT(*std::max_element(velocity_errors.begin(), velocity_errors.end()),
+              IsNear(10.4_⑴ * Nano(Metre / Second)));
+}
+
 }  // namespace physics
 }  // namespace principia

testing_utilities/discrete_trajectory_factories.hpp

@@ -46,6 +46,11 @@ class DiscreteTrajectoryFactoriesFriend {
 // TODO(phl): Must return unique_ptr because copying Segments is a bad idea due
 // to the pointers within iterators.
 
+// An empty trajectory.  Convenient for initializations.
+template<typename Frame>
+not_null<std::unique_ptr<Segments<Frame>>>
+NewEmptyTrajectorySegment();
+
 // A linear trajectory with constant velocity, going through
 // |degrees_of_freedom.position()| at t = 0.  The first point is at time |t1|,
 // the last point at a time < |t2|.
@@ -81,13 +86,14 @@ NewCircularTrajectorySegment(Time const& period,
                              Instant const& t2);
 
 template<typename Frame>
-void AppendTrajectorySegments(Segments<Frame> const& from,
-                              Segments<Frame>& to);
+void AppendTrajectorySegment(DiscreteTrajectorySegment<Frame> const& from,
+                             DiscreteTrajectorySegment<Frame>& to);
 
 }  // namespace internal_discrete_trajectory_factories
 
-using internal_discrete_trajectory_factories::AppendTrajectorySegments;
+using internal_discrete_trajectory_factories::AppendTrajectorySegment;
 using internal_discrete_trajectory_factories::NewCircularTrajectorySegment;
+using internal_discrete_trajectory_factories::NewEmptyTrajectorySegment;
 using internal_discrete_trajectory_factories::NewLinearTrajectorySegment;
 
 }  // namespace testing_utilities

testing_utilities/discrete_trajectory_factories_body.hpp

@@ -44,16 +44,23 @@ DiscreteTrajectoryFactoriesFriend<Frame>::MakeDiscreteTrajectorySegment(
 
 template<typename Frame>
 not_null<std::unique_ptr<Segments<Frame>>>
-NewLinearTrajectorySegment(DegreesOfFreedom<Frame> const& degrees_of_freedom,
-                           Time const& Δt,
-                           Instant const& t1,
-                           Instant const& t2) {
-  static Instant const t0;
+NewEmptyTrajectorySegment() {
   auto segments = std::make_unique<Segments<Frame>>(1);
   auto const it = segments->begin();
   *it = std::make_unique<DiscreteTrajectorySegment<Frame>>(
       DiscreteTrajectoryFactoriesFriend<Frame>::MakeDiscreteTrajectorySegment(
           *segments, it));
+  return segments;
+}
+
+template<typename Frame>
+not_null<std::unique_ptr<Segments<Frame>>>
+NewLinearTrajectorySegment(DegreesOfFreedom<Frame> const& degrees_of_freedom,
+                           Time const& Δt,
+                           Instant const& t1,
+                           Instant const& t2) {
+  static Instant const t0;
+  auto segments = NewEmptyTrajectorySegment<Frame>();
   auto& segment = **segments->cbegin();
   for (auto t = t1; t < t2; t += Δt) {
     auto const velocity = degrees_of_freedom.velocity();
@@ -82,11 +89,7 @@ NewCircularTrajectorySegment(AngularFrequency const& ω,
                              Instant const& t1,
                              Instant const& t2) {
   static Instant const t0;
-  auto segments = std::make_unique<Segments<Frame>>(1);
-  auto const it = segments->begin();
-  *it = std::make_unique<DiscreteTrajectorySegment<Frame>>(
-      DiscreteTrajectoryFactoriesFriend<Frame>::MakeDiscreteTrajectorySegment(
-          *segments, it));
+  auto segments = NewEmptyTrajectorySegment<Frame>();
   auto& segment = **segments->cbegin();
   Speed const v = ω * r / Radian;
   for (auto t = t1; t < t2; t += Δt) {
@@ -117,10 +120,10 @@ NewCircularTrajectorySegment(Time const& period,
 }
 
 template<typename Frame>
-void AppendTrajectorySegments(Segments<Frame> const& from,
-                              Segments<Frame>& to) {
-  for (auto const& [t, dof] : *from.front()) {
-    DiscreteTrajectoryFactoriesFriend<Frame>::Append(t, dof, to.rbegin());
+void AppendTrajectorySegment(DiscreteTrajectorySegment<Frame> const& from,
+                             DiscreteTrajectorySegment<Frame>& to) {
+  for (auto const& [t, degrees_of_freedom] : from) {
+    DiscreteTrajectoryFactoriesFriend<Frame>::Append(t, degrees_of_freedom, to);
   }
 }
 

testing_utilities/discrete_trajectory_factories_test.cpp

@@ -39,6 +39,12 @@ class DiscreteTrajectoryFactoriesTest : public ::testing::Test {
                       serialization::Frame::TEST>;
 };
 
+TEST_F(DiscreteTrajectoryFactoriesTest, NewEmptyTrajectorySegment) {
+  auto const segments = NewEmptyTrajectorySegment<World>();
+  auto const& segment = *segments->front();
+  EXPECT_TRUE(segment.empty());
+}
+
 TEST_F(DiscreteTrajectoryFactoriesTest, NewLinearTrajectorySegment) {
   auto const segments = NewLinearTrajectorySegment<World>(
       /*degrees_of_freedom=*/