Skip to content
Permalink

Comparing changes

Choose two branches to see what’s changed or to start a new pull request. If you need to, you can also or learn more about diff comparisons.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also . Learn more about diff comparisons here.
base repository: mockingbirdnest/Principia
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: 326bf4ca225c
Choose a base ref
...
head repository: mockingbirdnest/Principia
Failed to load repositories. Confirm that selected head ref is valid, then try again.
Loading
compare: 64c4c6c124f4
Choose a head ref
  • 9 commits
  • 5 files changed
  • 1 contributor

Commits on Nov 16, 2021

  1. Start to implement Merge.

    pleroy committed Nov 16, 2021
    Copy the full SHA
    b60457c View commit details

Commits on Nov 23, 2021

  1. Copy the full SHA
    063bb2b View commit details
  2. Copy the full SHA
    e18e63e View commit details
  3. Improve Merge by moving.

    pleroy committed Nov 23, 2021
    Copy the full SHA
    9f000d8 View commit details

Commits on Nov 24, 2021

  1. A test and some fixes.

    pleroy committed Nov 24, 2021
    Copy the full SHA
    5018406 View commit details
  2. Check consistency.

    pleroy committed Nov 24, 2021
    Copy the full SHA
    9b678b2 View commit details
  3. Comments.

    pleroy committed Nov 24, 2021
    Copy the full SHA
    2036305 View commit details
  4. More comments.

    pleroy committed Nov 24, 2021
    Copy the full SHA
    a365f5d View commit details
  5. Merge pull request #3218 from pleroy/Merge

    Add a function to merge two nonoverlapping DiscreteTrajectory objects
    pleroy authored Nov 24, 2021
    Copy the full SHA
    64c4c6c View commit details
