UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
map.hpp
1
42#ifndef UFO_MAP_DISTANCE_MAP_HPP
43#define UFO_MAP_DISTANCE_MAP_HPP
44
45// UFO
46#include <ufo/container/tree/container.hpp>
47#include <ufo/container/tree/tree.hpp>
48#include <ufo/geometry/distance.hpp>
49#include <ufo/geometry/line_segment.hpp>
50#include <ufo/map/distance/block.hpp>
51#include <ufo/map/distance/info.hpp>
52#include <ufo/map/distance/interpolate.hpp>
53#include <ufo/map/type.hpp>
54
55// STL
56#include <algorithm>
57#include <cassert>
58#include <cmath>
59#include <functional>
60#include <limits>
61#include <type_traits>
62#include <utility>
63
64namespace ufo
65{
66template <class Derived, class Tree, bool WithBounds = true>
67class DistanceMap
68{
69 template <class Derived2, class Tree2, bool WithBounds2>
70 friend class DistanceMap;
71
72 static constexpr auto const BF = Tree::branchingFactor();
73 static constexpr auto const Dim = Tree::dimensions();
74
75 using Block = DistanceBlock<Dim, BF, WithBounds>;
76
77 public:
78 /**************************************************************************************
79 | |
80 | Tags |
81 | |
82 **************************************************************************************/
83
84 static constexpr MapType const Type = MapType::DISTANCE;
85
86 // Container
87 using Index = typename Tree::Index;
88 using Node = typename Tree::Node;
89 using Code = typename Tree::Code;
90 using Key = typename Tree::Key;
91 using Point = typename Tree::Point;
92 using Coord = typename Tree::Coord;
93 using coord_t = typename Tree::coord_t;
94 using depth_t = typename Tree::depth_t;
95 using offset_t = typename Tree::offset_t;
96 using length_t = typename Tree::length_t;
97 using pos_t = typename Tree::pos_t;
98
99 using DistanceBounds = typename Block::Bounds;
100
101 /**************************************************************************************
102 | |
103 | Access |
104 | |
105 **************************************************************************************/
106
107 template <class Geometry>
108 [[nodiscard]] bool intersects(Index node, Geometry const& query) const
109 {
110 // TODO: Implement
111 return false;
112 }
113
114 template <class Geometry>
115 [[nodiscard]] float distanceSquared(Index node, Geometry const& query) const
116 {
117 if (!distanceContainsSurface(node)) {
118 return -1.0f;
119 }
120
121 if (intersects(node, query)) {
122 return 0.0f;
123 }
124
125 if constexpr (2 == Dim) {
126 constexpr std::array<std::array<std::uint8_t, 25>, ipow(2, 8)> const lut{
127 std::array<std::uint8_t, 25>{2, 4, 4},
128 std::array<std::uint8_t, 25>{4, 0, 9, 4, 9},
129 std::array<std::uint8_t, 25>{2, 1, 4},
130 std::array<std::uint8_t, 25>{8, 0, 1, 0, 9, 1, 4, 4, 9},
131 std::array<std::uint8_t, 25>{4, 2, 10, 4, 10},
132 std::array<std::uint8_t, 25>{8, 0, 9, 2, 10, 4, 9, 4, 10},
133 std::array<std::uint8_t, 25>{8, 1, 2, 1, 4, 2, 10, 4, 10},
134 std::array<std::uint8_t, 25>{12, 0, 1, 0, 9, 1, 2, 2, 10, 4, 9, 4, 10},
135 std::array<std::uint8_t, 25>{2, 3, 4},
136 std::array<std::uint8_t, 25>{8, 0, 3, 0, 9, 3, 4, 4, 9},
137 std::array<std::uint8_t, 25>{8, 1, 4, 1, 9, 3, 4, 3, 9},
138 std::array<std::uint8_t, 25>{8, 0, 1, 0, 3, 1, 4, 3, 4},
139 std::array<std::uint8_t, 25>{6, 2, 10, 3, 4, 4, 10},
140 std::array<std::uint8_t, 25>{12, 0, 3, 0, 9, 2, 10, 3, 4, 4, 9, 4, 10},
141 std::array<std::uint8_t, 25>{12, 1, 2, 1, 9, 2, 10, 3, 4, 3, 9, 4, 10},
142 std::array<std::uint8_t, 25>{12, 0, 1, 0, 3, 1, 2, 2, 10, 3, 4, 4, 10},
143 std::array<std::uint8_t, 25>{2, 4, 5},
144 std::array<std::uint8_t, 25>{6, 0, 9, 4, 5, 4, 9},
145 std::array<std::uint8_t, 25>{8, 1, 4, 1, 10, 4, 5, 5, 10},
146 std::array<std::uint8_t, 25>{12, 0, 1, 0, 9, 1, 10, 4, 5, 4, 9, 5, 10},
147 std::array<std::uint8_t, 25>{8, 2, 5, 2, 10, 4, 5, 4, 10},
148 std::array<std::uint8_t, 25>{12, 0, 9, 2, 5, 2, 10, 4, 5, 4, 9, 4, 10},
149 std::array<std::uint8_t, 25>{8, 1, 2, 1, 4, 2, 5, 4, 5},
150 std::array<std::uint8_t, 25>{12, 0, 1, 0, 9, 1, 2, 2, 5, 4, 5, 4, 9},
151 std::array<std::uint8_t, 25>{4, 3, 4, 4, 5},
152 std::array<std::uint8_t, 25>{10, 0, 3, 0, 9, 3, 4, 4, 5, 4, 9},
153 std::array<std::uint8_t, 25>{12, 1, 9, 1, 10, 3, 4, 3, 9, 4, 5, 5, 10},
154 std::array<std::uint8_t, 25>{12, 0, 1, 0, 3, 1, 10, 3, 4, 4, 5, 5, 10},
155 std::array<std::uint8_t, 25>{10, 2, 5, 2, 10, 3, 4, 4, 5, 4, 10},
156 std::array<std::uint8_t, 25>{16, 0, 3, 0, 9, 2, 5, 2, 10, 3, 4, 4, 5, 4, 9, 4,
157 10},
158 std::array<std::uint8_t, 25>{12, 1, 2, 1, 9, 2, 5, 3, 4, 3, 9, 4, 5},
159 std::array<std::uint8_t, 25>{12, 0, 1, 0, 3, 1, 2, 2, 5, 3, 4, 4, 5},
160 std::array<std::uint8_t, 25>{4, 4, 11, 6, 11},
161 std::array<std::uint8_t, 25>{8, 0, 9, 4, 9, 4, 11, 6, 11},
162 std::array<std::uint8_t, 25>{6, 1, 4, 4, 11, 6, 11},
163 std::array<std::uint8_t, 25>{12, 0, 1, 0, 9, 1, 4, 4, 9, 4, 11, 6, 11},
164 std::array<std::uint8_t, 25>{8, 2, 10, 4, 10, 4, 11, 6, 11},
165 std::array<std::uint8_t, 25>{12, 0, 9, 2, 10, 4, 9, 4, 10, 4, 11, 6, 11},
166 std::array<std::uint8_t, 25>{12, 1, 2, 1, 4, 2, 10, 4, 10, 4, 11, 6, 11},
167 std::array<std::uint8_t, 25>{16, 0, 1, 0, 9, 1, 2, 2, 10, 4, 9, 4, 10, 4, 11, 6,
168 11},
169 std::array<std::uint8_t, 25>{8, 3, 4, 3, 6, 4, 11, 6, 11},
170 std::array<std::uint8_t, 25>{12, 0, 3, 0, 9, 3, 6, 4, 9, 4, 11, 6, 11},
171 std::array<std::uint8_t, 25>{12, 1, 4, 1, 9, 3, 6, 3, 9, 4, 11, 6, 11},
172 std::array<std::uint8_t, 25>{12, 0, 1, 0, 3, 1, 4, 3, 6, 4, 11, 6, 11},
173 std::array<std::uint8_t, 25>{12, 2, 10, 3, 4, 3, 6, 4, 10, 4, 11, 6, 11},
174 std::array<std::uint8_t, 25>{16, 0, 3, 0, 9, 2, 10, 3, 6, 4, 9, 4, 10, 4, 11, 6,
175 11},
176 std::array<std::uint8_t, 25>{16, 1, 2, 1, 9, 2, 10, 3, 6, 3, 9, 4, 10, 4, 11, 6,
177 11},
178 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 2, 2, 10, 3, 6, 4, 10, 4, 11, 6,
179 11},
180 std::array<std::uint8_t, 25>{6, 4, 5, 4, 11, 6, 11},
181 std::array<std::uint8_t, 25>{10, 0, 9, 4, 5, 4, 9, 4, 11, 6, 11},
182 std::array<std::uint8_t, 25>{12, 1, 4, 1, 10, 4, 5, 4, 11, 5, 10, 6, 11},
183 std::array<std::uint8_t, 25>{16, 0, 1, 0, 9, 1, 10, 4, 5, 4, 9, 4, 11, 5, 10, 6,
184 11},
185 std::array<std::uint8_t, 25>{12, 2, 5, 2, 10, 4, 5, 4, 10, 4, 11, 6, 11},
186 std::array<std::uint8_t, 25>{16, 0, 9, 2, 5, 2, 10, 4, 5, 4, 9, 4, 10, 4, 11, 6,
187 11},
188 std::array<std::uint8_t, 25>{12, 1, 2, 1, 4, 2, 5, 4, 5, 4, 11, 6, 11},
189 std::array<std::uint8_t, 25>{16, 0, 1, 0, 9, 1, 2, 2, 5, 4, 5, 4, 9, 4, 11, 6,
190 11},
191 std::array<std::uint8_t, 25>{10, 3, 4, 3, 6, 4, 5, 4, 11, 6, 11},
192 std::array<std::uint8_t, 25>{14, 0, 3, 0, 9, 3, 6, 4, 5, 4, 9, 4, 11, 6, 11},
193 std::array<std::uint8_t, 25>{16, 1, 9, 1, 10, 3, 6, 3, 9, 4, 5, 4, 11, 5, 10, 6,
194 11},
195 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 10, 3, 6, 4, 5, 4, 11, 5, 10, 6,
196 11},
197 std::array<std::uint8_t, 25>{16, 2, 5, 2, 10, 3, 4, 3, 6, 4, 5, 4, 10, 4, 11, 6,
198 11},
199 std::array<std::uint8_t, 25>{20, 0, 3, 0, 9, 2, 5, 2, 10, 3, 6,
200 4, 5, 4, 9, 4, 10, 4, 11, 6, 11},
201 std::array<std::uint8_t, 25>{16, 1, 2, 1, 9, 2, 5, 3, 6, 3, 9, 4, 5, 4, 11, 6,
202 11},
203 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 2, 2, 5, 3, 6, 4, 5, 4, 11, 6,
204 11},
205 std::array<std::uint8_t, 25>{2, 4, 7},
206 std::array<std::uint8_t, 25>{6, 0, 9, 4, 7, 4, 9},
207 std::array<std::uint8_t, 25>{4, 1, 4, 4, 7},
208 std::array<std::uint8_t, 25>{10, 0, 1, 0, 9, 1, 4, 4, 7, 4, 9},
209 std::array<std::uint8_t, 25>{6, 2, 10, 4, 7, 4, 10},
210 std::array<std::uint8_t, 25>{10, 0, 9, 2, 10, 4, 7, 4, 9, 4, 10},
211 std::array<std::uint8_t, 25>{10, 1, 2, 1, 4, 2, 10, 4, 7, 4, 10},
212 std::array<std::uint8_t, 25>{14, 0, 1, 0, 9, 1, 2, 2, 10, 4, 7, 4, 9, 4, 10},
213 std::array<std::uint8_t, 25>{8, 3, 4, 3, 11, 4, 7, 7, 11},
214 std::array<std::uint8_t, 25>{12, 0, 3, 0, 9, 3, 11, 4, 7, 4, 9, 7, 11},
215 std::array<std::uint8_t, 25>{12, 1, 4, 1, 9, 3, 9, 3, 11, 4, 7, 7, 11},
216 std::array<std::uint8_t, 25>{12, 0, 1, 0, 3, 1, 4, 3, 11, 4, 7, 7, 11},
217 std::array<std::uint8_t, 25>{12, 2, 10, 3, 4, 3, 11, 4, 7, 4, 10, 7, 11},
218 std::array<std::uint8_t, 25>{16, 0, 3, 0, 9, 2, 10, 3, 11, 4, 7, 4, 9, 4, 10, 7,
219 11},
220 std::array<std::uint8_t, 25>{16, 1, 2, 1, 9, 2, 10, 3, 9, 3, 11, 4, 7, 4, 10, 7,
221 11},
222 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 2, 2, 10, 3, 11, 4, 7, 4, 10, 7,
223 11},
224 std::array<std::uint8_t, 25>{8, 4, 5, 4, 7, 5, 12, 7, 12},
225 std::array<std::uint8_t, 25>{12, 0, 9, 4, 5, 4, 7, 4, 9, 5, 12, 7, 12},
226 std::array<std::uint8_t, 25>{12, 1, 4, 1, 10, 4, 7, 5, 10, 5, 12, 7, 12},
227 std::array<std::uint8_t, 25>{16, 0, 1, 0, 9, 1, 10, 4, 7, 4, 9, 5, 10, 5, 12, 7,
228 12},
229 std::array<std::uint8_t, 25>{12, 2, 5, 2, 10, 4, 7, 4, 10, 5, 12, 7, 12},
230 std::array<std::uint8_t, 25>{16, 0, 9, 2, 5, 2, 10, 4, 7, 4, 9, 4, 10, 5, 12, 7,
231 12},
232 std::array<std::uint8_t, 25>{12, 1, 2, 1, 4, 2, 5, 4, 7, 5, 12, 7, 12},
233 std::array<std::uint8_t, 25>{16, 0, 1, 0, 9, 1, 2, 2, 5, 4, 7, 4, 9, 5, 12, 7,
234 12},
235 std::array<std::uint8_t, 25>{12, 3, 4, 3, 11, 4, 5, 5, 12, 7, 11, 7, 12},
236 std::array<std::uint8_t, 25>{16, 0, 3, 0, 9, 3, 11, 4, 5, 4, 9, 5, 12, 7, 11, 7,
237 12},
238 std::array<std::uint8_t, 25>{16, 1, 9, 1, 10, 3, 9, 3, 11, 5, 10, 5, 12, 7, 11,
239 7, 12},
240 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 10, 3, 11, 5, 10, 5, 12, 7, 11,
241 7, 12},
242 std::array<std::uint8_t, 25>{16, 2, 5, 2, 10, 3, 4, 3, 11, 4, 10, 5, 12, 7, 11,
243 7, 12},
244 std::array<std::uint8_t, 25>{20, 0, 3, 0, 9, 2, 5, 2, 10, 3, 11,
245 4, 9, 4, 10, 5, 12, 7, 11, 7, 12},
246 std::array<std::uint8_t, 25>{16, 1, 2, 1, 9, 2, 5, 3, 9, 3, 11, 5, 12, 7, 11, 7,
247 12},
248 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 2, 2, 5, 3, 11, 5, 12, 7, 11, 7,
249 12},
250 std::array<std::uint8_t, 25>{8, 4, 7, 4, 11, 6, 7, 6, 11},
251 std::array<std::uint8_t, 25>{12, 0, 9, 4, 7, 4, 9, 4, 11, 6, 7, 6, 11},
252 std::array<std::uint8_t, 25>{10, 1, 4, 4, 7, 4, 11, 6, 7, 6, 11},
253 std::array<std::uint8_t, 25>{16, 0, 1, 0, 9, 1, 4, 4, 7, 4, 9, 4, 11, 6, 7, 6,
254 11},
255 std::array<std::uint8_t, 25>{12, 2, 10, 4, 7, 4, 10, 4, 11, 6, 7, 6, 11},
256 std::array<std::uint8_t, 25>{16, 0, 9, 2, 10, 4, 7, 4, 9, 4, 10, 4, 11, 6, 7, 6,
257 11},
258 std::array<std::uint8_t, 25>{16, 1, 2, 1, 4, 2, 10, 4, 7, 4, 10, 4, 11, 6, 7, 6,
259 11},
260 std::array<std::uint8_t, 25>{20, 0, 1, 0, 9, 1, 2, 2, 10, 4, 7,
261 4, 9, 4, 10, 4, 11, 6, 7, 6, 11},
262 std::array<std::uint8_t, 25>{8, 3, 4, 3, 6, 4, 7, 6, 7},
263 std::array<std::uint8_t, 25>{12, 0, 3, 0, 9, 3, 6, 4, 7, 4, 9, 6, 7},
264 std::array<std::uint8_t, 25>{12, 1, 4, 1, 9, 3, 6, 3, 9, 4, 7, 6, 7},
265 std::array<std::uint8_t, 25>{12, 0, 1, 0, 3, 1, 4, 3, 6, 4, 7, 6, 7},
266 std::array<std::uint8_t, 25>{12, 2, 10, 3, 4, 3, 6, 4, 7, 4, 10, 6, 7},
267 std::array<std::uint8_t, 25>{16, 0, 3, 0, 9, 2, 10, 3, 6, 4, 7, 4, 9, 4, 10, 6,
268 7},
269 std::array<std::uint8_t, 25>{16, 1, 2, 1, 9, 2, 10, 3, 6, 3, 9, 4, 7, 4, 10, 6,
270 7},
271 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 2, 2, 10, 3, 6, 4, 7, 4, 10, 6,
272 7},
273 std::array<std::uint8_t, 25>{12, 4, 5, 4, 11, 5, 12, 6, 7, 6, 11, 7, 12},
274 std::array<std::uint8_t, 25>{16, 0, 9, 4, 5, 4, 9, 4, 11, 5, 12, 6, 7, 6, 11, 7,
275 12},
276 std::array<std::uint8_t, 25>{16, 1, 4, 1, 10, 4, 11, 5, 10, 5, 12, 6, 7, 6, 11,
277 7, 12},
278 std::array<std::uint8_t, 25>{20, 0, 1, 0, 9, 1, 10, 4, 9, 4, 11,
279 5, 10, 5, 12, 6, 7, 6, 11, 7, 12},
280 std::array<std::uint8_t, 25>{16, 2, 5, 2, 10, 4, 10, 4, 11, 5, 12, 6, 7, 6, 11,
281 7, 12},
282 std::array<std::uint8_t, 25>{20, 0, 9, 2, 5, 2, 10, 4, 9, 4, 10,
283 4, 11, 5, 12, 6, 7, 6, 11, 7, 12},
284 std::array<std::uint8_t, 25>{16, 1, 2, 1, 4, 2, 5, 4, 11, 5, 12, 6, 7, 6, 11, 7,
285 12},
286 std::array<std::uint8_t, 25>{20, 0, 1, 0, 9, 1, 2, 2, 5, 4, 9,
287 4, 11, 5, 12, 6, 7, 6, 11, 7, 12},
288 std::array<std::uint8_t, 25>{12, 3, 4, 3, 6, 4, 5, 5, 12, 6, 7, 7, 12},
289 std::array<std::uint8_t, 25>{16, 0, 3, 0, 9, 3, 6, 4, 5, 4, 9, 5, 12, 6, 7, 7,
290 12},
291 std::array<std::uint8_t, 25>{16, 1, 9, 1, 10, 3, 6, 3, 9, 5, 10, 5, 12, 6, 7, 7,
292 12},
293 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 10, 3, 6, 5, 10, 5, 12, 6, 7, 7,
294 12},
295 std::array<std::uint8_t, 25>{16, 2, 5, 2, 10, 3, 4, 3, 6, 4, 10, 5, 12, 6, 7, 7,
296 12},
297 std::array<std::uint8_t, 25>{20, 0, 3, 0, 9, 2, 5, 2, 10, 3, 6,
298 4, 9, 4, 10, 5, 12, 6, 7, 7, 12},
299 std::array<std::uint8_t, 25>{16, 1, 2, 1, 9, 2, 5, 3, 6, 3, 9, 5, 12, 6, 7, 7,
300 12},
301 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 2, 2, 5, 3, 6, 5, 12, 6, 7, 7,
302 12},
303 std::array<std::uint8_t, 25>{4, 4, 12, 8, 12},
304 std::array<std::uint8_t, 25>{8, 0, 9, 4, 9, 4, 12, 8, 12},
305 std::array<std::uint8_t, 25>{6, 1, 4, 4, 12, 8, 12},
306 std::array<std::uint8_t, 25>{12, 0, 1, 0, 9, 1, 4, 4, 9, 4, 12, 8, 12},
307 std::array<std::uint8_t, 25>{8, 2, 10, 4, 10, 4, 12, 8, 12},
308 std::array<std::uint8_t, 25>{12, 0, 9, 2, 10, 4, 9, 4, 10, 4, 12, 8, 12},
309 std::array<std::uint8_t, 25>{12, 1, 2, 1, 4, 2, 10, 4, 10, 4, 12, 8, 12},
310 std::array<std::uint8_t, 25>{16, 0, 1, 0, 9, 1, 2, 2, 10, 4, 9, 4, 10, 4, 12, 8,
311 12},
312 std::array<std::uint8_t, 25>{6, 3, 4, 4, 12, 8, 12},
313 std::array<std::uint8_t, 25>{12, 0, 3, 0, 9, 3, 4, 4, 9, 4, 12, 8, 12},
314 std::array<std::uint8_t, 25>{12, 1, 4, 1, 9, 3, 4, 3, 9, 4, 12, 8, 12},
315 std::array<std::uint8_t, 25>{12, 0, 1, 0, 3, 1, 4, 3, 4, 4, 12, 8, 12},
316 std::array<std::uint8_t, 25>{10, 2, 10, 3, 4, 4, 10, 4, 12, 8, 12},
317 std::array<std::uint8_t, 25>{16, 0, 3, 0, 9, 2, 10, 3, 4, 4, 9, 4, 10, 4, 12, 8,
318 12},
319 std::array<std::uint8_t, 25>{16, 1, 2, 1, 9, 2, 10, 3, 4, 3, 9, 4, 10, 4, 12, 8,
320 12},
321 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 2, 2, 10, 3, 4, 4, 10, 4, 12, 8,
322 12},
323 std::array<std::uint8_t, 25>{8, 4, 5, 4, 12, 5, 8, 8, 12},
324 std::array<std::uint8_t, 25>{12, 0, 9, 4, 5, 4, 9, 4, 12, 5, 8, 8, 12},
325 std::array<std::uint8_t, 25>{12, 1, 4, 1, 10, 4, 12, 5, 8, 5, 10, 8, 12},
326 std::array<std::uint8_t, 25>{16, 0, 1, 0, 9, 1, 10, 4, 9, 4, 12, 5, 8, 5, 10, 8,
327 12},
328 std::array<std::uint8_t, 25>{12, 2, 5, 2, 10, 4, 10, 4, 12, 5, 8, 8, 12},
329 std::array<std::uint8_t, 25>{16, 0, 9, 2, 5, 2, 10, 4, 9, 4, 10, 4, 12, 5, 8, 8,
330 12},
331 std::array<std::uint8_t, 25>{12, 1, 2, 1, 4, 2, 5, 4, 12, 5, 8, 8, 12},
332 std::array<std::uint8_t, 25>{16, 0, 1, 0, 9, 1, 2, 2, 5, 4, 9, 4, 12, 5, 8, 8,
333 12},
334 std::array<std::uint8_t, 25>{10, 3, 4, 4, 5, 4, 12, 5, 8, 8, 12},
335 std::array<std::uint8_t, 25>{16, 0, 3, 0, 9, 3, 4, 4, 5, 4, 9, 4, 12, 5, 8, 8,
336 12},
337 std::array<std::uint8_t, 25>{16, 1, 9, 1, 10, 3, 4, 3, 9, 4, 12, 5, 8, 5, 10, 8,
338 12},
339 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 10, 3, 4, 4, 12, 5, 8, 5, 10, 8,
340 12},
341 std::array<std::uint8_t, 25>{14, 2, 5, 2, 10, 3, 4, 4, 10, 4, 12, 5, 8, 8, 12},
342 std::array<std::uint8_t, 25>{20, 0, 3, 0, 9, 2, 5, 2, 10, 3, 4,
343 4, 9, 4, 10, 4, 12, 5, 8, 8, 12},
344 std::array<std::uint8_t, 25>{16, 1, 2, 1, 9, 2, 5, 3, 4, 3, 9, 4, 12, 5, 8, 8,
345 12},
346 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 2, 2, 5, 3, 4, 4, 12, 5, 8, 8,
347 12},
348 std::array<std::uint8_t, 25>{8, 4, 11, 4, 12, 6, 11, 8, 12},
349 std::array<std::uint8_t, 25>{12, 0, 9, 4, 9, 4, 11, 4, 12, 6, 11, 8, 12},
350 std::array<std::uint8_t, 25>{10, 1, 4, 4, 11, 4, 12, 6, 11, 8, 12},
351 std::array<std::uint8_t, 25>{16, 0, 1, 0, 9, 1, 4, 4, 9, 4, 11, 4, 12, 6, 11, 8,
352 12},
353 std::array<std::uint8_t, 25>{12, 2, 10, 4, 10, 4, 11, 4, 12, 6, 11, 8, 12},
354 std::array<std::uint8_t, 25>{16, 0, 9, 2, 10, 4, 9, 4, 10, 4, 11, 4, 12, 6, 11,
355 8, 12},
356 std::array<std::uint8_t, 25>{16, 1, 2, 1, 4, 2, 10, 4, 10, 4, 11, 4, 12, 6, 11,
357 8, 12},
358 std::array<std::uint8_t, 25>{20, 0, 1, 0, 9, 1, 2, 2, 10, 4, 9,
359 4, 10, 4, 11, 4, 12, 6, 11, 8, 12},
360 std::array<std::uint8_t, 25>{12, 3, 4, 3, 6, 4, 11, 4, 12, 6, 11, 8, 12},
361 std::array<std::uint8_t, 25>{16, 0, 3, 0, 9, 3, 6, 4, 9, 4, 11, 4, 12, 6, 11, 8,
362 12},
363 std::array<std::uint8_t, 25>{16, 1, 4, 1, 9, 3, 6, 3, 9, 4, 11, 4, 12, 6, 11, 8,
364 12},
365 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 4, 3, 6, 4, 11, 4, 12, 6, 11, 8,
366 12},
367 std::array<std::uint8_t, 25>{16, 2, 10, 3, 4, 3, 6, 4, 10, 4, 11, 4, 12, 6, 11,
368 8, 12},
369 std::array<std::uint8_t, 25>{20, 0, 3, 0, 9, 2, 10, 3, 6, 4, 9,
370 4, 10, 4, 11, 4, 12, 6, 11, 8, 12},
371 std::array<std::uint8_t, 25>{20, 1, 2, 1, 9, 2, 10, 3, 6, 3, 9,
372 4, 10, 4, 11, 4, 12, 6, 11, 8, 12},
373 std::array<std::uint8_t, 25>{20, 0, 1, 0, 3, 1, 2, 2, 10, 3, 6,
374 4, 10, 4, 11, 4, 12, 6, 11, 8, 12},
375 std::array<std::uint8_t, 25>{12, 4, 5, 4, 11, 4, 12, 5, 8, 6, 11, 8, 12},
376 std::array<std::uint8_t, 25>{16, 0, 9, 4, 5, 4, 9, 4, 11, 4, 12, 5, 8, 6, 11, 8,
377 12},
378 std::array<std::uint8_t, 25>{16, 1, 4, 1, 10, 4, 11, 4, 12, 5, 8, 5, 10, 6, 11,
379 8, 12},
380 std::array<std::uint8_t, 25>{20, 0, 1, 0, 9, 1, 10, 4, 9, 4, 11,
381 4, 12, 5, 8, 5, 10, 6, 11, 8, 12},
382 std::array<std::uint8_t, 25>{16, 2, 5, 2, 10, 4, 10, 4, 11, 4, 12, 5, 8, 6, 11,
383 8, 12},
384 std::array<std::uint8_t, 25>{20, 0, 9, 2, 5, 2, 10, 4, 9, 4, 10,
385 4, 11, 4, 12, 5, 8, 6, 11, 8, 12},
386 std::array<std::uint8_t, 25>{16, 1, 2, 1, 4, 2, 5, 4, 11, 4, 12, 5, 8, 6, 11, 8,
387 12},
388 std::array<std::uint8_t, 25>{20, 0, 1, 0, 9, 1, 2, 2, 5, 4, 9,
389 4, 11, 4, 12, 5, 8, 6, 11, 8, 12},
390 std::array<std::uint8_t, 25>{16, 3, 4, 3, 6, 4, 5, 4, 11, 4, 12, 5, 8, 6, 11, 8,
391 12},
392 std::array<std::uint8_t, 25>{20, 0, 3, 0, 9, 3, 6, 4, 5, 4, 9,
393 4, 11, 4, 12, 5, 8, 6, 11, 8, 12},
394 std::array<std::uint8_t, 25>{20, 1, 9, 1, 10, 3, 6, 3, 9, 4, 11,
395 4, 12, 5, 8, 5, 10, 6, 11, 8, 12},
396 std::array<std::uint8_t, 25>{20, 0, 1, 0, 3, 1, 10, 3, 6, 4, 11,
397 4, 12, 5, 8, 5, 10, 6, 11, 8, 12},
398 std::array<std::uint8_t, 25>{20, 2, 5, 2, 10, 3, 4, 3, 6, 4, 10,
399 4, 11, 4, 12, 5, 8, 6, 11, 8, 12},
400 std::array<std::uint8_t, 25>{24, 0, 3, 0, 9, 2, 5, 2, 10, 3, 6, 4, 9,
401 4, 10, 4, 11, 4, 12, 5, 8, 6, 11, 8, 12},
402 std::array<std::uint8_t, 25>{20, 1, 2, 1, 9, 2, 5, 3, 6, 3, 9,
403 4, 11, 4, 12, 5, 8, 6, 11, 8, 12},
404 std::array<std::uint8_t, 25>{20, 0, 1, 0, 3, 1, 2, 2, 5, 3, 6,
405 4, 11, 4, 12, 5, 8, 6, 11, 8, 12},
406 std::array<std::uint8_t, 25>{8, 4, 7, 4, 12, 7, 8, 8, 12},
407 std::array<std::uint8_t, 25>{12, 0, 9, 4, 7, 4, 9, 4, 12, 7, 8, 8, 12},
408 std::array<std::uint8_t, 25>{10, 1, 4, 4, 7, 4, 12, 7, 8, 8, 12},
409 std::array<std::uint8_t, 25>{16, 0, 1, 0, 9, 1, 4, 4, 7, 4, 9, 4, 12, 7, 8, 8,
410 12},
411 std::array<std::uint8_t, 25>{12, 2, 10, 4, 7, 4, 10, 4, 12, 7, 8, 8, 12},
412 std::array<std::uint8_t, 25>{16, 0, 9, 2, 10, 4, 7, 4, 9, 4, 10, 4, 12, 7, 8, 8,
413 12},
414 std::array<std::uint8_t, 25>{16, 1, 2, 1, 4, 2, 10, 4, 7, 4, 10, 4, 12, 7, 8, 8,
415 12},
416 std::array<std::uint8_t, 25>{20, 0, 1, 0, 9, 1, 2, 2, 10, 4, 7,
417 4, 9, 4, 10, 4, 12, 7, 8, 8, 12},
418 std::array<std::uint8_t, 25>{12, 3, 4, 3, 11, 4, 12, 7, 8, 7, 11, 8, 12},
419 std::array<std::uint8_t, 25>{16, 0, 3, 0, 9, 3, 11, 4, 9, 4, 12, 7, 8, 7, 11, 8,
420 12},
421 std::array<std::uint8_t, 25>{16, 1, 4, 1, 9, 3, 9, 3, 11, 4, 12, 7, 8, 7, 11, 8,
422 12},
423 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 4, 3, 11, 4, 12, 7, 8, 7, 11, 8,
424 12},
425 std::array<std::uint8_t, 25>{16, 2, 10, 3, 4, 3, 11, 4, 10, 4, 12, 7, 8, 7, 11,
426 8, 12},
427 std::array<std::uint8_t, 25>{20, 0, 3, 0, 9, 2, 10, 3, 11, 4, 9,
428 4, 10, 4, 12, 7, 8, 7, 11, 8, 12},
429 std::array<std::uint8_t, 25>{20, 1, 2, 1, 9, 2, 10, 3, 9, 3, 11,
430 4, 10, 4, 12, 7, 8, 7, 11, 8, 12},
431 std::array<std::uint8_t, 25>{20, 0, 1, 0, 3, 1, 2, 2, 10, 3, 11,
432 4, 10, 4, 12, 7, 8, 7, 11, 8, 12},
433 std::array<std::uint8_t, 25>{8, 4, 5, 4, 7, 5, 8, 7, 8},
434 std::array<std::uint8_t, 25>{12, 0, 9, 4, 5, 4, 7, 4, 9, 5, 8, 7, 8},
435 std::array<std::uint8_t, 25>{12, 1, 4, 1, 10, 4, 7, 5, 8, 5, 10, 7, 8},
436 std::array<std::uint8_t, 25>{16, 0, 1, 0, 9, 1, 10, 4, 7, 4, 9, 5, 8, 5, 10, 7,
437 8},
438 std::array<std::uint8_t, 25>{12, 2, 5, 2, 10, 4, 7, 4, 10, 5, 8, 7, 8},
439 std::array<std::uint8_t, 25>{16, 0, 9, 2, 5, 2, 10, 4, 7, 4, 9, 4, 10, 5, 8, 7,
440 8},
441 std::array<std::uint8_t, 25>{12, 1, 2, 1, 4, 2, 5, 4, 7, 5, 8, 7, 8},
442 std::array<std::uint8_t, 25>{16, 0, 1, 0, 9, 1, 2, 2, 5, 4, 7, 4, 9, 5, 8, 7,
443 8},
444 std::array<std::uint8_t, 25>{12, 3, 4, 3, 11, 4, 5, 5, 8, 7, 8, 7, 11},
445 std::array<std::uint8_t, 25>{16, 0, 3, 0, 9, 3, 11, 4, 5, 4, 9, 5, 8, 7, 8, 7,
446 11},
447 std::array<std::uint8_t, 25>{16, 1, 9, 1, 10, 3, 9, 3, 11, 5, 8, 5, 10, 7, 8, 7,
448 11},
449 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 10, 3, 11, 5, 8, 5, 10, 7, 8, 7,
450 11},
451 std::array<std::uint8_t, 25>{16, 2, 5, 2, 10, 3, 4, 3, 11, 4, 10, 5, 8, 7, 8, 7,
452 11},
453 std::array<std::uint8_t, 25>{20, 0, 3, 0, 9, 2, 5, 2, 10, 3, 11,
454 4, 9, 4, 10, 5, 8, 7, 8, 7, 11},
455 std::array<std::uint8_t, 25>{16, 1, 2, 1, 9, 2, 5, 3, 9, 3, 11, 5, 8, 7, 8, 7,
456 11},
457 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 2, 2, 5, 3, 11, 5, 8, 7, 8, 7,
458 11},
459 std::array<std::uint8_t, 25>{12, 4, 11, 4, 12, 6, 7, 6, 11, 7, 8, 8, 12},
460 std::array<std::uint8_t, 25>{16, 0, 9, 4, 9, 4, 11, 4, 12, 6, 7, 6, 11, 7, 8, 8,
461 12},
462 std::array<std::uint8_t, 25>{14, 1, 4, 4, 11, 4, 12, 6, 7, 6, 11, 7, 8, 8, 12},
463 std::array<std::uint8_t, 25>{20, 0, 1, 0, 9, 1, 4, 4, 9, 4, 11,
464 4, 12, 6, 7, 6, 11, 7, 8, 8, 12},
465 std::array<std::uint8_t, 25>{16, 2, 10, 4, 10, 4, 11, 4, 12, 6, 7, 6, 11, 7, 8,
466 8, 12},
467 std::array<std::uint8_t, 25>{20, 0, 9, 2, 10, 4, 9, 4, 10, 4, 11,
468 4, 12, 6, 7, 6, 11, 7, 8, 8, 12},
469 std::array<std::uint8_t, 25>{20, 1, 2, 1, 4, 2, 10, 4, 10, 4, 11,
470 4, 12, 6, 7, 6, 11, 7, 8, 8, 12},
471 std::array<std::uint8_t, 25>{24, 0, 1, 0, 9, 1, 2, 2, 10, 4, 9, 4, 10,
472 4, 11, 4, 12, 6, 7, 6, 11, 7, 8, 8, 12},
473 std::array<std::uint8_t, 25>{12, 3, 4, 3, 6, 4, 12, 6, 7, 7, 8, 8, 12},
474 std::array<std::uint8_t, 25>{16, 0, 3, 0, 9, 3, 6, 4, 9, 4, 12, 6, 7, 7, 8, 8,
475 12},
476 std::array<std::uint8_t, 25>{16, 1, 4, 1, 9, 3, 6, 3, 9, 4, 12, 6, 7, 7, 8, 8,
477 12},
478 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 4, 3, 6, 4, 12, 6, 7, 7, 8, 8,
479 12},
480 std::array<std::uint8_t, 25>{16, 2, 10, 3, 4, 3, 6, 4, 10, 4, 12, 6, 7, 7, 8, 8,
481 12},
482 std::array<std::uint8_t, 25>{20, 0, 3, 0, 9, 2, 10, 3, 6, 4, 9,
483 4, 10, 4, 12, 6, 7, 7, 8, 8, 12},
484 std::array<std::uint8_t, 25>{20, 1, 2, 1, 9, 2, 10, 3, 6, 3, 9,
485 4, 10, 4, 12, 6, 7, 7, 8, 8, 12},
486 std::array<std::uint8_t, 25>{20, 0, 1, 0, 3, 1, 2, 2, 10, 3, 6,
487 4, 10, 4, 12, 6, 7, 7, 8, 8, 12},
488 std::array<std::uint8_t, 25>{12, 4, 5, 4, 11, 5, 8, 6, 7, 6, 11, 7, 8},
489 std::array<std::uint8_t, 25>{16, 0, 9, 4, 5, 4, 9, 4, 11, 5, 8, 6, 7, 6, 11, 7,
490 8},
491 std::array<std::uint8_t, 25>{16, 1, 4, 1, 10, 4, 11, 5, 8, 5, 10, 6, 7, 6, 11,
492 7, 8},
493 std::array<std::uint8_t, 25>{20, 0, 1, 0, 9, 1, 10, 4, 9, 4, 11,
494 5, 8, 5, 10, 6, 7, 6, 11, 7, 8},
495 std::array<std::uint8_t, 25>{16, 2, 5, 2, 10, 4, 10, 4, 11, 5, 8, 6, 7, 6, 11,
496 7, 8},
497 std::array<std::uint8_t, 25>{20, 0, 9, 2, 5, 2, 10, 4, 9, 4, 10,
498 4, 11, 5, 8, 6, 7, 6, 11, 7, 8},
499 std::array<std::uint8_t, 25>{16, 1, 2, 1, 4, 2, 5, 4, 11, 5, 8, 6, 7, 6, 11, 7,
500 8},
501 std::array<std::uint8_t, 25>{20, 0, 1, 0, 9, 1, 2, 2, 5, 4, 9,
502 4, 11, 5, 8, 6, 7, 6, 11, 7, 8},
503 std::array<std::uint8_t, 25>{12, 3, 4, 3, 6, 4, 5, 5, 8, 6, 7, 7, 8},
504 std::array<std::uint8_t, 25>{16, 0, 3, 0, 9, 3, 6, 4, 5, 4, 9, 5, 8, 6, 7, 7,
505 8},
506 std::array<std::uint8_t, 25>{16, 1, 9, 1, 10, 3, 6, 3, 9, 5, 8, 5, 10, 6, 7, 7,
507 8},
508 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 10, 3, 6, 5, 8, 5, 10, 6, 7, 7,
509 8},
510 std::array<std::uint8_t, 25>{16, 2, 5, 2, 10, 3, 4, 3, 6, 4, 10, 5, 8, 6, 7, 7,
511 8},
512 std::array<std::uint8_t, 25>{20, 0, 3, 0, 9, 2, 5, 2, 10, 3, 6,
513 4, 9, 4, 10, 5, 8, 6, 7, 7, 8},
514 std::array<std::uint8_t, 25>{16, 1, 2, 1, 9, 2, 5, 3, 6, 3, 9, 5, 8, 6, 7, 7,
515 8},
516 std::array<std::uint8_t, 25>{16, 0, 1, 0, 3, 1, 2, 2, 5, 3, 6, 5, 8, 6, 7, 7,
517 8}};
518
519 QuadKey key = derived().key(node);
520 auto depth = derived().depth(key);
521 auto center = derived().center(key);
522 auto half_length = derived().halfLength(key);
523
524 QuadKey::Key k = static_cast<QuadKey::Key>(key);
525 --k.x;
526 --k.y;
527
528 std::array<Index, 8> nodes{derived().index(QuadKey(k + QuadKey::Key(0, 0), depth)),
529 derived().index(QuadKey(k + QuadKey::Key(1, 0), depth)),
530 derived().index(QuadKey(k + QuadKey::Key(2, 0), depth)),
531 derived().index(QuadKey(k + QuadKey::Key(0, 1), depth)),
532 derived().index(QuadKey(k + QuadKey::Key(2, 1), depth)),
533 derived().index(QuadKey(k + QuadKey::Key(0, 2), depth)),
534 derived().index(QuadKey(k + QuadKey::Key(1, 2), depth)),
535 derived().index(QuadKey(k + QuadKey::Key(2, 2), depth))};
536
537 unsigned idx{};
538 for (unsigned i{}; 8 > i; ++i) {
539 idx |= static_cast<unsigned>(distanceContainsSurface(nodes[i])) << i;
540 }
541
542 std::array<Point, 13> points{distance_[nodes[0].pos][nodes[0].offset].point,
543 distance_[nodes[1].pos][nodes[1].offset].point,
544 distance_[nodes[2].pos][nodes[2].offset].point,
545 distance_[nodes[3].pos][nodes[3].offset].point,
546 distance_[node.pos][node.offset].point,
547 distance_[nodes[4].pos][nodes[4].offset].point,
548 distance_[nodes[5].pos][nodes[5].offset].point,
549 distance_[nodes[6].pos][nodes[6].offset].point,
550 distance_[nodes[7].pos][nodes[7].offset].point,
551 center + Point(-half_length, -half_length),
552 center + Point(half_length, -half_length),
553 center + Point(-half_length, half_length),
554 center + Point(half_length, half_length)};
555
556 float distance = std::numeric_limits<float>::max();
557 for (unsigned i = 1u; lut[idx][0] > i; i += 2) {
558 LineSegment<Dim, float> line(points[lut[idx][i]], points[lut[idx][i + 1u]]);
559 distance = std::min(distance, ufo::distanceSquared(line, query));
560 }
561
562 return distance;
563 } else if constexpr (3 == Dim) {
564 // TODO: Implement
565 }
566 }
567
568 template <class Geometry>
569 [[nodiscard]] float distanceSomething(Index node, Geometry const& query) const
570 {
571 return std::sqrt(distanceSquared(node, query));
572 }
573
574 template <class Geometry, class Predicate = pred::Leaf,
575 std::enable_if_t<pred::is_pred_v<Predicate>, bool> = true>
576 [[nodiscard]] float distance(
577 Geometry const& query, Predicate const& pred = pred::Leaf(),
578 DistanceInterpolate interpolate = DistanceInterpolate::ALL,
579 float max_distance = std::numeric_limits<float>::max(), float epsilon = 0.0f,
580 NearestSearchAlgorithm search_alg = NearestSearchAlgorithm::DEPTH_FIRST) const
581 {
582 return distance(derived().index(), pred, query, interpolate, max_distance, epsilon,
583 search_alg);
584 }
585
586 template <class NodeType, class Pred, class Geometry,
587 std::enable_if_t<Tree::template is_node_type_v<NodeType>, bool> = true,
588 std::enable_if_t<pred::is_pred_v<Predicate>, bool> = true>
589 [[nodiscard]] float distance(
590 NodeType node, Predicate const& pred, Geometry const& query,
591 DistanceInterpolate interpolate = DistanceInterpolate::ALL,
592 float max_distance = std::numeric_limits<float>::max(), float epsilon = 0.0f,
593 NearestSearchAlgorithm search_alg = NearestSearchAlgorithm::DEPTH_FIRST) const
594 {
595 // TODO: Implement
596 return 0.0f;
597 }
598
599 template <bool FastAsSonic, class Geometry>
600 [[nodiscard]] float distance(
601 Geometry const& query, DistanceInterpolate interpolate = DistanceInterpolate::ALL,
602 float max_distance = std::numeric_limits<float>::max(), float epsilon = 0.0f,
603 NearestSearchAlgorithm search_alg = NearestSearchAlgorithm::DEPTH_FIRST) const
604 {
605 return distance<FastAsSonic>(derived().index(), query, interpolate, max_distance,
606 epsilon, search_alg);
607 }
608
609 template <bool FastAsSonic, class NodeType, class Geometry,
610 std::enable_if_t<Tree::template is_node_type_v<NodeType>, bool> = true>
611 [[nodiscard]] float distance(
612 NodeType node, Geometry const& query,
613 DistanceInterpolate interpolate = DistanceInterpolate::ALL,
614 float max_distance = std::numeric_limits<float>::max(), float epsilon = 0.0f,
615 NearestSearchAlgorithm search_alg = NearestSearchAlgorithm::DEPTH_FIRST) const
616 {
617 auto n = derived().index(node);
618
619 // TODO: Implement
620
621 auto value_f = [this, &query](Index node) {
622 return derived().isLeaf(node)
623 ? std::numeric_limits<float>::infinity()
624 : [this, &query](Index node) {
625 if constexpr (std::is_same_v<Vec<Dim, float>,
626 std::decay_t<Geometry>>) {
627 auto p = distance_[node.pos][node.offset].point;
628 p.x -= query.x;
629 p.x *= p.x;
630 p.y -= query.y;
631 p.y *= p.y;
632 // p.z -= query.z;
633 // p.z *= p.z;
634 return p.x + p.y; // + p.z;
635 } else {
636 // TODO: Implement
637 }
638 }(node);
639 };
640
641 auto inner_f = [this, &query](Index node) {
642 if constexpr (std::is_same_v<Vec<Dim, float>, std::decay_t<Geometry>>) {
643 float x = UFO_CLAMP(query.x, distance_[node.pos].bounds(node.offset).min.x,
644 distance_[node.pos].bounds(node.offset).max.x);
645 float y = UFO_CLAMP(query.y, distance_[node.pos].bounds(node.offset).min.y,
646 distance_[node.pos].bounds(node.offset).max.y);
647 // float z = UFO_CLAMP(query.z, distance_[node.pos].bounds(node.offset).min.z,
648 // distance_[node.pos].bounds(node.offset).max.z);
649
650 x -= query.x;
651 x *= x;
652 y -= query.y;
653 y *= y;
654 // z -= query.z;
655 // z *= z;
656
657 return x + y; // + z;
658 } else {
659 // TODO: Implement
660 }
661 };
662
663 return std::sqrt(derived().template nearest<true, FastAsSonic>(
664 n, search_alg, value_f, inner_f, max_distance, epsilon));
665 }
666
667 template <class Predicate, class Geometry,
668 std::enable_if_t<pred::is_pred_v<Predicate>, bool> = true>
669 [[nodiscard]] std::pair<float, Point> distancePoint(
670 Predicate const& pred, Geometry const& query,
671 DistanceInterpolate interpolate = DistanceInterpolate::ALL,
672 float max_distance = std::numeric_limits<float>::max(), float epsilon = 0.0f,
673 NearestSearchAlgorithm search_alg = NearestSearchAlgorithm::DEPTH_FIRST) const
674 {
675 return distancePoint(derived().index(), pred, query, interpolate, max_distance,
676 epsilon, search_alg);
677 }
678
679 template <class NodeType, class Predicate, class Geometry,
680 std::enable_if_t<Tree::template is_node_type_v<NodeType>, bool> = true,
681 std::enable_if_t<pred::is_pred_v<Predicate>, bool> = true>
682 [[nodiscard]] std::pair<float, Point> distancePoint(
683 NodeType node, Predicate const& pred, Geometry const& query,
684 DistanceInterpolate interpolate = DistanceInterpolate::ALL,
685 float max_distance = std::numeric_limits<float>::max(), float epsilon = 0.0f,
686 NearestSearchAlgorithm search_alg = NearestSearchAlgorithm::DEPTH_FIRST) const
687 {
688 auto n = derived().index(node);
689
690 // TODO: Implement
691 }
692
693 template <bool FastAsSonic, class Geometry>
694 [[nodiscard]] std::pair<float, Index> distancePoint(
695 Geometry const& query, DistanceInterpolate interpolate = DistanceInterpolate::ALL,
696 float max_distance = std::numeric_limits<float>::max(), float epsilon = 0.0f,
697 NearestSearchAlgorithm search_alg = NearestSearchAlgorithm::DEPTH_FIRST) const
698 {
699 return distancePoint<FastAsSonic>(derived().index(), query, interpolate, max_distance,
700 epsilon, search_alg);
701 }
702
703 template <bool FastAsSonic, class NodeType, class Geometry,
704 std::enable_if_t<Tree::template is_node_type_v<NodeType>, bool> = true>
705 [[nodiscard]] std::pair<float, Index> distancePoint(
706 NodeType node, Geometry const& query,
707 DistanceInterpolate interpolate = DistanceInterpolate::ALL,
708 float max_distance = std::numeric_limits<float>::max(), float epsilon = 0.0f,
709 NearestSearchAlgorithm search_alg = NearestSearchAlgorithm::DEPTH_FIRST) const
710 {
711 auto n = derived().index(node);
712
713 // TODO: Implement
714
715 auto value_f = [this, &query](Index node) {
716 if constexpr (std::is_same_v<Vec<Dim, float>, std::decay_t<Geometry>>) {
717 auto p = distance_[node.pos][node.offset].point;
718 p.x -= query.x;
719 p.x *= p.x;
720 p.y -= query.y;
721 p.y *= p.y;
722 // p.z -= query.z;
723 // p.z *= p.z;
724 return p.x + p.y; // + p.z;
725 } else {
726 // TODO: Implement
727 }
728 };
729
730 auto inner_f = [this, &query](Index node) {
731 if constexpr (std::is_same_v<Vec<Dim, float>, std::decay_t<Geometry>>) {
732 float x = UFO_CLAMP(query.x, distance_[node.pos].bounds(node.offset).min.x,
733 distance_[node.pos].bounds(node.offset).max.x);
734 float y = UFO_CLAMP(query.y, distance_[node.pos].bounds(node.offset).min.y,
735 distance_[node.pos].bounds(node.offset).max.y);
736 // float z = UFO_CLAMP(query.z, distance_[node.pos].bounds(node.offset).min.z,
737 // distance_[node.pos].bounds(node.offset).max.z);
738
739 x -= query.x;
740 x *= x;
741 y -= query.y;
742 y *= y;
743 // z -= query.z;
744 // z *= z;
745
746 return x + y; // + z;
747 } else {
748 // TODO: Implement
749 }
750 };
751
752 auto [c_dist, c_node] = derived().template nearest<false, FastAsSonic>(
753 n, search_alg, value_f, inner_f, max_distance, epsilon);
754
755 // return std::pair(std::sqrt(c_dist), distance_[c_node.pos][c_node.offset].point);
756 return std::pair(std::sqrt(c_dist), c_node);
757 }
758
759 template <class NodeType,
760 std::enable_if_t<Tree::template is_node_type_v<NodeType>, bool> = true>
761 [[nodiscard]] bool distanceContainsSurface(NodeType node) const
762 {
763 auto n = derived().index(node);
764 return 0.0f < distance_[node.pos][node.offset].weight;
765 }
766
767 template <class NodeType,
768 std::enable_if_t<Tree::template is_node_type_v<NodeType>, bool> = true>
769 [[nodiscard]] DistanceInfo<Dim> distanceInfo(NodeType node) const
770 {
771 auto n = derived().index(node);
772 return distance_[n.pos][n.offset];
773 }
774
775 template <class NodeType,
776 std::enable_if_t<Tree::template is_node_type_v<NodeType>, bool> = true>
777 [[nodiscard]] DistanceBounds distanceBounds(NodeType node) const
778 {
779 if constexpr (WithBounds) {
780 auto n = derived().index(node);
781 return distance_[n.pos].bounds(n.offset);
782 } else {
783 return derived().bounds(node);
784 }
785 }
786
787 /**************************************************************************************
788 | |
789 | Modifiers |
790 | |
791 **************************************************************************************/
792
793 template <class NodeType,
794 std::enable_if_t<Tree::template is_node_type_v<NodeType>, bool> = true>
795 void distanceSet(NodeType node, DistanceInfo<Dim> info, bool propagate = true)
796 {
797 assert(isfinite(info.point));
798 assert(std::isfinite(info.weight));
799
800 DistanceBounds bounds;
801
802 if (0.0f >= info.weight) {
803 info = Block::resetInfo();
804 bounds = Block::resetBounds();
805 } else {
806 bounds = DistanceBounds(info.point, info.point);
807 }
808
809 auto node_f = [this, &info, &bounds](Index node) {
810 distance_[node.pos][node.offset] = info;
811 distance_[node.pos].bounds(node.offset) = bounds;
812 };
813
814 auto block_f = [this, &info, &bounds](pos_t pos) {
815 distance_[pos].fill(info, bounds);
816 };
817
818 if constexpr (std::is_same_v<std::decay_t<NodeType>, Index>) {
819 if (propagate) {
820 derived().recursParentFirst(node, node_f, block_f);
821 } else {
822 derived().recursLeaves(node, node_f, block_f);
823 }
824 } else {
825 if (propagate) {
826 auto update_f = [this](Index node, pos_t children) {
827 onUpdateNode(node, children);
828 };
829
830 derived().recursLeaves(derived().code(node), node_f, block_f, update_f,
831 propagate);
832 } else {
833 derived().recursParentFirst(derived().code(node), node_f, block_f);
834 }
835 }
836 }
837
838 template <class NodeType,
839 std::enable_if_t<Tree::template is_node_type_v<NodeType>, bool> = true>
840 void distanceSet(NodeType node, Point const& point, float weight = 1.0f,
841 bool propagate = true)
842 {
843 distanceSet(node, DistanceInfo<Dim>{point, weight}, propagate);
844 }
845
846 template <class NodeType, class UnaryOp,
847 std::enable_if_t<Tree::template is_node_type_v<NodeType>, bool> = true,
848 std::enable_if_t<std::is_invocable_r_v<DistanceInfo<Dim>, UnaryOp, Index>,
849 bool> = true>
850 void distanceSet(NodeType node, UnaryOp unary_op, bool propagate = true)
851 {
852 auto node_f = [this, unary_op](Index node) {
853 DistanceInfo<Dim> info = unary_op(node);
854
855 assert(isfinite(info.point));
856 assert(std::isfinite(info.weight));
857
858 if (0.0f >= info.weight) {
859 distance_[node.pos].reset(node.offset);
860 } else {
861 distance_[node.pos][node.offset] = info;
862 distance_[node.pos].bounds(node.offset) = DistanceBounds(info.point, info.point);
863 }
864 };
865
866 auto block_f = [this, node_f](pos_t pos) {
867 for (std::size_t i{}; BF > i; ++i) {
868 node_f(Index(pos, i));
869 }
870 };
871
872 auto update_f = [this](Index node, pos_t children) { onUpdateNode(node, children); };
873
874 if constexpr (std::is_same_v<Index, std::decay_t<NodeType>>) {
875 if (propagate) {
876 derived().recursLeaves(node, node_f, block_f, update_f);
877 } else {
878 derived().recursLeaves(node, node_f, block_f);
879 }
880 } else {
881 derived().recursLeaves(derived().code(node), node_f, block_f, update_f, propagate);
882 }
883 }
884
885 template <class NodeType,
886 std::enable_if_t<Tree::template is_node_type_v<NodeType>, bool> = true>
887 void distanceUpdate(NodeType node, Point const& point, float weight = 1.0f,
888 bool propagate = true)
889 {
890 assert(isfinite(point));
891 assert(std::isfinite(weight));
892
893 auto node_f = [this, &point, weight](Index node) {
894 DistanceInfo<Dim>& cur = distance_[node.pos][node.offset];
895
896 auto w = cur.weight + weight;
897
898 if (0.0f >= w) {
899 distance_[node.pos].reset(node.offset);
900 } else {
901 cur.point = cur.point * (cur.weight / w) + point * (weight / w);
902 cur.weight = w;
903 if constexpr (WithBounds) {
904 distance_[node.pos].bounds(node.offset) = DistanceBounds(cur.point, cur.point);
905 }
906 }
907
908 assert(isfinite(cur.point));
909 assert(std::isfinite(cur.weight));
910 };
911
912 auto block_f = [this, node_f](pos_t pos) {
913 for (std::size_t i{}; BF > i; ++i) {
914 node_f(Index(pos, i));
915 }
916 };
917
918 auto update_f = [this](Index node, pos_t children) { onUpdateNode(node, children); };
919
920 if constexpr (std::is_same_v<Index, std::decay_t<NodeType>>) {
921 if (propagate) {
922 derived().recursLeaves(node, node_f, block_f, update_f);
923 } else {
924 derived().recursLeaves(node, node_f, block_f);
925 }
926 } else {
927 derived().recursLeaves(derived().code(node), node_f, block_f, update_f, propagate);
928 }
929 }
930
931 template <class NodeType,
932 std::enable_if_t<Tree::template is_node_type_v<NodeType>, bool> = true>
933 void distanceReset(NodeType node, bool propagate = true)
934 {
935 auto info = Block::resetInfo();
936 distanceSet(node, info.point, info.weight, propagate);
937 }
938
939 template <class NodeType, class UnaryPred,
940 std::enable_if_t<Tree::template is_node_type_v<NodeType>, bool> = true,
941 std::enable_if_t<std::is_invocable_r_v<bool, UnaryPred, Index>, bool> = true>
942 void distanceResetIf(NodeType node, UnaryPred p, bool propagate = true)
943 {
944 auto node_f = [this, p](Index node) {
945 if (p) {
946 distance_[node.pos].reset(node.offset);
947 }
948 };
949
950 auto block_f = [this, node_f](pos_t pos) {
951 for (std::size_t i{}; BF > i; ++i) {
952 node_f(Index(pos, i));
953 }
954 };
955
956 auto update_f = [this](Index node, pos_t children) { onUpdateNode(node, children); };
957
958 if constexpr (std::is_same_v<Index, std::decay_t<NodeType>>) {
959 if (propagate) {
960 derived().recursLeaves(node, node_f, block_f, update_f);
961 } else {
962 derived().recursLeaves(node, node_f, block_f);
963 }
964 } else {
965 derived().recursLeaves(derived().code(node), node_f, block_f, update_f, propagate);
966 }
967 }
968
969 protected:
970 /**************************************************************************************
971 | |
972 | Constructors |
973 | |
974 **************************************************************************************/
975
976 DistanceMap() { createRoot(); }
977
978 DistanceMap(DistanceMap const& other) = default;
979
980 DistanceMap(DistanceMap&& other) = default;
981
982 template <class Derived2, class Tree2, bool WithBounds2>
983 DistanceMap(DistanceMap<Derived2, Tree2, WithBounds2> const& other)
984 : distance_(other.distance_)
985 {
986 }
987
988 template <class Derived2, class Tree2, bool WithBounds2>
989 DistanceMap(DistanceMap<Derived2, Tree2, WithBounds2>&& other)
990 : distance_(std::move(other.distance_))
991 {
992 }
993
994 /**************************************************************************************
995 | |
996 | Destructor |
997 | |
998 **************************************************************************************/
999
1000 ~DistanceMap() = default;
1001
1002 /**************************************************************************************
1003 | |
1004 | Assignment operator |
1005 | |
1006 **************************************************************************************/
1007
1008 DistanceMap& operator=(DistanceMap const& rhs) = default;
1009
1010 DistanceMap& operator=(DistanceMap&& rhs) = default;
1011
1012 template <class Derived2, class Tree2, bool WithBounds2>
1013 DistanceMap& operator=(DistanceMap<Derived2, Tree2, WithBounds2> const& rhs)
1014 {
1015 distance_ = rhs.distance_;
1016 return *this;
1017 }
1018
1019 template <class Derived2, class Tree2, bool WithBounds2>
1020 DistanceMap& operator=(DistanceMap<Derived2, Tree2, WithBounds2>&& rhs)
1021 {
1022 distance_ = std::move(rhs.distance_);
1023 return *this;
1024 }
1025
1026 /**************************************************************************************
1027 | |
1028 | Swap |
1029 | |
1030 **************************************************************************************/
1031
1032 void swap(DistanceMap& other) noexcept(noexcept(distance_.swap(other.distance_)))
1033 {
1034 distance_.swap(other.distance_);
1035 }
1036
1037 /**************************************************************************************
1038 | |
1039 | Create root |
1040 | |
1041 **************************************************************************************/
1042
1043 void createRoot() { distance_.emplace_back(); }
1044
1045 /**************************************************************************************
1046 | |
1047 | Derived |
1048 | |
1049 **************************************************************************************/
1050
1051 [[nodiscard]] constexpr Derived& derived() { return *static_cast<Derived*>(this); }
1052
1053 [[nodiscard]] constexpr Derived const& derived() const
1054 {
1055 return *static_cast<Derived const*>(this);
1056 }
1057
1058 /**************************************************************************************
1059 | |
1060 | Functions Derived expect |
1061 | |
1062 **************************************************************************************/
1063
1064 void onClear()
1065 {
1066 distance_.clear();
1067 createRoot();
1068 }
1069
1070 void onCreateChildren(Index node)
1071 {
1072 distance_.emplace_back(distance_[node.pos], node.offset);
1073 }
1074
1075 void onFillChildren(Index node, pos_t children)
1076 {
1077 distance_[children].fill(distance_[node.pos], node.offset);
1078 }
1079
1080 void onPruneChildren(Index node, pos_t /* children */)
1081 {
1082 // Update bounds to correspond to the node alone
1083 auto const& info = distance_[node.pos][node.offset];
1084 distance_[node.pos].bounds(node.offset) =
1085 0.0f == info.weight ? Block::resetBounds()
1086 : DistanceBounds(info.point, info.point);
1087 }
1088
1089 void onReserve(std::size_t cap) { distance_.reserve(cap); }
1090
1091 void onSetSize(std::size_t size) { distance_.setSize(size); }
1092
1093 void onEnable(std::size_t num_blocks)
1094 {
1095 createRoot();
1096 distance_.resize(num_blocks, distance_[0]);
1097 }
1098
1099 void onDisable() { distance_.clear(); }
1100
1101 void onUpdateNode(Index node, pos_t children)
1102 {
1103 DistanceInfo<Dim> info{Point{}, 0.0f};
1104 DistanceBounds bounds = Block::resetBounds();
1105
1106 auto& c = distance_[children];
1107 for (std::size_t i{}; BF > i; ++i) {
1108 info.point += c[i].point * c[i].weight;
1109 info.weight += c[i].weight;
1110 if constexpr (WithBounds) {
1111 bounds.min = min(bounds.min, c.bounds(i).min);
1112 bounds.max = max(bounds.max, c.bounds(i).max);
1113 }
1114 }
1115
1116 info.point =
1117 0.0f == info.weight ? Block::resetInfo().point : info.point / info.weight;
1118
1119 distance_[node.pos][node.offset] = info;
1120 if constexpr (WithBounds) {
1121 distance_[node.pos].bounds(node.offset) = bounds;
1122 }
1123 }
1124
1125 [[nodiscard]] bool isPrunable(pos_t block) const
1126 {
1127 using std::begin;
1128 using std::end;
1129 return std::all_of(begin(distance_[block].info()) + 1, end(distance_[block].info()),
1130 [v = distance_[block][0]](auto const& e) { return v == e; });
1131 }
1132
1133 //
1134 // Input/output (read/write)
1135 //
1136
1137 [[nodiscard]] static constexpr std::size_t serializedSizeNode() noexcept
1138 {
1139 return sizeof(DistanceInfo<Dim>);
1140 }
1141
1142 [[nodiscard]] constexpr std::size_t serializedSize(
1143 std::vector<std::pair<pos_t, BitSet<BF>>> const& /* nodes */,
1144 std::size_t num_nodes) const
1145 {
1146 return num_nodes * serializedSizeNode();
1147 }
1148
1149 void onRead(ReadBuffer& in, std::vector<std::pair<pos_t, BitSet<BF>>> const& nodes)
1150 {
1151 for (auto [block, offset] : nodes) {
1152 if (offset.all()) {
1153 in.read(distance_[block].info());
1154 for (offset_t i{}; BF > i; ++i) {
1155 distance_[block].bounds(i) =
1156 0.0f == distance_[block][i].weight
1157 ? Block::resetBounds()
1158 : DistanceBounds(distance_[block][i].point, distance_[block][i].point);
1159 }
1160 } else {
1161 for (offset_t i{}; BF > i; ++i) {
1162 if (offset[i]) {
1163 in.read(distance_[block][i]);
1164 distance_[block].bounds(i) = 0.0f == distance_[block][i].weight
1165 ? Block::resetBounds()
1166 : DistanceBounds(distance_[block][i].point,
1167 distance_[block][i].point);
1168 }
1169 }
1170 }
1171 }
1172 }
1173
1174 void onWrite(WriteBuffer& out, std::vector<std::pair<pos_t, BitSet<BF>>> const& nodes)
1175 {
1176 for (auto [block, offset] : nodes) {
1177 if (offset.all()) {
1178 out.write(distance_[block].info());
1179 } else {
1180 for (offset_t i{}; BF > i; ++i) {
1181 if (offset[i]) {
1182 out.write(distance_[block][i]);
1183 }
1184 }
1185 }
1186 }
1187 }
1188
1189 //
1190 // Dot file info
1191 //
1192
1193 void onDotFileInfo(std::ostream& out, Index node) const
1194 {
1195 out << "Distance: " << distanceInfo(node);
1196 }
1197
1198 /**************************************************************************************
1199 | |
1200 | Distance |
1201 | |
1202 **************************************************************************************/
1203
1204 protected:
1205 TreeContainer<Block> distance_;
1206};
1207
1208template <class Derived, class Tree>
1209using DistanceMapFast = DistanceMap<Derived, Tree, true>;
1210
1211template <class Derived, class Tree>
1212using DistanceMapSmall = DistanceMap<Derived, Tree, false>;
1213} // namespace ufo
1214
1215#endif // UFO_MAP_DISTANCE_MAP_HPP
ufo::Tree::branchingFactor
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
ufo::Tree::dimensions
static constexpr std::size_t dimensions() noexcept
Returns the number of dimensions of the tree (i.e., 1 = binary tree, 2 = quadtree,...
Definition tree.hpp:211
ufo::ipow
constexpr T ipow(T base, int exp) noexcept
Computes integer power of a base.
Definition math.hpp:112
ufo::weight
constexpr weight_type_t< C > weight(C color) noexcept
Returns the weight of color, or 1 if color has no weight field.
Definition weight.hpp:67
ufo
All vision-related classes and functions.
Definition cloud.hpp:49
ufo::distanceSquared
constexpr auto distanceSquared(A const &a, B const &b)
Computes the minimum squared distance between two shapes.
Definition distance.hpp:80
ufo::max
constexpr Vec< Geometry::dimension(), typename Geometry::value_type > max(Geometry const &g)
Returns the maximum coordinate of the minimum spanning axis-aligned bounding box of a geometry.
Definition fun.hpp:72
ufo::min
constexpr Vec< Geometry::dimension(), typename Geometry::value_type > min(Geometry const &g)
Returns the minimum coordinate of the minimum spanning axis-aligned bounding box of a geometry.
Definition fun.hpp:58
ufo::isfinite
constexpr bool isfinite(Vec< Dim, T > const &v) noexcept
Returns true if all components of a floating-point vector are finite.
Definition vec.hpp:1610
ufo::distance
constexpr auto distance(A const &a, B const &b)
Computes the minimum distance between two shapes.
Definition distance.hpp:61
ufo::AABB
Axis-Aligned Bounding Box (AABB) in Dim-dimensional space.
Definition aabb.hpp:70
ufo::LineSegment
Line segment in Dim-dimensional space.
Definition line_segment.hpp:68
ufo::Vec::y
constexpr auto & y(this auto &self) noexcept
Accesses the second component (y).
Definition vec.hpp:146
ufo::Vec::x
constexpr auto & x(this auto &self) noexcept
Accesses the first component (x).
Definition vec.hpp:136