Skip to content

Commit ba4c3d6

Browse files
pleroypleroy
authored
Merge pull request #3145 from pleroy/Segment
DiscreteTrajectorySegment, part 1: iterators
2 parents 9324301 + b89257a commit ba4c3d6

8 files changed

+290
-32
lines changed

‎physics/discrete_trajectory2.hpp

+6
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
#pragma once
22

3+
#include <iterator>
34
#include <list>
45
#include <memory>
56
#include <vector>
@@ -34,6 +35,11 @@ using physics::DegreesOfFreedom;
3435
template<typename Frame>
3536
class DiscreteTrajectory2 : public Trajectory<Frame> {
3637
public:
38+
using key_type =
39+
typename internal_discrete_trajectory_types::Timeline<Frame>::key_type;
40+
using value_type =
41+
typename internal_discrete_trajectory_types::Timeline<Frame>::value_type;
42+
3743
using iterator = DiscreteTrajectoryIterator<Frame>;
3844
using reverse_iterator = std::reverse_iterator<iterator>;
3945
using SegmentIterator = DiscreteTrajectorySegmentIterator<Frame>;

‎physics/discrete_trajectory_iterator.hpp

+14-8
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
#pragma once
22

3+
#include <cstdint>
34
#include <optional>
45

56
#include "absl/container/btree_map.h"
@@ -11,8 +12,9 @@
1112
namespace principia {
1213
namespace physics {
1314

14-
template<typename Frame>
15-
class DiscreteTrajectory;
15+
FORWARD_DECLARE_FROM(discrete_trajectory_segment,
16+
TEMPLATE(typename Frame) class,
17+
DiscreteTrajectorySegment);
1618

1719
namespace internal_discrete_trajectory_iterator {
1820

@@ -22,19 +24,21 @@ using physics::DegreesOfFreedom;
2224
template<typename Frame>
2325
class DiscreteTrajectoryIterator {
2426
public:
27+
using difference_type = std::int64_t;
28+
using value_type =
29+
typename internal_discrete_trajectory_types::Timeline<Frame>::value_type;
30+
using pointer = value_type const*;
31+
using reference = value_type const&;
32+
2533
DiscreteTrajectoryIterator() = default;
2634

2735
DiscreteTrajectoryIterator& operator++();
2836
DiscreteTrajectoryIterator& operator--();
2937
DiscreteTrajectoryIterator operator++(int);
3038
DiscreteTrajectoryIterator operator--(int);
3139

32-
typename
33-
internal_discrete_trajectory_types::Timeline<Frame>::value_type const&
34-
operator*() const;
35-
typename
36-
internal_discrete_trajectory_types::Timeline<Frame>::value_type const*
37-
operator->() const;
40+
reference operator*() const;
41+
pointer operator->() const;
3842

3943
bool operator==(DiscreteTrajectoryIterator const& other) const;
4044
bool operator!=(DiscreteTrajectoryIterator const& other) const;
@@ -68,6 +72,8 @@ class DiscreteTrajectoryIterator {
6872
// times.
6973
std::optional<Instant> previous_time_;
7074

75+
template<typename F>
76+
friend class DiscreteTrajectorySegment;
7177
friend class DiscreteTrajectoryIteratorTest;
7278
};
7379

‎physics/discrete_trajectory_segment.hpp

+26-13
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,11 @@
11
#pragma once
22

33
#include <cstdint>
4+
#include <iterator>
45

56
#include "absl/container/btree_map.h"
67
#include "absl/container/btree_set.h"
8+
#include "absl/status/status.h"
79
#include "geometry/named_quantities.hpp"
810
#include "physics/degrees_of_freedom.hpp"
911
#include "physics/discrete_trajectory_iterator.hpp"
@@ -14,6 +16,7 @@ namespace principia {
1416
namespace physics {
1517

1618
class DiscreteTrajectoryIteratorTest;
19+
class DiscreteTrajectorySegmentTest;
1720

1821
namespace internal_discrete_trajectory_segment {
1922

@@ -22,8 +25,20 @@ using physics::DegreesOfFreedom;
2225

2326
template<typename Frame>
2427
class DiscreteTrajectorySegment {
28+
using Timeline = internal_discrete_trajectory_types::Timeline<Frame>;
29+
2530
public:
31+
using key_type = typename Timeline::key_type;
32+
using value_type = typename Timeline::value_type;
33+
34+
using iterator = DiscreteTrajectoryIterator<Frame>;
35+
using reverse_iterator = std::reverse_iterator<iterator>;
36+
37+
// TODO(phl): Decide which constructors should be public.
2638
DiscreteTrajectorySegment() = default;
39+
explicit DiscreteTrajectorySegment(
40+
DiscreteTrajectorySegmentIterator<Frame> self);
41+
2742
virtual ~DiscreteTrajectorySegment() = default;
2843

2944
// Moveable.
@@ -33,27 +48,24 @@ class DiscreteTrajectorySegment {
3348
DiscreteTrajectorySegment& operator=(const DiscreteTrajectorySegment&) =
3449
delete;
3550

36-
DiscreteTrajectoryIterator<Frame> begin() const;
37-
DiscreteTrajectoryIterator<Frame> end() const;
51+
iterator begin() const;
52+
iterator end() const;
3853

39-
DiscreteTrajectoryIterator<Frame> rbegin() const;
40-
DiscreteTrajectoryIterator<Frame> rend() const;
54+
reverse_iterator rbegin() const;
55+
reverse_iterator rend() const;
4156

42-
DiscreteTrajectoryIterator<Frame> find(Instant const& t) const;
57+
iterator find(Instant const& t) const;
4358

44-
DiscreteTrajectoryIterator<Frame> lower_bound(Instant const& t) const;
45-
DiscreteTrajectoryIterator<Frame> upper_bound(Instant const& t) const;
59+
iterator lower_bound(Instant const& t) const;
60+
iterator upper_bound(Instant const& t) const;
4661

62+
// TODO(phl): We probably don't want empty segments.
4763
bool empty() const;
4864
virtual std::int64_t size() const;
4965

50-
protected:
51-
// For mocking.
52-
using Timeline = internal_discrete_trajectory_types::Timeline<Frame>;
53-
5466
private:
55-
void Append(Instant const& t,
56-
DegreesOfFreedom<Frame> const& degrees_of_freedom);
67+
absl::Status Append(Instant const& t,
68+
DegreesOfFreedom<Frame> const& degrees_of_freedom);
5769

5870
void ForgetAfter(Instant const& t);
5971
void ForgetAfter(typename Timeline::const_iterator begin);
@@ -73,6 +85,7 @@ class DiscreteTrajectorySegment {
7385
friend class internal_discrete_trajectory_iterator::
7486
DiscreteTrajectoryIterator;
7587
friend class DiscreteTrajectoryIteratorTest;
88+
friend class DiscreteTrajectorySegmentTest;
7689
};
7790

7891
} // namespace internal_discrete_trajectory_segment

‎physics/discrete_trajectory_segment_body.hpp

+120-8
Original file line numberDiff line numberDiff line change
@@ -1,27 +1,139 @@
11
#pragma once
22

3-
#include "glog/logging.h"
43
#include "physics/discrete_trajectory_segment.hpp"
54

5+
#include "glog/logging.h"
6+
67
namespace principia {
78
namespace physics {
89
namespace internal_discrete_trajectory_segment {
910

1011
template<typename Frame>
11-
typename DiscreteTrajectorySegment<Frame>::Timeline::const_iterator
12-
DiscreteTrajectorySegment<Frame>::timeline_begin() const {
13-
LOG(FATAL) << "NYI";
12+
DiscreteTrajectorySegment<Frame>::DiscreteTrajectorySegment(
13+
DiscreteTrajectorySegmentIterator<Frame> const self)
14+
: self_(self) {}
15+
16+
template<typename Frame>
17+
typename DiscreteTrajectorySegment<Frame>::iterator
18+
DiscreteTrajectorySegment<Frame>::begin() const {
19+
return iterator(self_, timeline_.begin());
1420
}
1521

1622
template<typename Frame>
17-
typename DiscreteTrajectorySegment<Frame>::Timeline::const_iterator
18-
DiscreteTrajectorySegment<Frame>::timeline_end() const {
19-
LOG(FATAL) << "NYI";
23+
typename DiscreteTrajectorySegment<Frame>::iterator
24+
DiscreteTrajectorySegment<Frame>::end() const {
25+
// TODO(phl): We probably don't want empty segments.
26+
if (timeline_.empty()) {
27+
return iterator(self_, timeline_.begin());
28+
} else {
29+
// The decrement/increment ensures that we normalize the end iterator to the
30+
// next segment or to the end of the trajectory.
31+
return ++iterator(self_, --timeline_.end());
32+
}
33+
}
34+
35+
template<typename Frame>
36+
typename DiscreteTrajectorySegment<Frame>::reverse_iterator
37+
DiscreteTrajectorySegment<Frame>::rbegin() const {
38+
return reverse_iterator(end());
39+
}
40+
41+
template<typename Frame>
42+
typename DiscreteTrajectorySegment<Frame>::reverse_iterator
43+
DiscreteTrajectorySegment<Frame>::rend() const {
44+
return reverse_iterator(begin());
45+
}
46+
47+
template<typename Frame>
48+
typename DiscreteTrajectorySegment<Frame>::iterator
49+
DiscreteTrajectorySegment<Frame>::find(Instant const& t) const {
50+
auto const it = timeline_.find(t);
51+
if (it == timeline_.end()) {
52+
return end();
53+
} else {
54+
return iterator(self_, it);
55+
}
56+
}
57+
58+
template<typename Frame>
59+
typename DiscreteTrajectorySegment<Frame>::iterator
60+
DiscreteTrajectorySegment<Frame>::lower_bound(Instant const& t) const {
61+
auto const it = timeline_.lower_bound(t);
62+
if (it == timeline_.end()) {
63+
return end();
64+
} else {
65+
return iterator(self_, it);
66+
}
67+
}
68+
69+
template<typename Frame>
70+
typename DiscreteTrajectorySegment<Frame>::iterator
71+
DiscreteTrajectorySegment<Frame>::upper_bound(Instant const& t) const {
72+
auto const it = timeline_.upper_bound(t);
73+
if (it == timeline_.end()) {
74+
return end();
75+
} else {
76+
return iterator(self_, it);
77+
}
78+
}
79+
80+
template<typename Frame>
81+
bool DiscreteTrajectorySegment<Frame>::empty() const {
82+
return timeline_.empty();
2083
}
2184

2285
template<typename Frame>
2386
std::int64_t DiscreteTrajectorySegment<Frame>::size() const {
24-
LOG(FATAL) << "NYI";
87+
// NOTE(phl): This assumes that there are no repeated times *within* a
88+
// segment. This is enforced by Append.
89+
return timeline_.size();
90+
}
91+
92+
template<typename Frame>
93+
absl::Status DiscreteTrajectorySegment<Frame>::Append(
94+
Instant const& t,
95+
DegreesOfFreedom<Frame> const& degrees_of_freedom) {
96+
if (!timeline_.empty() && timeline_.cbegin()->first == t) {
97+
LOG(WARNING) << "Append at existing time " << t << ", time range = ["
98+
<< timeline_.cbegin()->first << ", "
99+
<< timeline_.crbegin()->first << "]";
100+
return absl::OkStatus();
101+
}
102+
auto it = timeline_.emplace_hint(timeline_.cend(),
103+
t,
104+
degrees_of_freedom);
105+
CHECK(++it == timeline_.end())
106+
<< "Append out of order at " << t << ", last time is "
107+
<< timeline_.crbegin()->first;
108+
109+
// TODO(phl): Downsampling.
110+
return absl::OkStatus();
111+
}
112+
113+
template<typename Frame>
114+
void DiscreteTrajectorySegment<Frame>::ForgetAfter(Instant const& t) {}
115+
116+
template<typename Frame>
117+
void DiscreteTrajectorySegment<Frame>::ForgetAfter(
118+
typename Timeline::const_iterator const begin) {}
119+
120+
template<typename Frame>
121+
void DiscreteTrajectorySegment<Frame>::ForgetBefore(Instant const& t) {}
122+
123+
template<typename Frame>
124+
void DiscreteTrajectorySegment<Frame>::ForgetBefore(
125+
typename Timeline::const_iterator const end) {}
126+
127+
template<typename Frame>
128+
typename DiscreteTrajectorySegment<Frame>::Timeline::const_iterator
129+
DiscreteTrajectorySegment<Frame>::timeline_begin() const {
130+
return timeline_.cbegin();
131+
}
132+
133+
template<typename Frame>
134+
typename DiscreteTrajectorySegment<Frame>::Timeline::const_iterator
135+
DiscreteTrajectorySegment<Frame>::timeline_end() const {
136+
return timeline_.cend();
25137
}
26138

27139
} // namespace internal_discrete_trajectory_segment

‎physics/discrete_trajectory_segment_iterator.hpp

+10-3
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@ FORWARD_DECLARE_FROM(discrete_trajectory_segment,
1717

1818
class DiscreteTrajectoryIteratorTest;
1919
class DiscreteTrajectorySegmentIteratorTest;
20+
class DiscreteTrajectorySegmentTest;
2021

2122
namespace internal_discrete_trajectory_segment_iterator {
2223

@@ -25,15 +26,20 @@ using base::not_null;
2526
template<typename Frame>
2627
class DiscreteTrajectorySegmentIterator {
2728
public:
29+
using difference_type = std::int64_t;
30+
using value_type = DiscreteTrajectorySegment<Frame>;
31+
using pointer = value_type const*;
32+
using reference = value_type const&;
33+
2834
DiscreteTrajectorySegmentIterator() = default;
2935

3036
DiscreteTrajectorySegmentIterator& operator++();
3137
DiscreteTrajectorySegmentIterator& operator--();
3238
DiscreteTrajectorySegmentIterator operator++(int);
3339
DiscreteTrajectorySegmentIterator operator--(int);
3440

35-
DiscreteTrajectorySegment<Frame> const& operator*() const;
36-
DiscreteTrajectorySegment<Frame> const* operator->() const;
41+
reference operator*() const;
42+
pointer operator->() const;
3743

3844
bool operator==(DiscreteTrajectorySegmentIterator const& other) const;
3945
bool operator!=(DiscreteTrajectorySegmentIterator const& other) const;
@@ -51,10 +57,11 @@ class DiscreteTrajectorySegmentIterator {
5157
Segments const* segments_ = nullptr;
5258
typename Segments::const_iterator iterator_;
5359

54-
template<typename Frame>
60+
template<typename F>
5561
friend class DiscreteTrajectoryIterator;
5662
friend class DiscreteTrajectoryIteratorTest;
5763
friend class DiscreteTrajectorySegmentIteratorTest;
64+
friend class DiscreteTrajectorySegmentTest;
5865
};
5966

6067
} // namespace internal_discrete_trajectory_segment_iterator

0 commit comments

Comments
 (0)