UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
iterator.hpp
1
42#ifndef UFO_CONTAINER_DETAIL_TREE_SET_ITERATOR_HPP
43#define UFO_CONTAINER_DETAIL_TREE_SET_ITERATOR_HPP
44
45// UFO
46#include <ufo/container/tree/index.hpp>
47
48// STL
49#include <cassert>
50#include <cstddef>
51#include <iterator>
52#include <type_traits>
53
54namespace ufo
55{
56// Forward declare
57template <std::size_t Dim>
58class TreeSet;
59
60template <bool Const, std::size_t Dim>
62{
63 private:
64 //
65 // Friends
66 //
67
68 friend class TreeSetIterator<!Const, Dim>;
69
70 friend class TreeSet<Dim>;
71
72 private:
73 static constexpr std::size_t const BF = TreeSet<Dim>::branchingFactor();
74
75 using RawIterator =
76 std::conditional_t<Const, typename TreeSet<Dim>::container_type::const_iterator,
78
79 public:
80 //
81 // Tags
82 //
83
84 using iterator_category = std::forward_iterator_tag;
85 using difference_type = std::ptrdiff_t;
86 using value_type = typename std::iterator_traits<RawIterator>::value_type;
87 using reference = typename std::iterator_traits<RawIterator>::reference;
88 using pointer = typename std::iterator_traits<RawIterator>::pointer;
89
90 TreeSetIterator() = default;
91
92 TreeSetIterator(TreeSetIterator const&) = default;
93
94 // From non-const to const
95 template <bool Const2, std::enable_if_t<Const && !Const2, bool> = true>
97 : ts_(other.ts_)
98 , root_(other.root_)
99 , cur_(other.cur_)
100 , it_(other.it_)
101 , last_(other.last_)
102 {
103 }
104
105 TreeSetIterator& operator++()
106 {
107 if (++it_ == last_) {
108 nextNode();
109 }
110 return *this;
111 }
112
113 TreeSetIterator operator++(int)
114 {
115 TreeSetIterator tmp(*this);
116 ++*this;
117 return tmp;
118 }
119
120 reference operator*() const { return *it_; }
121
122 pointer operator->() const { return &*it_; }
123
124 template <bool Const2>
125 bool operator==(TreeSetIterator<Const2, Dim> const& other)
126 {
127 return it_ == other.it_;
128 }
129
130 template <bool Const2>
131 bool operator!=(TreeSetIterator<Const2, Dim> const& other)
132 {
133 return !(*this == other);
134 }
135
136 private:
137 [[nodiscard]] bool returnable(TreeIndex node) const
138 {
139 return ts_->isPureLeaf(node) && !ts_->empty(node);
140 }
141
142 [[nodiscard]] bool traversable(TreeIndex node) const { return ts_->isParent(node); }
143
144 void nextNode()
145 {
146 while (root_ != cur_) {
147 if (BF - 1 == cur_.offset) {
148 cur_ = ts_->parent(cur_);
149 continue;
150 }
151
152 ++cur_.offset;
153
154 if (nextNodeDownwards()) {
155 return;
156 }
157 }
158
159 // We have visited all nodes
160 it_ = {};
161 last_ = {};
162 }
163
169 bool nextNodeDownwards()
170 {
171 while (true) {
172 if (returnable(cur_)) {
173 it_ = ts_->values(cur_).begin();
174 last_ = ts_->values(cur_).end();
175 return true;
176 } else if (traversable(cur_)) {
177 cur_ = ts_->child(cur_, 0);
178 } else {
179 break;
180 }
181 }
182 return false;
183 }
184
185 [[nodiscard]] RawIterator iterator() { return it_; }
186
187 private:
188 TreeSetIterator(TreeSet<Dim>* ts, TreeIndex node) : ts_(ts), root_(node), cur_(node)
189 {
190 if (nextNodeDownwards()) {
191 return;
192 }
193
194 nextNode();
195 }
196
197 // From const to non-const
198 template <bool Const2, std::enable_if_t<!Const && Const2, bool> = true>
200 : ts_(other.ts_), root_(other.root_), cur_(other.cur_)
201 {
202 // Remove const from other.it_ and other.last_
203 it_ = ts_->values(cur_).erase(other.it_, other.it_);
204 last_ = ts_->values(cur_).erase(other.last_, other.last_);
205 }
206
207 private:
208 TreeSet<Dim>* ts_ = nullptr;
209
210 TreeIndex root_{};
211 TreeIndex cur_{};
212
213 RawIterator it_{};
214 RawIterator last_{};
215};
216} // namespace ufo
217
218#endif // UFO_CONTAINER_DETAIL_TREE_SET_ITERATOR_HPP
All vision-related classes and functions.
Definition cloud.hpp:49