mockingbirdnest · Mar 26, 2017 · Mar 27, 2017 · Mar 27, 2017 · Mar 27, 2017 · Mar 27, 2017
Showing with 171 additions and 2 deletions.

+2 −0 numerics/hermite3.hpp

+10 −0 numerics/hermite3_body.hpp

+43 −1 physics/discrete_trajectory.hpp

+70 −1 physics/discrete_trajectory_body.hpp

+46 −0 physics/discrete_trajectory_test.cpp
diff --git a/numerics/hermite3.hpp b/numerics/hermite3.hpp
@@ -23,6 +23,8 @@ class Hermite3 final {
            std::pair<Value, Value> const& values,
            std::pair<Derivative1, Derivative1> const& derivatives);
 
+  // NOTE(egg): this does not appear to use Casteljau's algorithm; perhaps it
+  // should?
   Value Evaluate(Argument const& argument) const;
   Derivative1 EvaluateDerivative(Argument const& argument) const;
 

diff --git a/numerics/hermite3_body.hpp b/numerics/hermite3_body.hpp
@@ -23,6 +23,16 @@ Hermite3<Argument, Value>::Hermite3(
   a0_ = values.first;
   a1_ = derivatives.first;
   Difference<Argument> const Δargument = arguments_.second - arguments_.first;
+  // If we were given the same point twice, there is a removable singularity.
+  // Otherwise, if the arguments are the same but not the values or the
+  // derivatives, we proceed to merrily NaN away as we should.
+  if (Δargument == Difference<Argument>{} &&
+      values.first == values.second &&
+      derivatives.first == derivatives.second) {
+    a2_ = {};
+    a3_ = {};
+    return;
+  }
   auto const one_over_Δargument = 1.0 / Δargument;
   auto const one_over_Δargument_squared =
       one_over_Δargument * one_over_Δargument;

diff --git a/physics/discrete_trajectory.hpp b/physics/discrete_trajectory.hpp
@@ -11,8 +11,10 @@
 #include "base/not_null.hpp"
 #include "geometry/grassmann.hpp"
 #include "geometry/named_quantities.hpp"
+#include "numerics/hermite3.hpp"
 #include "physics/degrees_of_freedom.hpp"
 #include "physics/forkable.hpp"
+#include "physics/trajectory.hpp"
 #include "quantities/named_quantities.hpp"
 #include "serialization/physics.pb.h"
 
@@ -54,16 +56,19 @@ namespace internal_discrete_trajectory {
 
 using base::not_null;
 using geometry::Instant;
+using geometry::Position;
 using geometry::Vector;
 using geometry::Velocity;
 using quantities::Acceleration;
 using quantities::Length;
 using quantities::Speed;
 using internal_forkable::DiscreteTrajectoryIterator;
+using numerics::Hermite3;
 
 template<typename Frame>
 class DiscreteTrajectory : public Forkable<DiscreteTrajectory<Frame>,
-                                           DiscreteTrajectoryIterator<Frame>> {
+                                           DiscreteTrajectoryIterator<Frame>>,
+                           public Trajectory<Frame> {
   using Timeline = std::map<Instant, DegreesOfFreedom<Frame>>;
   using TimelineConstIterator = typename Forkable<
       DiscreteTrajectory<Frame>,
@@ -126,6 +131,35 @@ class DiscreteTrajectory : public Forkable<DiscreteTrajectory<Frame>,
   // |time|.  This trajectory must be a root.
   void ForgetBefore(Instant const& time);
 
+  // Implementation of the interface |Trajectory|.
+
+  // The bounds are the times of |Begin()| and |last()| if this trajectory is
+  // nonempty, otherwise they are infinities of the appropriate signs.
+  Instant t_min() const override;
+  Instant t_max() const override;
+
+  not_null<std::unique_ptr<typename Trajectory<Frame>::Hint>> NewHint()
+      const override;
+
+  Position<Frame> EvaluatePosition(
+      Instant const& time,
+      typename Trajectory<Frame>::Hint* hint) const override;
+  Velocity<Frame> EvaluateVelocity(
+      Instant const& time,
+      typename Trajectory<Frame>::Hint* hint) const override;
+  DegreesOfFreedom<Frame> EvaluateDegreesOfFreedom(
+      Instant const& time,
+      typename Trajectory<Frame>::Hint* hint) const override;
+
+  // End of the implementation of the interface.
+
+  class Hint : public Trajectory<Frame>::Hint {
+   private:
+    explicit Hint(Iterator const& it);
+    Iterator it_;
+    friend class DiscreteTrajectory<Frame>;
+  };
+
   // This trajectory must be a root.  Only the given |forks| are serialized.
   // They must be descended from this trajectory.  The pointers in |forks| may
   // be null at entry.
@@ -164,6 +198,14 @@ class DiscreteTrajectory : public Forkable<DiscreteTrajectory<Frame>,
       serialization::DiscreteTrajectory const& message,
       std::vector<DiscreteTrajectory<Frame>**> const& forks);
 
+  // Returns the Hermite interpolation for the left-open, right-closed
+  // trajectory segment containing the given |time|, or, if |time| is |t_min()|,
+  // returns a first-degree polynomial which should be evaluated only at
+  // |t_min()|.
+  Hermite3<Instant, Position<Frame>> GetInterpolation(
+      Instant const& time,
+      Trajectory<Frame>::Hint* hint) const;
+
   Timeline timeline_;
 
   template<typename, typename>

diff --git a/physics/discrete_trajectory_body.hpp b/physics/discrete_trajectory_body.hpp
@@ -8,6 +8,7 @@
 #include <map>
 #include <vector>
 
+#include "astronomy/epoch.hpp"
 #include "geometry/named_quantities.hpp"
 #include "glog/logging.h"
 
@@ -50,8 +51,9 @@ DiscreteTrajectoryIterator<Frame>::that() const {
 
 namespace internal_discrete_trajectory {
 
+using astronomy::InfiniteFuture;
+using astronomy::InfinitePast;
 using base::make_not_null_unique;
-using geometry::Instant;
 
 template<typename Frame>
 typename DiscreteTrajectory<Frame>::Iterator
@@ -177,6 +179,49 @@ void DiscreteTrajectory<Frame>::ForgetBefore(Instant const& time) {
   timeline_.erase(timeline_.begin(), it);
 }
 
+template<typename Frame>
+Instant DiscreteTrajectory<Frame>::t_min() const {
+  return Empty() ? InfiniteFuture : Begin().time();
+}
+
+template<typename Frame>
+Instant DiscreteTrajectory<Frame>::t_max() const {
+  return Empty() ? InfinitePast : last().time();
+}
+
+template<typename Frame>
+not_null<std::unique_ptr<typename Trajectory<Frame>::Hint>>
+DiscreteTrajectory<Frame>::NewHint() const {
+  return std::unique_ptr<typename Trajectory<Frame>::Hint>(new Hint(Begin()));
+}
+
+template<typename Frame>
+Position<Frame> DiscreteTrajectory<Frame>::EvaluatePosition(
+    Instant const& time,
+    typename Trajectory<Frame>::Hint* hint) const {
+  return GetInterpolation(time, hint).Evaluate(time);
+}
+
+template<typename Frame>
+Velocity<Frame> DiscreteTrajectory<Frame>::EvaluateVelocity(
+    Instant const& time,
+    typename Trajectory<Frame>::Hint* hint) const {;
+  return GetInterpolation(time, hint).EvaluateDerivative(time);
+}
+
+template<typename Frame>
+DegreesOfFreedom<Frame> DiscreteTrajectory<Frame>::EvaluateDegreesOfFreedom(
+    Instant const& time,
+    typename Trajectory<Frame>::Hint* hint) const {
+  Hint* const down_cast_hint =
+      hint == nullptr ? nullptr : CHECK_NOTNULL(dynamic_cast<Hint*>(hint));
+  auto const interpolation = GetInterpolation(time, hint);
+  return {interpolation.Evaluate(time), interpolation.EvaluateDerivative(time)};
+}
+
+template<typename Frame>
+DiscreteTrajectory<Frame>::Hint::Hint(Iterator const& it) : it_(it) {}
+
 template<typename Frame>
 void DiscreteTrajectory<Frame>::WriteToMessage(
     not_null<serialization::DiscreteTrajectory*> const message,
@@ -283,6 +328,30 @@ void DiscreteTrajectory<Frame>::FillSubTreeFromMessage(
                                                                  forks);
 }
 
+template<typename Frame>
+Hermite3<Instant, Position<Frame>> DiscreteTrajectory<Frame>::GetInterpolation(
+    Instant const& time,
+    Trajectory<Frame>::Hint* hint) const {
+  Hint* const down_cast_hint =
+      hint == nullptr ? nullptr : CHECK_NOTNULL(dynamic_cast<Hint*>(hint));
+  CHECK_LE(t_min(), time);
+  CHECK_GE(t_max(), time);
+  {
+    // TODO(egg): do things with the hint.
+    Hint* const hint = down_cast_hint;
+    // This is the upper bound of the interval upon which we will do the
+    // interpolation.
+    auto const upper = LowerBound(time);
+    auto const lower = upper == Begin() ? upper : --Iterator{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()}};
+  }
+}
+
 }  // namespace internal_discrete_trajectory
 }  // namespace physics
 }  // namespace principia
diff --git a/physics/discrete_trajectory_test.cpp b/physics/discrete_trajectory_test.cpp
@@ -1,6 +1,7 @@
 
 #include "physics/discrete_trajectory.hpp"
 
+#include <algorithm>
 #include <functional>
 #include <list>
 #include <map>
@@ -16,6 +17,7 @@
 #include "gtest/gtest.h"
 #include "quantities/quantities.hpp"
 #include "quantities/si.hpp"
+#include "testing_utilities/numerics.hpp"
 
 namespace principia {
 namespace physics {
@@ -28,16 +30,23 @@ using geometry::Point;
 using geometry::Position;
 using geometry::R3Element;
 using geometry::Vector;
+using quantities::AngularFrequency;
 using quantities::Length;
 using quantities::Speed;
 using quantities::SIUnit;
+using quantities::Time;
 using quantities::si::Metre;
+using quantities::si::Radian;
 using quantities::si::Second;
+using testing_utilities::RelativeError;
 using ::std::placeholders::_1;
 using ::std::placeholders::_2;
 using ::std::placeholders::_3;
+using ::testing::AllOf;
 using ::testing::ElementsAre;
 using ::testing::Eq;
+using ::testing::Ge;
+using ::testing::Le;
 using ::testing::Pair;
 using ::testing::Ref;
 
@@ -815,6 +824,43 @@ TEST_F(DiscreteTrajectoryTest, IteratorSuccess) {
   EXPECT_TRUE(it == fork->End());
 }
 
+TEST_F(DiscreteTrajectoryTest, QuadrilateralCircle) {
+  DiscreteTrajectory<World> circle;
+  AngularFrequency const ω = 3 * Radian / Second;
+  Length const r = 2 * Metre;
+  Speed const v = ω * r / Radian;
+  Time const period = 2 * π * Radian / ω;
+  for (Time t; t <= period; t += period / 4) {
+    circle.Append(
+        t0_ + t,
+        {World::origin + Displacement<World>{{r * Cos(ω * t),
+                                              r * Sin(ω * t),
+                                              0 * Metre}},
+         Velocity<World>{{-v * Sin(ω * t),
+                          v * Cos(ω * t),
+                          0 * Metre / Second}}});
+  }
+  double max_r_error = 0;
+  double max_v_error = 0;
+  auto hint = circle.NewHint();
+  for (Time t; t < period; t += period / 32) {
+    auto const degrees_of_freedom_interpolated =
+        circle.EvaluateDegreesOfFreedom(t0_ + t, hint.get());
+    auto const& q_interpolated = degrees_of_freedom_interpolated.position();
+    auto const& v_interpolated = degrees_of_freedom_interpolated.velocity();
+    EXPECT_THAT(circle.EvaluatePosition(t0_ + t, /*hint=*/nullptr),
+                Eq(q_interpolated));
+    EXPECT_THAT(circle.EvaluateVelocity(t0_ + t, /*hint=*/nullptr),
+                Eq(v_interpolated));
+    max_r_error = std::max(
+        max_r_error, RelativeError(r, (q_interpolated - World::origin).Norm()));
+    max_v_error =
+        std::max(max_v_error, RelativeError(v, v_interpolated.Norm()));
+  }
+  EXPECT_THAT(max_r_error, AllOf(Ge(0.014), Le(0.016)));
+  EXPECT_THAT(max_v_error, AllOf(Ge(0.011), Le(0.013)));
+}
+
 }  // namespace internal_discrete_trajectory
 }  // namespace physics
 }  // namespace principia