UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
query_iterator.hpp
1
42#ifndef UFO_CONTAINER_TREE_MAP_QUERY_ITERATOR_HPP
43#define UFO_CONTAINER_TREE_MAP_QUERY_ITERATOR_HPP
44
45// UFO
46#include <ufo/container/tree/index.hpp>
47#include <ufo/container/tree/predicate/filter.hpp>
48#include <ufo/container/tree/predicate/predicate.hpp>
49
50// STL
51#include <cassert>
52#include <cstddef>
53#include <iterator>
54
55namespace ufo
56{
57// Forward declare
58template <std::size_t Dim, class T>
59class TreeMap;
60
61template <bool Const, std::size_t Dim, class T,
62 class Predicate = pred::Predicate<TreeMap<Dim, T>>>
64{
65 private:
66 //
67 // Friends
68 //
69
70 template <bool, std::size_t, class, class>
71 friend class TreeMapQueryIterator;
72
73 friend class TreeMap<Dim, T>;
74
75 private:
76 static constexpr std::size_t const BF = TreeMap<Dim, T>::branchingFactor();
77
78 using RawIterator =
79 std::conditional_t<Const, typename TreeMap<Dim, T>::container_type::const_iterator,
81
83
84 public:
85 //
86 // Tags
87 //
88
89 using iterator_category = std::forward_iterator_tag;
90 using difference_type = std::ptrdiff_t;
91 using value_type = typename std::iterator_traits<RawIterator>::value_type;
92 using reference = typename std::iterator_traits<RawIterator>::reference;
93 using pointer = typename std::iterator_traits<RawIterator>::pointer;
94
95 TreeMapQueryIterator() = default;
96
98
99 // From non-const to const or change of predicate type
100 template <
101 bool Const2, class Predicate2,
102 std::enable_if_t<(Const && !Const2) ||
103 (Const == Const2 && !std::is_same_v<Predicate, Predicate2>),
104 bool> = true>
106 : tm_(other.tm_)
107 , pred_(other.pred_)
108 , root_(other.root_)
109 , cur_(other.cur_)
110 , it_(other.it_)
111 , last_(other.last_)
112 {
113 }
114
115 TreeMapQueryIterator& operator++()
116 {
117 ++it_;
118 while (!nextValue()) {
119 if (!nextNode()) {
120 break;
121 }
122 }
123 return *this;
124 }
125
126 TreeMapQueryIterator operator++(int)
127 {
128 TreeMapQueryIterator tmp(*this);
129 ++*this;
130 return tmp;
131 }
132
133 reference operator*() const { return *it_; }
134
135 pointer operator->() const { return &*it_; }
136
137 template <bool Const2, class Predicate2>
138 bool operator==(TreeMapQueryIterator<Const2, Dim, T, Predicate2> const& other)
139 {
140 return it_ == other.it_;
141 }
142
143 template <bool Const2, class Predicate2>
144 bool operator!=(TreeMapQueryIterator<Const2, Dim, T, Predicate2> const& other)
145 {
146 return !(*this == other);
147 }
148
149 private:
150 [[nodiscard]] bool returnable(value_type const& value) const
151 {
152 return Filter::returnable(pred_, value);
153 }
154
155 [[nodiscard]] bool returnable(TreeIndex node) const
156 {
157 return tm_->isPureLeaf(node) && !tm_->empty(node);
158 // FIXME: && pred::Filter<Predicate>::returnable(pred_, *tm_, node);
159 // FIXME: && pred::Filter<Predicate>::traversable(pred_, *tm_, node);
160 }
161
162 [[nodiscard]] bool traversable(TreeIndex node) const
163 {
164 return tm_->isParent(node) && Filter::traversable(pred_, *tm_, tm_->node(node));
165 }
166
172 bool nextValue()
173 {
174 it_ = std::find_if(it_, last_, [this](auto const& v) { return returnable(v); });
175 return it_ != last_;
176 }
177
183 bool nextNode()
184 {
185 while (root_ != cur_) {
186 if (BF - 1 == cur_.offset) {
187 cur_ = tm_->parent(cur_);
188 continue;
189 }
190
191 ++cur_.offset;
192
193 if (nextNodeDownwards()) {
194 return true;
195 }
196 }
197
198 // We have visited all nodes
199 it_ = {};
200 last_ = {};
201 return false;
202 }
203
209 bool nextNodeDownwards()
210 {
211 while (true) {
212 if (returnable(cur_)) {
213 it_ = tm_->values(cur_).begin();
214 last_ = tm_->values(cur_).end();
215 return true;
216 } else if (traversable(cur_)) {
217 cur_ = tm_->child(cur_, 0);
218 } else {
219 break;
220 }
221 }
222 return false;
223 }
224
225 [[nodiscard]] RawIterator iterator() { return it_; }
226
227 private:
228 TreeMapQueryIterator(TreeMap<Dim, T>* tm, TreeIndex node, Predicate const& pred)
229 : tm_(tm), pred_(pred), root_(node), cur_(node)
230 {
231 Filter::init(pred_, *tm_);
232
233 nextNodeDownwards();
234 while (!nextValue()) {
235 if (!nextNode()) {
236 break;
237 }
238 }
239 }
240
241 // From const to non-const
242 template <bool Const2, class Predicate2,
243 std::enable_if_t<!Const && Const2, bool> = true>
245 : tm_(other.tm_), pred_(other.pred_), root_(other.root_), cur_(other.cur_)
246 {
247 // Remove const from other.it_ and other.last_
248 it_ = tm_->values(cur_).erase(other.it_, other.it_);
249 last_ = tm_->values(cur_).erase(other.last_, other.last_);
250 }
251
252 private:
253 TreeMap<Dim, T>* tm_ = nullptr;
254
255 Predicate pred_{};
256
257 TreeIndex root_{};
258 TreeIndex cur_{};
259
260 RawIterator it_{};
261 RawIterator last_{};
262};
263} // namespace ufo
264
265#endif // UFO_CONTAINER_TREE_MAP_QUERY_ITERATOR_HPP
static constexpr offset_type branchingFactor() noexcept
Returns the branching factor of the tree (i.e., 2 = binary tree, 4 = quadtree, 8 = octree,...
Definition tree.hpp:203
All vision-related classes and functions.
Definition cloud.hpp:49