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_MAP_ITERATOR_HPP
43#define UFO_CONTAINER_DETAIL_TREE_MAP_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, class T>
58class TreeMap;
59
60template <bool Const, std::size_t Dim, class T>
62{
63 private:
64 //
65 // Friends
66 //
67
68 friend class TreeMapIterator<!Const, Dim, T>;
69
70 friend class TreeMap<Dim, T>;
71
72 private:
73 static constexpr std::size_t const BF = TreeMap<Dim, T>::branchingFactor();
74
75 using RawIterator =
76 std::conditional_t<Const, typename TreeMap<Dim, T>::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 TreeMapIterator() = default;
91
92 TreeMapIterator(TreeMapIterator const&) = default;
93
94 // From non-const to const
95 template <bool Const2, std::enable_if_t<Const && !Const2, bool> = true>
97 : tm_(other.tm_)
98 , root_(other.root_)
99 , cur_(other.cur_)
100 , it_(other.it_)
101 , last_(other.last_)
102 {
103 }
104
105 TreeMapIterator& operator++()
106 {
107 if (++it_ == last_) {
108 nextNode();
109 }
110 return *this;
111 }
112
113 TreeMapIterator operator++(int)
114 {
115 TreeMapIterator 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==(TreeMapIterator<Const2, Dim, T> const& other)
126 {
127 return it_ == other.it_;
128 }
129
130 template <bool Const2>
131 bool operator!=(TreeMapIterator<Const2, Dim, T> const& other)
132 {
133 return !(*this == other);
134 }
135
136 private:
137 [[nodiscard]] bool returnable(TreeIndex node) const
138 {
139 return tm_->isPureLeaf(node) && !tm_->empty(node);
140 }
141
142 [[nodiscard]] bool traversable(TreeIndex node) const { return tm_->isParent(node); }
143
144 void nextNode()
145 {
146 while (root_ != cur_) {
147 if (BF - 1 == cur_.offset) {
148 cur_ = tm_->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_ = tm_->values(cur_).begin();
174 last_ = tm_->values(cur_).end();
175 return true;
176 } else if (traversable(cur_)) {
177 cur_ = tm_->child(cur_, 0);
178 } else {
179 break;
180 }
181 }
182 return false;
183 }
184
185 [[nodiscard]] RawIterator iterator() { return it_; }
186
187 private:
188 TreeMapIterator(TreeMap<Dim, T>* tm, TreeIndex node) : tm_(tm), 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 : tm_(other.tm_), root_(other.root_), cur_(other.cur_)
201 {
202 // Remove const from other.it_ and other.last_
203 it_ = tm_->values(cur_).erase(other.it_, other.it_);
204 last_ = tm_->values(cur_).erase(other.last_, other.last_);
205 }
206
207 private:
208 TreeMap<Dim, T>* tm_ = 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_MAP_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