mockingbirdnest · Nov 7, 2021 · Nov 7, 2021 · Nov 7, 2021 · Nov 7, 2021
Showing with 62 additions and 12 deletions.

+8 −4 ksp_plugin/pile_up.cpp

+14 −7 physics/discrete_traject0ry_body.hpp

+27 −1 physics/discrete_traject0ry_test.cpp

+3 −0 physics/discrete_trajectory_segment.hpp

+10 −0 physics/discrete_trajectory_segment_body.hpp
diff --git a/ksp_plugin/pile_up.cpp b/ksp_plugin/pile_up.cpp
@@ -612,12 +612,16 @@ absl::Status PileUp::AdvanceTime(Instant const& t) {
     // time we go through this function.  It will be re-created as needed.
     fixed_instance_ = nullptr;
     // We make the |psychohistory_|, if any, authoritative, i.e. append it to
-    // the end of the |history_|. We integrate on top of it, and it gets
-    // appended authoritatively to the part tails.
+    // the end of the |history_|.  We integrate on top of it.  Note how we skip
+    // the first point of the psychohistory, which is already present in the
+    // |trajectory_|.
     auto const psychohistory_trajectory =
         trajectory_.DetachSegments(psychohistory_);
-    for (auto const& [time, degrees_of_freedom] : psychohistory_trajectory) {
-      trajectory_.Append(time, degrees_of_freedom);
+    CHECK(!psychohistory_trajectory.empty());
+    for (auto it = std::next(psychohistory_trajectory.begin());
+         it != psychohistory_trajectory.end();
+         ++it) {
+      trajectory_.Append(it->time, it->degrees_of_freedom);
     }
 
     auto const intrinsic_acceleration =

diff --git a/physics/discrete_traject0ry_body.hpp b/physics/discrete_traject0ry_body.hpp
@@ -207,16 +207,23 @@ typename DiscreteTraject0ry<Frame>::SegmentIterator
 DiscreteTraject0ry<Frame>::AttachSegments(
     DiscreteTraject0ry&& trajectory) {
   CHECK(!trajectory.empty());
-  // NOTE(phl): This check might be too strict, we might want to allow LT as the
-  // time comparison, and to adjust the first point of trajectory as needed.
-  // We'll see if the clients need that.
-  CHECK_EQ(rbegin()->time, trajectory.begin()->time)
-      << "Mismatching times when attaching segments";
-  CHECK_EQ(rbegin()->degrees_of_freedom, trajectory.begin()->degrees_of_freedom)
-      << "Mismatching degrees of freedom when attaching segments";
 
   if (empty()) {
     *this = DiscreteTraject0ry(uninitialized);
+  } else if (back().time == trajectory.front().time) {
+    CHECK_EQ(back().degrees_of_freedom, trajectory.front().degrees_of_freedom)
+        << "Mismatching degrees of freedom when attaching segments";
+  } else {
+    // If the points are not matching, prepend a matching point to |trajectory|
+    // and update the time-to-segment map.
+    CHECK_LT(back().time, trajectory.front().time)
+        << "Mismatching times when attaching segments";
+    trajectory.segments_->begin()->Prepend(back().time,
+                                           back().degrees_of_freedom);
+    auto const leit = trajectory.segment_by_left_endpoint_.cbegin();
+    auto const sit = leit->second;
+    trajectory.segment_by_left_endpoint_.erase(leit);
+    trajectory.segment_by_left_endpoint_.emplace(back().time, sit);
   }
 
   // The |end| iterator keeps pointing at the end after the splice.  Instead,

diff --git a/physics/discrete_traject0ry_test.cpp b/physics/discrete_traject0ry_test.cpp
@@ -406,7 +406,7 @@ TEST_F(DiscreteTraject0ryTest, DetachSegments) {
   }
 }
 
-TEST_F(DiscreteTraject0ryTest, AttachSegments) {
+TEST_F(DiscreteTraject0ryTest, AttachSegmentsMatching) {
   auto trajectory1 = MakeTrajectory();
   auto trajectory2 = MakeTrajectory(
       t0_ + 14 * Second,
@@ -453,6 +453,32 @@ TEST_F(DiscreteTraject0ryTest, AttachSegments) {
   }
 }
 
+TEST_F(DiscreteTraject0ryTest, AttachSegmentsMismatching) {
+  auto trajectory1 = MakeTrajectory();
+  auto trajectory2 = MakeTrajectory(
+      t0_ + 15 * Second,
+      DegreesOfFreedom<World>(
+          World::origin + Displacement<World>({5 * Metre,
+                                               5 * Metre,
+                                               5 * Metre}),
+          Velocity<World>({0 * Metre / Second,
+                           0 * Metre / Second,
+                           1 * Metre / Second})));
+  trajectory1.AttachSegments(std::move(trajectory2));
+  EXPECT_EQ(6, trajectory1.segments().size());
+  EXPECT_EQ(t0_, trajectory1.begin()->time);
+  EXPECT_EQ(t0_ + 29 * Second, trajectory1.rbegin()->time);
+
+  EXPECT_EQ(trajectory1.EvaluatePosition(t0_ + 14 * Second),
+            World::origin + Displacement<World>({4 * Metre,
+                                                 4 * Metre,
+                                                 4 * Metre}));
+  EXPECT_EQ(trajectory1.EvaluatePosition(t0_ + 15 * Second),
+            World::origin + Displacement<World>({5 * Metre,
+                                                 5 * Metre,
+                                                 5 * Metre}));
+}
+
 TEST_F(DiscreteTraject0ryTest, DeleteSegments) {
   auto trajectory = MakeTrajectory();
   auto const first_segment = trajectory.segments().begin();

diff --git a/physics/discrete_trajectory_segment.hpp b/physics/discrete_trajectory_segment.hpp
@@ -140,6 +140,9 @@ class DiscreteTrajectorySegment : public Trajectory<Frame> {
   // segments.
   void SetSelf(DiscreteTrajectorySegmentIterator<Frame> self);
 
+  void Prepend(Instant const& t,
+               DegreesOfFreedom<Frame> const& degrees_of_freedom);
+
   absl::Status Append(Instant const& t,
                       DegreesOfFreedom<Frame> const& degrees_of_freedom);
 

diff --git a/physics/discrete_trajectory_segment_body.hpp b/physics/discrete_trajectory_segment_body.hpp
@@ -318,6 +318,16 @@ void DiscreteTrajectorySegment<Frame>::SetSelf(
   self_ = self;
 }
 
+template<typename Frame>
+void DiscreteTrajectorySegment<Frame>::Prepend(
+    Instant const& t,
+    DegreesOfFreedom<Frame> const& degrees_of_freedom) {
+  CHECK(!timeline_.empty() || t < timeline_.cbegin()->time)
+      << "Prepend out of order at " << t << ", first time is "
+      << timeline_.cbegin()->time;
+  timeline_.emplace_hint(timeline_.cbegin(), t, degrees_of_freedom);
+}
+
 template<typename Frame>
 absl::Status DiscreteTrajectorySegment<Frame>::Append(
     Instant const& t,