diff --git a/physics/discrete_trajectory_segment.hpp b/physics/discrete_trajectory_segment.hpp index f415f5fb73..8a5380668e 100644 --- a/physics/discrete_trajectory_segment.hpp +++ b/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; diff --git a/physics/discrete_trajectory_segment_body.hpp b/physics/discrete_trajectory_segment_body.hpp index 32d970365d..7dd3a3a4f1 100644 --- a/physics/discrete_trajectory_segment_body.hpp +++ b/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 { diff --git a/physics/discrete_trajectory_segment_test.cpp b/physics/discrete_trajectory_segment_test.cpp index 9c70435125..f1aad5e1e2 100644 --- a/physics/discrete_trajectory_segment_test.cpp +++ b/physics/discrete_trajectory_segment_test.cpp @@ -1,6 +1,7 @@ -#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" @@ -8,7 +9,12 @@ #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 { @@ -16,7 +22,20 @@ 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 diff --git a/testing_utilities/discrete_trajectory_factories.hpp b/testing_utilities/discrete_trajectory_factories.hpp index 52a5a5215d..0ccd56d2ad 100644 --- a/testing_utilities/discrete_trajectory_factories.hpp +++ b/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 diff --git a/testing_utilities/discrete_trajectory_factories_body.hpp b/testing_utilities/discrete_trajectory_factories_body.hpp index b1e1b2dc83..0a01b4127d 100644 --- a/testing_utilities/discrete_trajectory_factories_body.hpp +++ b/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); } } diff --git a/testing_utilities/discrete_trajectory_factories_test.cpp b/testing_utilities/discrete_trajectory_factories_test.cpp index cc30aa0e95..364ceb9205 100644 --- a/testing_utilities/discrete_trajectory_factories_test.cpp +++ b/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=*/