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_QUERY_ITERATOR_HPP
43#define UFO_CONTAINER_TREE_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 <cstddef>
52#include <iterator>
53
54namespace ufo
55{
56template <class Tree, class Predicate = pred::Predicate<Tree>>
58{
59 private:
60 //
61 // Friends
62 //
63
64 template <class, class>
65 friend class TreeQueryIterator;
66
67 private:
68 static constexpr std::size_t const BF = Tree::branchingFactor();
69
70 using Node = typename Tree::Node;
71 using offset_type = typename Tree::offset_type;
72 using depth_type = typename Tree::depth_type;
73
75
76 public:
77 //
78 // Tags
79 //
80
81 using iterator_category = std::forward_iterator_tag;
82 using difference_type = std::ptrdiff_t;
83 using value_type = Node;
84 using reference = value_type const&;
85 using pointer = value_type const*;
86
87 constexpr TreeQueryIterator() = default;
88
89 TreeQueryIterator(Tree const* tree, Node node, Predicate const& pred, bool only_exists,
90 bool early_stopping)
91 : tree_(tree)
92 , pred_(pred)
93 , cur_(node)
94 , start_(tree->depth(node))
95 , nextNode(nextNodeFun(only_exists, early_stopping))
96 {
97 if (only_exists && !tree_->exists(cur_)) {
98 cur_ = {};
99 return;
100 }
101
102 Filter::init(pred_, *tree_);
103
104 if (!Filter::returnable(pred_, *tree_, cur_)) {
105 if (only_exists) {
106 cur_ = next<true, false>(*tree_, pred_, cur_, start_);
107 } else {
108 cur_ = next<false, false>(*tree_, pred_, cur_, start_);
109 }
110 }
111 }
112
113 TreeQueryIterator(TreeQueryIterator const&) = default;
114
115 template <class Predicate2>
117 : tree_(other.tree_)
118 , pred_(other.pred_)
119 , cur_(other.cur_)
120 , start_(other.start_)
121 , nextNode(other.nextNode)
122 {
123 }
124
125 TreeQueryIterator& operator++()
126 {
127 cur_ = nextNode(*tree_, pred_, cur_, start_);
128 return *this;
129 }
130
131 TreeQueryIterator operator++(int)
132 {
133 TreeQueryIterator tmp(*this);
134 ++*this;
135 return tmp;
136 }
137
138 reference operator*() const { return cur_; }
139
140 pointer operator->() const { return &cur_; }
141
142 template <class Predicate2>
143 bool operator==(TreeQueryIterator<Tree, Predicate2> const& other)
144 {
145 return cur_.code == other.cur_.code;
146 }
147
148 template <class Predicate2>
149 bool operator!=(TreeQueryIterator<Tree, Predicate2> const& other)
150 {
151 return !(*this == other);
152 }
153
154 private:
155 [[nodiscard]] static auto nextNodeFun(bool only_exists, bool early_stopping)
156 {
157 if (only_exists && early_stopping) {
158 return &TreeQueryIterator::next<true, true>;
159 } else if (only_exists && !early_stopping) {
160 return &TreeQueryIterator::next<true, false>;
161 } else if (!only_exists && early_stopping) {
162 return &TreeQueryIterator::next<false, true>;
163 } else {
164 return &TreeQueryIterator::next<false, false>;
165 }
166 }
167
168 template <bool OnlyExists>
169 [[nodiscard]] static bool traversable(Tree const& tree, Predicate const& pred,
170 Node node)
171 {
172 if constexpr (OnlyExists) {
173 return tree.isParent(node.index) && Filter::traversable(pred, tree, node);
174 } else {
175 return 0 < node.code.depth() && Filter::traversable(pred, tree, node);
176 }
177 }
178
179 template <bool OnlyExists>
180 [[nodiscard]] static Node firstborn(Tree const& tree, Node node)
181 {
182 node.code = node.code.firstborn();
183 if constexpr (OnlyExists) {
184 node.index = tree.child(node.index, 0);
185 } else if (tree.isParent(node.index)) {
186 node.index = tree.child(node.index, 0);
187 }
188 return node;
189 }
190
191 template <bool OnlyExists, bool EarlyStopping>
192 [[nodiscard]] static Node next(Tree const& tree, Predicate const& pred, Node node,
193 depth_type start)
194 {
195 if constexpr (!EarlyStopping) {
196 // Traverse down the branch
197
198 while (traversable<OnlyExists>(tree, pred, node)) {
199 node = firstborn<OnlyExists>(tree, node);
200
201 if (Filter::returnable(pred, tree, node)) {
202 return node;
203 }
204 }
205 }
206
207 while (true) {
208 while (BF - 1 <= node.code.offset()) {
209 node.code = node.code.parent();
210 }
211
212 if (start <= node.code.depth()) {
213 return Node{};
214 }
215
216 node.code = node.code.nextSibling();
217 node.index = tree.ancestor(node.index, node.code.depth());
218
219 if constexpr (OnlyExists) {
220 ++node.index.offset;
221 } else {
222 node.index.offset += node.code.depth() == tree.depth(node.index) ? 1 : 0;
223 }
224
225 if (Filter::returnable(pred, tree, node)) {
226 return node;
227 }
228
229 while (traversable<OnlyExists>(tree, pred, node)) {
230 node = firstborn<OnlyExists>(tree, node);
231
232 if (Filter::returnable(pred, tree, node)) {
233 return node;
234 }
235 }
236 }
237 }
238
239 private:
240 Tree const* tree_;
241
242 Predicate pred_{};
243
244 Node cur_{};
245 depth_type start_;
246
247 decltype(&TreeQueryIterator::next<true, true>) nextNode;
248};
249} // namespace ufo
250
251#endif // UFO_CONTAINER_TREE_QUERY_ITERATOR_HPP
Utilizing curiously recurring template pattern (CRTP)
Definition tree.hpp:104
bool isParent(NodeType node) const
Checks if the node is a parent (i.e., has children).
Definition tree.hpp:1308
depth_type depth() const
Returns the depth of the root node, i.e. numDepthLevels() - 1.
Definition tree.hpp:290
constexpr auto child(NodeType node, offset_type offset) const
Returns the i:th child of node.
Definition tree.hpp:1450
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