15 changes: 14 additions & 1 deletion physics/discrete_trajectory.hpp
Original file line number Diff line number Diff line change
@@ -88,7 +88,7 @@ class DiscreteTrajectory : public Trajectory<Frame> {
SegmentIterator NewSegment();

DiscreteTrajectory DetachSegments(SegmentIterator begin);
SegmentIterator AttachSegments(DiscreteTrajectory&& trajectory);
SegmentIterator AttachSegments(DiscreteTrajectory trajectory);
void DeleteSegments(SegmentIterator& begin);

// Deletes the trajectory points with a time in [t, end[. Drops the segments
@@ -105,6 +105,19 @@ class DiscreteTrajectory : public Trajectory<Frame> {
absl::Status Append(Instant const& t,
DegreesOfFreedom<Frame> const& degrees_of_freedom);

// Merges |trajectory| (the source) into this object (the target). The
// operation processes pairs of segments taken from each trajectory and
// proceeds as follows:
// 1. If the source segment is empty (or missing), leave the target segment
// unchanged.
// 2. If the target segment is empty (or missing), move the source segment
// into the target. This includes its downsampling state.
// 3. If both segments are nonempty, they must be nonoverlapping. The points
// from the source segment are inserted in the target segment (possibly
// before the beginning of that segment). The downsampling state of the
// result is that of the latest segment (the one with the largest times).
void Merge(DiscreteTrajectory<Frame> trajectory);

Instant t_min() const override;
Instant t_max() const override;

56 changes: 54 additions & 2 deletions physics/discrete_trajectory_body.hpp
Original file line number Diff line number Diff line change
@@ -204,8 +204,7 @@ DiscreteTrajectory<Frame>::DetachSegments(SegmentIterator const begin) {

template<typename Frame>
typename DiscreteTrajectory<Frame>::SegmentIterator
DiscreteTrajectory<Frame>::AttachSegments(
DiscreteTrajectory&& trajectory) {
DiscreteTrajectory<Frame>::AttachSegments(DiscreteTrajectory trajectory) {
CHECK(!trajectory.empty());

if (empty()) {
@@ -375,6 +374,59 @@ absl::Status DiscreteTrajectory<Frame>::Append(
return absl::OkStatus();
}

template<typename Frame>
void DiscreteTrajectory<Frame>::Merge(DiscreteTrajectory<Frame> trajectory) {
auto sit_s = trajectory.segments_->begin(); // Source iterator.
auto sit_t = segments_->begin(); // Target iterator.
for (;;) {
if (sit_s != trajectory.segments_->end() && sit_t != segments_->end()) {
// Record the existing left endpoint to update the time-to-segment map as
// needed.
const std::optional<Instant> left_endpoint =
sit_t->empty() ? std::nullopt
: std::make_optional(sit_t->front().time);

// Merge corresponding segments.
sit_t->Merge(std::move(*sit_s));

// If the left endpoint has changed, remove its entry from the time-to-
// segment map. Insert a new entry if the segment is not empty.
if (left_endpoint.has_value() &&
sit_t->front().time < left_endpoint.value()) {
segment_by_left_endpoint_.erase(left_endpoint.value());
}
if (!sit_t->empty()) {
segment_by_left_endpoint_.insert_or_assign(sit_t->front().time, sit_t);
}

++sit_s;
++sit_t;
} else if (sit_s != trajectory.segments_->end()) {
// No more segments in the target. We splice the segments of the source.

// The |end| iterator keeps pointing at the end after the splice.
// Instead, we track the iterator to the last segment.
auto const last_before_splice = --segments_->end();

segments_->splice(segments_->end(),
*trajectory.segments_,
sit_s,
trajectory.segments_->end());

auto const end_before_splice = std::next(last_before_splice);
AdjustAfterSplicing(/*from=*/trajectory,
/*to=*/*this,
/*to_segments_begin=*/end_before_splice);
break;
} else {
// No more segments in the source, or both lists done.
break;
}
}

DCHECK_OK(ConsistencyStatus());
}

template<typename Frame>
Instant DiscreteTrajectory<Frame>::t_min() const {
return segments_->front().t_min();
11 changes: 8 additions & 3 deletions physics/discrete_trajectory_segment.hpp
Original file line number Diff line number Diff line change
@@ -143,9 +143,6 @@ class DiscreteTrajectorySegment : public Trajectory<Frame> {
void Prepend(Instant const& t,
DegreesOfFreedom<Frame> const& degrees_of_freedom);

absl::Status Append(Instant const& t,
DegreesOfFreedom<Frame> const& degrees_of_freedom);

// Removes all points with a time greater than or equal to |t| (1st overload)
// or starting at |begin| (2nd overload).
void ForgetAfter(Instant const& t);
@@ -156,6 +153,14 @@ class DiscreteTrajectorySegment : public Trajectory<Frame> {
void ForgetBefore(Instant const& t);
void ForgetBefore(typename Timeline::const_iterator end);

absl::Status Append(Instant const& t,
DegreesOfFreedom<Frame> const& degrees_of_freedom);

// Merges the points from the given |segment| into this object. The two
// segments must have nonoverlapping times. The downsampling state of the
// result is that of the latest segment (with the largest times).
void Merge(DiscreteTrajectorySegment<Frame> segment);

// Computes |number_of_dense_points_| based on the start of the dense
// timeline. Used for compatibility deserialization.
void SetStartOfDenseTimeline(Instant const& t);
75 changes: 51 additions & 24 deletions physics/discrete_trajectory_segment_body.hpp
Original file line number Diff line number Diff line change
@@ -328,30 +328,6 @@ void DiscreteTrajectorySegment<Frame>::Prepend(
timeline_.emplace_hint(timeline_.cbegin(), t, degrees_of_freedom);
}

template<typename Frame>
absl::Status DiscreteTrajectorySegment<Frame>::Append(
Instant const& t,
DegreesOfFreedom<Frame> const& degrees_of_freedom) {
if (!timeline_.empty() && timeline_.cbegin()->time == t) {
LOG(WARNING) << "Append at existing time " << t << ", time range = ["
<< timeline_.cbegin()->time << ", "
<< timeline_.crbegin()->time << "]";
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()->time;

if (downsampling_parameters_.has_value()) {
return DownsampleIfNeeded();
} else {
return absl::OkStatus();
}
}

template<typename Frame>
void DiscreteTrajectorySegment<Frame>::ForgetAfter(Instant const& t) {
ForgetAfter(timeline_.lower_bound(t));
@@ -386,6 +362,57 @@ void DiscreteTrajectorySegment<Frame>::ForgetBefore(
timeline_.erase(timeline_.cbegin(), end);
}

template<typename Frame>
absl::Status DiscreteTrajectorySegment<Frame>::Append(
Instant const& t,
DegreesOfFreedom<Frame> const& degrees_of_freedom) {
if (!timeline_.empty() && timeline_.cbegin()->time == t) {
LOG(WARNING) << "Append at existing time " << t << ", time range = ["
<< timeline_.cbegin()->time << ", "
<< timeline_.crbegin()->time << "]";
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()->time;

if (downsampling_parameters_.has_value()) {
return DownsampleIfNeeded();
} else {
return absl::OkStatus();
}
}

template<typename Frame>
void DiscreteTrajectorySegment<Frame>::Merge(
DiscreteTrajectorySegment<Frame> segment) {
if (segment.timeline_.empty()) {
return;
} else if (timeline_.empty()) {
downsampling_parameters_ = segment.downsampling_parameters_;
timeline_ = std::move(segment.timeline_);
number_of_dense_points_ = segment.number_of_dense_points_;
} else if (std::prev(timeline_.cend())->time <
segment.timeline_.cbegin()->time) {
downsampling_parameters_ = segment.downsampling_parameters_;
timeline_.merge(segment.timeline_);
number_of_dense_points_ = segment.number_of_dense_points_;
} else if (std::prev(segment.timeline_.cend())->time <
timeline_.cbegin()->time) {
timeline_.merge(segment.timeline_);
} else {
// TODO(phl): We might need to have to check <= above and to verify that the
// points match.
LOG(FATAL) << "Overlapping merge: [" << segment.timeline_.cbegin()->time
<< ", " << std::prev(segment.timeline_.cend())->time
<< "] into [" << timeline_.cbegin()->time << ", "
<< std::prev(timeline_.cend())->time << "]";
}
}

template<typename Frame>
void DiscreteTrajectorySegment<Frame>::SetStartOfDenseTimeline(
Instant const& t) {
49 changes: 49 additions & 0 deletions physics/discrete_trajectory_test.cpp
Original file line number Diff line number Diff line change
@@ -570,6 +570,55 @@ TEST_F(DiscreteTrajectoryTest, ForgetBefore) {
EXPECT_TRUE(trajectory.empty());
}

TEST_F(DiscreteTrajectoryTest, Merge) {
{
auto trajectory1 = MakeTrajectory();
auto trajectory2 = MakeTrajectory();

trajectory1.ForgetAfter(t0_ + 6 * Second);
trajectory2.ForgetBefore(t0_ + 6 * Second);

trajectory1.Merge(std::move(trajectory2));

EXPECT_EQ(3, trajectory1.segments().size());
auto sit = trajectory1.segments().begin();
EXPECT_EQ(5, sit->size());
EXPECT_EQ(t0_, sit->front().time);
EXPECT_EQ(t0_ + 4 * Second, sit->back().time);
++sit;
EXPECT_EQ(6, sit->size());
EXPECT_EQ(t0_ + 4 * Second, sit->front().time);
EXPECT_EQ(t0_ + 9 * Second, sit->back().time);
++sit;
EXPECT_EQ(6, sit->size());
EXPECT_EQ(t0_ + 9 * Second, sit->front().time);
EXPECT_EQ(t0_ + 14 * Second, sit->back().time);
}
{
auto trajectory1 = MakeTrajectory();
auto trajectory2 = MakeTrajectory();

trajectory1.ForgetAfter(t0_ + 6 * Second);
trajectory2.ForgetBefore(t0_ + 6 * Second);

trajectory2.Merge(std::move(trajectory1));

EXPECT_EQ(3, trajectory2.segments().size());
auto sit = trajectory2.segments().begin();
EXPECT_EQ(5, sit->size());
EXPECT_EQ(t0_, sit->front().time);
EXPECT_EQ(t0_ + 4 * Second, sit->back().time);
++sit;
EXPECT_EQ(6, sit->size());
EXPECT_EQ(t0_ + 4 * Second, sit->front().time);
EXPECT_EQ(t0_ + 9 * Second, sit->back().time);
++sit;
EXPECT_EQ(6, sit->size());
EXPECT_EQ(t0_ + 9 * Second, sit->front().time);
EXPECT_EQ(t0_ + 14 * Second, sit->back().time);
}
}

TEST_F(DiscreteTrajectoryTest, TMinTMaxEvaluate) {
auto const trajectory = MakeTrajectory();
EXPECT_EQ(t0_, trajectory.t_min());