1 // Copyright 2006-2009 Brad King, Chuck Stewart
2 // Distributed under the Boost Software License, Version 1.0.
3 // (See accompanying file rgtl_license_1_0.txt or copy at
4 // http://www.boost.org/LICENSE_1_0.txt)
5 #ifndef rgtl_octree_objects_hxx
6 #define rgtl_octree_objects_hxx
7
8 #include <iostream>
9 #include <cmath>
10 #include <limits>
11 #include <map>
12 #include <set>
13 #include <stack>
14 #include <vector>
15 #include "rgtl_octree_objects.h"
16
17 #include "rgtl_object_array.h"
18 #include "rgtl_object_once.h"
19 #include "rgtl_octree_cell_bounds.hxx"
20 #include "rgtl_octree_cell_geometry.hxx"
21 #include "rgtl_octree_cell_location.hxx"
22 #include "rgtl_octree_data_fixed.hxx"
23 #include "rgtl_serialize_access.h"
24 #include "rgtl_serialize_base.h"
25 #include "rgtl_serialize_istream.h"
26 #include "rgtl_serialize_ostream.h"
27
28 #include <vnl/vnl_math.h>
29
30 #ifdef _MSC_VER
31 # include <vcl_msvc_warnings.h>
32 #endif
33
34 // TODO: During tree construction we should check the set of objects
35 // in the current node. Those with no boundary inside the node should
36 // be left out of the count of objects remaining because no amount of
37 // subdivision will reduce the count.
38
39 //#define RGTL_OCTREE_OBJECTS_DEBUG_BUILD
40 //#define RGTL_OCTREE_OBJECTS_DEBUG_QUERY
41 //#define RGTL_OCTREE_OBJECTS_DEBUG_FRONT
42
43 #if defined(RGTL_OCTREE_OBJECTS_DEBUG_BUILD) || \
44 defined(RGTL_OCTREE_OBJECTS_DEBUG_QUERY) || \
45 defined(RGTL_OCTREE_OBJECTS_DEBUG_FRONT) || 0
46 # include <std::iostream.h>
47 #endif
48
49 // Compile-time option to store distances at node centers and using
50 // them during recursion to establish smaller query radius bounds. It
51 // seems like this should help for query points outside the root cell
52 // but is slower for query points inside the root cell because the
53 // overhead of computing the bound for every node on the way down the
54 // tree is too great compared to just looking up the initial leaf bound.
55 //#define RGTL_OCTREE_OBJECTS_NODE_DT
56
57 //----------------------------------------------------------------------------
58 struct rgtl_octree_objects_cell_data
59 {
60 // Distance from the cell center to the nearest object. NOTE:
61 // During tree construction and distance transform this could be set
62 // to a tentative bound. We could use the sign bit to indicate this
63 // case. Negative values would be tentative bounds and positive
64 // values final distances. The initial bound for an empty cell
65 // would be -inf, and for a non-empty cell half the cell diagonal.
66 double distance;
67
68 // TODO: Instead of using rgtl_octree_data_fixed, setup a custom
69 // node/leaf data policy so that a variable number of distance
70 // values can be stored for each cell. This will allow a kth-order
71 // distance transform to be computed by the user so that it can be
72 // used for initial bounds with k > 1 for query_closest.
73
rgtl_octree_objects_cell_datargtl_octree_objects_cell_data74 rgtl_octree_objects_cell_data() {}
rgtl_octree_objects_cell_datargtl_octree_objects_cell_data75 rgtl_octree_objects_cell_data(double d): distance(d) {}
76 private:
77 friend class rgtl_serialize_access;
78 template <class Serializer>
serializergtl_octree_objects_cell_data79 void serialize(Serializer& sr)
80 {
81 sr & distance;
82 }
83 };
84
85 //----------------------------------------------------------------------------
86 struct rgtl_octree_objects_leaf_data: public rgtl_octree_objects_cell_data
87 {
88 typedef rgtl_octree_objects_cell_data derived;
89 typedef int index_type;
90
91 // Objects in this leaf are stored contiguously in an array. This
92 // is the index of the beginning and (one past) the end of the block
93 // of object ids. If index_begin == index_end the cell has no
94 // objects.
95 index_type index_begin;
96 index_type index_end;
97
rgtl_octree_objects_leaf_datargtl_octree_objects_leaf_data98 rgtl_octree_objects_leaf_data() {}
rgtl_octree_objects_leaf_datargtl_octree_objects_leaf_data99 rgtl_octree_objects_leaf_data(double d, index_type begin, index_type end):
100 rgtl_octree_objects_cell_data(d), index_begin(begin), index_end(end) {}
101 private:
102 friend class rgtl_serialize_access;
103 template <class Serializer>
serializergtl_octree_objects_leaf_data104 void serialize(Serializer& sr)
105 {
106 sr & rgtl_serialize_base<derived>(*this);
107 sr & index_begin;
108 sr & index_end;
109 }
110 };
111
112 //----------------------------------------------------------------------------
113 #ifdef RGTL_OCTREE_OBJECTS_NODE_DT
114 struct rgtl_octree_objects_node_data: public rgtl_octree_objects_cell_data
115 {
116 typedef rgtl_octree_objects_cell_data derived;
117
118 // Currently we have no extra data for nodes.
rgtl_octree_objects_node_datargtl_octree_objects_node_data119 rgtl_octree_objects_node_data() {}
rgtl_octree_objects_node_datargtl_octree_objects_node_data120 rgtl_octree_objects_node_data(double d): rgtl_octree_objects_cell_data(d) {}
121 private:
122 friend class rgtl_serialize_access;
123 template <class Serializer>
serializergtl_octree_objects_node_data124 void serialize(Serializer& sr)
125 {
126 sr & rgtl_serialize_base<derived>(*this);
127 }
128 };
129 #else
130 typedef void rgtl_octree_objects_node_data;
131 #endif
132
133 //----------------------------------------------------------------------------
134 template <unsigned int D>
135 class rgtl_octree_objects_distance_transform
136 {
137 public:
138 typedef rgtl_octree_objects_leaf_data leaf_data_type;
139 typedef rgtl_octree_objects_node_data node_data_type;
140 typedef rgtl_octree_data_fixed<D, leaf_data_type, node_data_type> tree_type;
141 typedef typename tree_type::cell_index_type cell_index_type;
142 typedef typename tree_type::cell_location_type cell_location_type;
143 typedef typename tree_type::child_index_type child_index_type;
144
145 // An entry on the front of propagating distance bounds.
146 struct entry_type
147 {
entry_typergtl_octree_objects_distance_transform::entry_type148 entry_type(cell_location_type const& idx,
149 cell_index_type cidx, double bound):
150 location(idx), cell_index(cidx), Bound(bound) {}
151
152 // Logical index of the cell.
153 cell_location_type location;
154
155 // Integer index of the cell.
156 cell_index_type cell_index;
157
158 // Current bound.
159 double Bound;
160 };
161 struct entry_compare_type
162 {
operator ()rgtl_octree_objects_distance_transform::entry_compare_type163 bool operator()(const entry_type& l, const entry_type& r) const
164 {
165 return l.Bound < r.Bound;
166 }
167 };
168 struct index_compare_type
169 {
operator ()rgtl_octree_objects_distance_transform::index_compare_type170 bool operator()(cell_index_type l, cell_index_type r) const
171 {
172 return l < r;
173 }
174 };
175
176 // The front of propagating distance bounds.
177 typedef std::multiset<entry_type, entry_compare_type> front_type;
178 front_type front_;
179 typedef std::map<cell_index_type,
180 typename front_type::iterator,
181 index_compare_type> front_index_map_type;
182 front_index_map_type front_index_map_;
183
184 typedef rgtl_octree_objects_internal<D> objects_type;
185 objects_type& objects_;
186 tree_type& tree_;
187 int k_;
188 std::vector<double> squared_distances_;
189
rgtl_octree_objects_distance_transform(objects_type * intern,int k)190 rgtl_octree_objects_distance_transform(objects_type* intern, int k):
191 objects_(*intern), tree_(intern->tree_), k_(k),
192 squared_distances_(k) {}
193 void initialize_front(cell_location_type const& cell, cell_index_type cidx);
194 bool execute_transform();
195 void propagate_front(cell_location_type const& cell, double const center[D],
196 double distance);
197 void propagate_front(double const from_center[D], double distance,
198 unsigned int face, cell_location_type const& cell,
199 cell_index_type cidx);
200 #ifdef RGTL_OCTREE_OBJECTS_NODE_DT
201 bool transform_nodes();
202 bool transform_nodes(cell_location_type const& cell, cell_index_type cidx,
203 double half_diagonal, double& parent_bound);
204 #endif
205 };
206
207 //----------------------------------------------------------------------------
208 template <unsigned int D>
209 class rgtl_octree_objects_query_closest
210 {
211 public:
212 // Construct with a reference to the main representation.
213 typedef rgtl_octree_objects_internal<D> internal_type;
214 rgtl_octree_objects_query_closest(internal_type const& intern, int k);
215
216 // Get useful types from the main representation.
217 typedef typename internal_type::cell_location_type cell_location_type;
218 typedef typename internal_type::cell_index_type cell_index_type;
219 typedef typename internal_type::child_index_type child_index_type;
220 typedef typename internal_type::leaf_data_type leaf_data_type;
221 typedef typename internal_type::node_data_type node_data_type;
222
223 // The query entry point.
224 int query(double const p[D], int k, double bound,
225 int* ids, double* squared_distances, double* points);
226
227 // The recursive query implementation.
228 void query_impl(cell_location_type const& cell, cell_index_type cell_index,
229 double const p[D], int k);
230
231 // Entry in the list of closest objects.
232 struct closest_object_entry
233 {
closest_object_entryrgtl_octree_objects_query_closest::closest_object_entry234 closest_object_entry(): id(-1), distance_squared(-1) {}
closest_object_entryrgtl_octree_objects_query_closest::closest_object_entry235 closest_object_entry(int idx, double d2, double q[D])
236 : id(idx), distance_squared(d2)
237 {
238 for (unsigned int a=0; a < D; ++a)
239 {
240 this->point[a] = q[a];
241 }
242 }
243 int id;
244 double distance_squared;
245 double point[D];
246
operator <rgtl_octree_objects_query_closest::closest_object_entry247 bool operator<(closest_object_entry const& that)
248 {
249 return this->distance_squared >= 0 &&
250 that.distance_squared >= 0 &&
251 this->distance_squared < that.distance_squared;
252 }
253 };
254
255 // The internal objects representation.
256 internal_type const& internal_;
257
258 // Keep a sorted list of the best k squared distances. Use -1 to
259 // indicate no object yet found.
260 std::vector<closest_object_entry> best_;
261
262 // Keep track of the current squared distance bound.
263 double bound_;
264
265 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
266 // Keep track of the number of object queries made.
267 int checked_count_;
268 #endif
269 };
270
271 //----------------------------------------------------------------------------
272 template <unsigned int D>
273 class rgtl_octree_objects_internal
274 {
275 public:
276 typedef rgtl_octree_objects_leaf_data leaf_data_type;
277 typedef rgtl_octree_objects_node_data node_data_type;
278 typedef rgtl_octree_data_fixed<D, leaf_data_type, node_data_type> tree_type;
279 typedef rgtl_object_array<D> object_array_type;
280 typedef rgtl_octree_cell_bounds<D> bounds_type;
281 typedef typename tree_type::cell_index_type cell_index_type;
282 typedef typename tree_type::cell_location_type cell_location_type;
283 typedef typename tree_type::child_index_type child_index_type;
284 typedef rgtl_octree_cell_geometry<D> cell_geometry_type;
285 rgtl_octree_objects_internal(object_array_type const& objs);
286
287 rgtl_octree_objects_internal(object_array_type const& objs,
288 bounds_type const& b, int ml);
289
290 // Wrap around public object array API.
number_of_objects() const291 int number_of_objects() const
292 {
293 return this->object_array_.number_of_objects();
294 }
object_intersects_object(int idA,int idB) const295 bool object_intersects_object(int idA, int idB) const
296 {
297 return this->object_array_.object_intersects_object(idA, idB);
298 }
object_intersects_box(int id,cell_geometry_type const & cell_geometry) const299 bool object_intersects_box(int id,
300 cell_geometry_type const& cell_geometry) const
301 {
302 return this->object_array_.object_intersects_box
303 (id,
304 cell_geometry.get_sphere_center(),
305 cell_geometry.get_sphere_radius(),
306 cell_geometry.get_lower(),
307 cell_geometry.get_upper(),
308 cell_geometry.get_corners());
309 }
object_closest_point(int id,double const x[D],double y[D],double bound_squared) const310 bool object_closest_point(int id,
311 double const x[D],
312 double y[D],
313 double bound_squared) const
314 {
315 return this->object_array_.object_closest_point(id, x, y, bound_squared);
316 }
317
318 // Compute the squared distance between two points.
compute_distance_squared(double const p[D],double const q[D])319 static double compute_distance_squared(double const p[D],
320 double const q[D])
321 {
322 double d = 0.0;
323 for (unsigned int a=0; a < D; ++a)
324 {
325 double da = p[a]-q[a];
326 d += da*da;
327 }
328 return d;
329 }
330
331 // Compute the center point of a cell.
compute_center(cell_location_type const & cell,double center[D]) const332 void compute_center(cell_location_type const& cell, double center[D]) const
333 {
334 rgtl_octree_cell_bounds<D> upper;
335 child_index_type last = child_index_type((1<<D)-1);
336 upper.compute_bounds(this->bounds_, cell.get_child(last));
337 upper.origin().copy_out(center);
338 }
339
340 // Compute the squared distances to the nearest and farthest point
341 // of a cell volume to the given point.
342 void cell_distances(cell_location_type const& cell, double const x[D],
343 double& nearest_squared, double& farthest_squared) const;
344 void cell_distances(bounds_type const& cell_bounds, double const x[D],
345 double& nearest_squared, double& farthest_squared) const;
346 void cell_nearest(cell_location_type const& cell, double const x[D],
347 double& nearest_squared) const;
348 void cell_nearest(bounds_type const& cell_bounds, double const x[D],
349 double& nearest_squared) const;
350
351 // Build the spatial structure within the given cell.
352 void build(cell_geometry_type const& cell_geometry, std::vector<int>& ids);
353
354 // Query the closest k objects.
355 int query_closest(double const p[D], int k, double bound_squared, int* ids,
356 double* squared_distances, double* points) const;
357 int query_closest_impl(double const p[D], int k, double current_bound,
358 int* ids, double* squared_distances,
359 double* points) const;
360
361 // Lookup an initial closest object distance bound.
362 void lookup_leaf(cell_location_type const& cell,
363 cell_index_type cidx,
364 double const p[D],
365 cell_location_type& leaf, cell_index_type& lidx) const;
366
367 // Compute a kth-order distance transform on the cell centers so
368 // that each has a known distance to the kth-closest object.
369 bool compute_distance_transform(int k);
370
371 // Query the objects intersecting a sphere.
372 int query_sphere(double const center[D], double radius,
373 std::vector<int>& ids) const;
374 void query_sphere(cell_location_type const& cell,
375 cell_index_type cidx,
376 double const p[D],
377 double radius_squared,
378 std::vector<int>& ids) const;
379
380 // Extract objects from a cell.
381 void extract_objects(cell_index_type cidx,
382 double const* center, double radius_squared,
383 std::vector<int>& ids) const;
384
385 // Query the objects intersecting an object.
386 int query_object(int id, std::vector<int>& ids) const;
387 void query_object(cell_location_type const& cell,
388 cell_index_type cidx,
389 int idA, std::vector<int>& ids) const;
390
391 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
set_query_closest_debug(bool b)392 void set_query_closest_debug(bool b) { this->query_closest_debug_ = b; }
393 #else
set_query_closest_debug(bool)394 void set_query_closest_debug(bool) {}
395 #endif
396
397 private:
398 object_array_type const& object_array_;
399 bounds_type bounds_;
400
401 // The maximum subdivision level.
402 int max_level_;
403
404 // The maximum number of objects per leaf.
405 int max_per_leaf_;
406
407 // The order of the computed distance transform.
408 int distance_transform_order_;
409
410 // Keep track of objects visited on a per-query basis.
411 rgtl_object_once object_once_;
412
413 // The actual octree structure.
414 tree_type tree_;
415
416 // Leaf data stores index ranges into this array which maps to the
417 // object ids in each leaf.
418 std::vector<int> object_ids_;
419
420 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
421 // Enable closest point query output only after the distance
422 // transform has finished.
423 bool query_closest_debug_;
424 #endif
425
426 friend class rgtl_octree_objects_distance_transform<D>;
427 friend class rgtl_octree_objects_query_closest<D>;
428
429 friend class rgtl_serialize_access;
430 template <class Serializer>
serialize(Serializer & sr)431 void serialize(Serializer& sr)
432 {
433 sr & bounds_;
434 sr & max_level_;
435 sr & max_per_leaf_;
436 sr & distance_transform_order_;
437 sr & object_once_;
438 sr & tree_;
439 sr & object_ids_;
440 }
441 };
442
443 //----------------------------------------------------------------------------
444 template <unsigned int D>
445 rgtl_octree_objects_internal<D>
rgtl_octree_objects_internal(object_array_type const & objs)446 ::rgtl_octree_objects_internal(object_array_type const& objs):
447 object_array_(objs), bounds_(), max_level_(0), max_per_leaf_(0),
448 distance_transform_order_(0), object_once_()
449 {
450 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
451 this->query_closest_debug_ = false;
452 #endif
453 }
454
455 //----------------------------------------------------------------------------
456 template <unsigned int D>
457 rgtl_octree_objects_internal<D>
rgtl_octree_objects_internal(object_array_type const & objs,bounds_type const & b,int ml)458 ::rgtl_octree_objects_internal(object_array_type const& objs,
459 bounds_type const& b, int ml):
460 object_array_(objs), bounds_(b), max_level_(ml), max_per_leaf_(10),
461 distance_transform_order_(0), object_once_(objs.number_of_objects())
462 {
463 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
464 this->query_closest_debug_ = false;
465 #endif
466 int n = this->number_of_objects();
467 std::vector<int> ids(n);
468 for (int i=0; i < n; ++i)
469 {
470 ids[i] = i;
471 }
472 cell_geometry_type root_geometry(cell_location_type(), this->bounds_);
473 this->build(root_geometry, ids);
474 }
475
476 //----------------------------------------------------------------------------
477 template <unsigned int D>
478 void
479 rgtl_octree_objects_internal<D>
cell_distances(cell_location_type const & cell,double const x[D],double & nearest_squared,double & farthest_squared) const480 ::cell_distances(cell_location_type const& cell, double const x[D],
481 double& nearest_squared, double& farthest_squared) const
482 {
483 // Get the bounds of the cell.
484 bounds_type cell_bounds;
485 cell_bounds.compute_bounds(this->bounds_, cell);
486
487 // Compute the distances for these bounds.
488 this->cell_distances(cell_bounds, x, nearest_squared, farthest_squared);
489 }
490
491 //----------------------------------------------------------------------------
492 template <unsigned int D>
493 void
494 rgtl_octree_objects_internal<D>
cell_distances(bounds_type const & cell_bounds,double const x[D],double & nearest_squared,double & farthest_squared) const495 ::cell_distances(bounds_type const& cell_bounds, double const x[D],
496 double& nearest_squared, double& farthest_squared) const
497 {
498 // Compute the squared magnitude of vectors pointing from the given
499 // point the nearest and farthest points in the volume of the cell.
500 nearest_squared = 0;
501 farthest_squared = 0;
502 for (unsigned int a=0; a < D; ++a)
503 {
504 double l = cell_bounds.origin(a)-x[a];
505 double u = cell_bounds.origin(a)+cell_bounds.size()-x[a];
506 double l2 = l*l;
507 double u2 = u*u;
508 if (l > 0) { nearest_squared += l2; }
509 else if (u < 0) { nearest_squared += u2; }
510 if (l2 > u2) { farthest_squared += l2; }
511 else { farthest_squared += u2; }
512 }
513 }
514
515 //----------------------------------------------------------------------------
516 template <unsigned int D>
517 void
518 rgtl_octree_objects_internal<D>
cell_nearest(cell_location_type const & cell,double const x[D],double & nearest_squared) const519 ::cell_nearest(cell_location_type const& cell, double const x[D],
520 double& nearest_squared) const
521 {
522 // Get the bounds of the cell.
523 bounds_type cell_bounds;
524 cell_bounds.compute_bounds(this->bounds_, cell);
525
526 // Compute the distances for these bounds.
527 this->cell_nearest(cell_bounds, x, nearest_squared);
528 }
529
530 //----------------------------------------------------------------------------
531 template <unsigned int D>
532 void
533 rgtl_octree_objects_internal<D>
cell_nearest(bounds_type const & cell_bounds,double const x[D],double & nearest_squared) const534 ::cell_nearest(bounds_type const& cell_bounds, double const x[D],
535 double& nearest_squared) const
536 {
537 // Compute the squared magnitude of vectors pointing from the given
538 // point the nearest point in the volume of the cell.
539 nearest_squared = 0;
540 for (unsigned int a=0; a < D; ++a)
541 {
542 double l = cell_bounds.origin(a)-x[a];
543 double u = cell_bounds.origin(a)+cell_bounds.size()-x[a];
544 double l2 = l*l;
545 double u2 = u*u;
546 if (l > 0) { nearest_squared += l2; }
547 else if (u < 0) { nearest_squared += u2; }
548 }
549 }
550
551 //----------------------------------------------------------------------------
552 template <unsigned int D>
553 void
554 rgtl_octree_objects_internal<D>
build(cell_geometry_type const & cell_geometry,std::vector<int> & ids)555 ::build(cell_geometry_type const& cell_geometry, std::vector<int>& ids)
556 {
557 cell_location_type const& cell = cell_geometry.location();
558 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_BUILD
559 std::cout << "Considering " << ids.size()
560 << " objects in cell " << cell << std::endl;
561 #endif
562 bool tooDeep = cell.level() >= this->max_level_;
563 bool tooMany = (static_cast<int>(ids.size()) > this->max_per_leaf_);
564 if (tooMany && !tooDeep)
565 {
566 // We need to divide this cell.
567 // Compute the child cell geometries.
568 cell_geometry_type child_geometry[1<<D];
569 cell_geometry.get_children(child_geometry);
570
571 // Distribute objects into children.
572 std::vector<int> child_ids[1<<D];
573 for (unsigned int i=0; i < (1<<D); ++i)
574 {
575 // Build a list of objects intersecting this child.
576 for (std::vector<int>::const_iterator pi = ids.begin();
577 pi != ids.end(); ++pi)
578 {
579 int id = *pi;
580 if (this->object_intersects_box(id, child_geometry[i]))
581 {
582 child_ids[i].push_back(id);
583 }
584 }
585 }
586
587 // Erase memory used by id list for this cell.
588 ids.clear();
589
590 // Build the children recursively.
591 for (unsigned int i=0; i < (1<<D); ++i)
592 {
593 this->build(child_geometry[i], child_ids[i]);
594 }
595 }
596 else
597 {
598 // We will not divide this cell further.
599 // Store the objects for this cell.
600 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_BUILD
601 if (!ids.empty())
602 {
603 std::cout << "Storing " << ids.size() << " objects in cell "
604 << cell << std::endl;
605 }
606 #endif
607 typedef typename leaf_data_type::index_type index_type;
608 index_type index_begin = index_type(this->object_ids_.size());
609 for (std::vector<int>::const_iterator pi = ids.begin();
610 pi != ids.end(); ++pi)
611 {
612 this->object_ids_.push_back(*pi);
613 }
614 index_type index_end = index_type(this->object_ids_.size());
615 leaf_data_type leaf_data(-std::numeric_limits<double>::infinity(),
616 index_begin, index_end);
617
618 // Store the data in the leaf.
619 this->tree_.set_leaf_data(cell, &leaf_data);
620 }
621 }
622
623 //----------------------------------------------------------------------------
624 template <unsigned int D>
625 int
626 rgtl_octree_objects_internal<D>
query_closest(double const p[D],int k,double bound_squared,int * ids,double * squared_distances,double * points) const627 ::query_closest(double const p[D], int k, double bound_squared,
628 int* ids, double* squared_distances, double* points) const
629 {
630 // Establish an initial upper-bound on the distance to an object.
631 double nearest_squared;
632 double farthest_squared;
633 this->cell_distances(this->bounds_, p, nearest_squared, farthest_squared);
634
635 // Check for a smaller bound given by the caller.
636 if (bound_squared >= 0 && bound_squared < farthest_squared)
637 {
638 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
639 if (this->query_closest_debug_)
640 {
641 std::cout << "User initial bound "
642 << bound_squared << " < " << farthest_squared << std::endl;
643 }
644 #endif
645 farthest_squared = bound_squared;
646 }
647
648 #ifndef RGTL_OCTREE_OBJECTS_NODE_DT
649 // Check for a smaller bound given by the distance transform.
650 if (nearest_squared <= 0 && k <= this->distance_transform_order_)
651 {
652 // The query point is inside the root cell and the number of
653 // objects desired is not larger than the order of the distance
654 // transform. An initial bound may be available in the leaf
655 // containing the query point.
656 cell_location_type leaf;
657 cell_index_type lidx;
658 this->lookup_leaf(cell_location_type(), cell_index_type(), p, leaf, lidx);
659 if (leaf_data_type const* leaf_data = this->tree_.get_leaf_data(lidx))
660 {
661 if (vnl_math::isfinite(leaf_data->distance))
662 {
663 // Compute a bound on the distance by adding the distance from
664 // the query point to the cell center and the distance from
665 // the cell center to the nearest object.
666 double bound = std::fabs(leaf_data->distance);
667 double center[D];
668 this->compute_center(leaf, center);
669 double distance_squared = this->compute_distance_squared(center, p);
670 bound += std::sqrt(distance_squared);
671
672 // Shrink the squared distance bound if possible.
673 // Increase the computed potential bound by a small fraction
674 // in order to avoid missing an object altogether due to
675 // rounding problems.
676 double squared_bound = bound*bound*1.00001;
677 if (squared_bound < farthest_squared)
678 {
679 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
680 if (this->query_closest_debug_)
681 {
682 std::cout << "Reduced initial bound "
683 << squared_bound << " < " << farthest_squared
684 << " using leaf " << leaf << std::endl;
685 }
686 #endif
687 farthest_squared = squared_bound;
688 }
689 }
690 }
691 }
692 #endif
693
694 // Perform the real query.
695 return this->query_closest_impl(p, k, farthest_squared,
696 ids, squared_distances, points);
697 }
698
699 //----------------------------------------------------------------------------
700 template <unsigned int D>
701 int
702 rgtl_octree_objects_internal<D>
query_closest_impl(double const p[D],int k,double current_bound,int * ids,double * squared_distances,double * points) const703 ::query_closest_impl(double const p[D], int k, double current_bound, int* ids,
704 double* squared_distances, double* points) const
705 {
706 rgtl_octree_objects_query_closest<D> qc(*this, k);
707 return qc.query(p, k, current_bound, ids, squared_distances, points);
708 }
709
710 //----------------------------------------------------------------------------
711 template <unsigned int D>
712 void rgtl_octree_objects_internal<D>
lookup_leaf(cell_location_type const & cell,cell_index_type cidx,double const p[D],cell_location_type & leaf,cell_index_type & lidx) const713 ::lookup_leaf(cell_location_type const& cell,
714 cell_index_type cidx,
715 double const p[D],
716 cell_location_type& leaf, cell_index_type& lidx) const
717 {
718 if (this->tree_.has_children(cidx))
719 {
720 // Compute the index of the child containing the point.
721 double center[D];
722 this->compute_center(cell, center);
723 child_index_type child_index(0);
724 for (unsigned int a=0; a < D; ++a)
725 {
726 child_index |= (p[a] < center[a])? 0 : (1<<a);
727 }
728
729 // Recursively explore the child.
730 this->lookup_leaf(cell.get_child(child_index),
731 this->tree_.get_child(cidx, child_index),
732 p, leaf, lidx);
733 }
734 else
735 {
736 // Return this leaf.
737 leaf = cell;
738 lidx = cidx;
739 }
740 }
741
742 //----------------------------------------------------------------------------
743 template <unsigned int D>
compute_distance_transform(int k)744 bool rgtl_octree_objects_internal<D>::compute_distance_transform(int k)
745 {
746 // Make sure we do not compute more than one distance transform.
747 if (this->distance_transform_order_ > 0)
748 {
749 return this->distance_transform_order_ == k;
750 }
751
752 // Setup the kth-order transform.
753 rgtl_octree_objects_distance_transform<D> dt(this, k);
754
755 // Initialize the transform front.
756 dt.initialize_front(cell_location_type(), cell_index_type());
757
758 // Execute the entire transform. Disable debugging output for
759 // closest object computation during the transform.
760 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
761 bool old = this->query_closest_debug_;
762 this->query_closest_debug_ = false;
763 #endif
764 bool success = dt.execute_transform();
765 #ifdef RGTL_OCTREE_OBJECTS_NODE_DT
766 success = success && dt.transform_nodes();
767 #endif
768 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
769 this->query_closest_debug_ = old;
770 #endif
771
772 // If the transform succeeded store its order to enable use during
773 // closest object queries.
774 if (success)
775 {
776 this->distance_transform_order_ = k;
777 return true;
778 }
779 else
780 {
781 return false;
782 }
783 }
784
785 //----------------------------------------------------------------------------
786 template <unsigned int D>
787 void
788 rgtl_octree_objects_distance_transform<D>
initialize_front(cell_location_type const & cell,cell_index_type cidx)789 ::initialize_front(cell_location_type const& cell, cell_index_type cidx)
790 {
791 if (this->tree_.has_children(cidx))
792 {
793 // Recursively bound the children.
794 for (child_index_type i(0); i < (1<<D); ++i)
795 {
796 this->initialize_front(cell.get_child(i),
797 this->tree_.get_child(cidx, i));
798 }
799 }
800 else if (leaf_data_type const* leaf_data =
801 this->tree_.get_leaf_data(cidx))
802 {
803 // The front is initialized with non-empty leaves.
804 if (leaf_data->index_begin != leaf_data->index_end)
805 {
806 double bound;
807 if (leaf_data->index_end - leaf_data->index_begin >= k_)
808 {
809 // The initial bound is the half-diagonal length because the
810 // leaf is known to contain at least k objects in its volume.
811 rgtl_octree_cell_bounds<D> cell_bounds;
812 cell_bounds.compute_bounds(this->objects_.bounds_, cell);
813 double half = cell_bounds.size() / 2;
814 bound = std::sqrt(half*half*D);
815 }
816 else
817 {
818 // The initial bound is infinity because the leaf does not
819 // contain at least k objects.
820 bound = std::numeric_limits<double>::infinity();
821 }
822
823 // Update the leaf data with this bound.
824 leaf_data_type new_leaf(-bound,
825 leaf_data->index_begin, leaf_data->index_end);
826 this->tree_.set_leaf_data(cidx, &new_leaf);
827
828 // Create the front entry.
829 entry_type entry(cell, cidx, bound);
830 typename front_type::iterator ei = this->front_.insert(entry);
831 this->front_index_map_[cidx] = ei;
832 }
833 }
834 }
835
836 //----------------------------------------------------------------------------
837 template <unsigned int D>
838 bool
839 rgtl_octree_objects_distance_transform<D>
execute_transform()840 ::execute_transform()
841 {
842 // Loop until the front is empty.
843 while (!this->front_.empty())
844 {
845 // Get the next entry in the front.
846 entry_type entry = *this->front_.begin();
847
848 // Erase the entry from the front.
849 this->front_.erase(this->front_.begin());
850 this->front_index_map_.erase(entry.cell_index);
851
852 // Compute the true distance for this entry's cell center.
853 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_FRONT
854 std::cout << "Checking front entry cell " << entry.location
855 << " with bound " << entry.Bound << std::endl;
856 #endif
857 double squared_bound = entry.Bound*entry.Bound;
858 double* squared_distances = &this->squared_distances_[0];
859 double center[D];
860 this->objects_.compute_center(entry.location, center);
861 int n = this->objects_.query_closest_impl(center, this->k_, squared_bound,
862 0, squared_distances, 0);
863 if (n != this->k_)
864 {
865 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_FRONT
866 std::cout << "Could not find " << this->k_
867 << " closest object(s)." << std::endl;
868 #endif
869 return false;
870 }
871 double distance = std::sqrt(squared_distances[n-1]);
872 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_FRONT
873 std::cout << "Found distance " << distance << std::endl;
874 #endif
875
876 // Update the leaf data with this distance.
877 leaf_data_type const* old_leaf =
878 this->tree_.get_leaf_data(entry.cell_index);
879 leaf_data_type new_leaf(distance,
880 old_leaf->index_begin, old_leaf->index_end);
881 this->tree_.set_leaf_data(entry.cell_index, &new_leaf);
882
883 // Propagate bounds to adjacent cells based on this distance.
884 this->propagate_front(entry.location, center, distance);
885 }
886
887 return true;
888 }
889
890 //----------------------------------------------------------------------------
891 template <unsigned int D>
892 void
893 rgtl_octree_objects_distance_transform<D>
propagate_front(cell_location_type const & cell,double const center[D],double distance)894 ::propagate_front(cell_location_type const& cell,
895 double const center[D],
896 double distance)
897 {
898 // Propagate a distance bound through each face of the cell.
899 for (unsigned int face = 0; face < 2*D; ++face)
900 {
901 // Get the neighbor through this face.
902 cell_location_type neighbor;
903 cell_index_type nidx;
904 if (this->tree_.get_neighbor(cell, face, neighbor, nidx))
905 {
906 this->propagate_front(center, distance, face, neighbor, nidx);
907 }
908 }
909 }
910
911 //----------------------------------------------------------------------------
912 template <unsigned int D>
913 void
914 rgtl_octree_objects_distance_transform<D>
propagate_front(double const from_center[D],double distance,unsigned int face,cell_location_type const & cell,cell_index_type cidx)915 ::propagate_front(double const from_center[D],
916 double distance,
917 unsigned int face,
918 cell_location_type const& cell,
919 cell_index_type cidx)
920 {
921 if (this->tree_.has_children(cidx))
922 {
923 // Recurse to the children that are the real face neighbors of the
924 // original cell.
925 unsigned int axis = face >> 1;
926 unsigned int side = (face & 1) ^ 1;
927
928 // The bit corresponding to the face axis is fixed to one side.
929 // The other bits come from i.
930 unsigned int lower_mask = ((1<<axis)-1);
931 unsigned int middle_bit = (side << axis);
932 unsigned int upper_mask = ((1<<(D-1))-1) ^ lower_mask;
933 for (unsigned int i=0; i < (1<<(D-1)); ++i)
934 {
935 child_index_type child_index =
936 child_index_type(((upper_mask & i) << 1) |
937 middle_bit |
938 (lower_mask & i));
939 this->propagate_front(from_center, distance, face,
940 cell.get_child(child_index),
941 this->tree_.get_child(cidx, child_index));
942 }
943 }
944 else if (leaf_data_type const* old_leaf = this->tree_.get_leaf_data(cidx))
945 {
946 // This is a leaf to which we may propagate a bound and update the
947 // front.
948 if (old_leaf->distance >= 0)
949 {
950 // This leaf already has the true distance.
951 return;
952 }
953
954 // Compute the propagated bound.
955 double to_center[D];
956 this->objects_.compute_center(cell, to_center);
957 double distance_squared =
958 this->objects_.compute_distance_squared(to_center, from_center);
959 double bound = distance + std::sqrt(distance_squared);
960
961 // Propagate the bound if it is smaller. Note that tentative
962 // bounds are stored in the leaves as negative distances.
963 if (bound < -old_leaf->distance)
964 {
965 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_FRONT
966 std::cout << "Updating bound of leaf " << cell
967 << " with " << bound << " < " << (-old_leaf->distance)
968 << std::endl;
969 #endif
970 // Update the leaf data with the new bound.
971 leaf_data_type new_leaf(-bound,
972 old_leaf->index_begin, old_leaf->index_end);
973 this->tree_.set_leaf_data(cidx, &new_leaf);
974
975 // Update the front entry for the leaf. Note that we must
976 // remove the old entry and insert a new entry to be sure the
977 // ordering of the front is updated properly.
978 entry_type entry(cell, cidx, bound);
979 typename front_type::iterator ei = this->front_.insert(entry);
980 typename front_index_map_type::iterator mi =
981 this->front_index_map_.find(cidx);
982 if (mi == this->front_index_map_.end())
983 {
984 // The leaf was not already in the front. Create a front
985 // index for the new entry.
986 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_FRONT
987 std::cout << "Creating new front entry." << std::endl;
988 #endif
989 this->front_index_map_[cidx] = ei;
990 }
991 else
992 {
993 // The leaf was already in the front. Remove the old entry
994 // and update the front index for the new entry.
995 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_FRONT
996 std::cout << "Replacing old front entry." << std::endl;
997 #endif
998 this->front_.erase(mi->second);
999 mi->second = ei;
1000 }
1001 }
1002 }
1003 }
1004
1005 #ifdef RGTL_OCTREE_OBJECTS_NODE_DT
1006 //----------------------------------------------------------------------------
1007 template <unsigned int D>
1008 bool
1009 rgtl_octree_objects_distance_transform<D>
transform_nodes()1010 ::transform_nodes()
1011 {
1012 double half = this->objects_.bounds_.size()/2;
1013 double half_diagonal = std::sqrt(D*half*half);
1014 double bound = 0;
1015 return this->transform_nodes(cell_location_type(), cell_index_type(),
1016 half_diagonal, bound);
1017 }
1018
1019 //----------------------------------------------------------------------------
1020 template <unsigned int D>
1021 bool
1022 rgtl_octree_objects_distance_transform<D>
transform_nodes(cell_location_type const & cell,cell_index_type cidx,double half_diagonal,double & parent_bound)1023 ::transform_nodes(cell_location_type const& cell, cell_index_type cidx,
1024 double half_diagonal, double& parent_bound)
1025 {
1026 double distance;
1027 if (this->tree_.has_children(cidx))
1028 {
1029 // Recursively transform the children.
1030 double bound = std::numeric_limits<double>::infinity();
1031 for (child_index_type i(0); i < (1<<D); ++i)
1032 {
1033 if (!this->transform_nodes(cell.get_child(i),
1034 this->tree_.get_child(cidx, i),
1035 half_diagonal/2, bound))
1036 {
1037 return false;
1038 }
1039 }
1040
1041 // We now have a bound provided by our children. Use it to
1042 // compute this node center distance.
1043 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_FRONT
1044 std::cout << "Checking node " << cell
1045 << " with bound " << bound << std::endl;
1046 #endif
1047 double squared_bound = bound*bound;
1048 double squared_distance = -1;
1049 double* squared_distances = &this->squared_distances_[0];
1050 double center[D];
1051 this->objects_.compute_center(cell, center);
1052 int n = this->objects_.query_closest_impl(center, this->k_, squared_bound,
1053 0, squared_distances, 0);
1054 if (n != this->k_)
1055 {
1056 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_FRONT
1057 std::cout << "Could not find " << this->k_
1058 << " closest object(s)." << std::endl;
1059 #endif
1060 return false;
1061 }
1062 distance = std::sqrt(squared_distances[n-1]);
1063 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_FRONT
1064 std::cout << "Found distance " << distance << std::endl;
1065 #endif
1066
1067 // Store this distance in the node.
1068 node_data_type new_node(distance);
1069 this->tree_.set_node_data(cidx, &new_node);
1070 }
1071 else if (leaf_data_type const* leaf_data =
1072 this->tree_.get_leaf_data(cidx))
1073 {
1074 // Check this leaf.
1075 if (leaf_data->distance < 0)
1076 {
1077 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_FRONT
1078 std::cout << "No distance for leaf " << cell << std::endl;
1079 #endif
1080 return false;
1081 }
1082 else
1083 {
1084 // Use the distance provided by this leaf.
1085 distance = leaf_data->distance;
1086 }
1087 }
1088 else
1089 {
1090 // There is no leaf!
1091 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_FRONT
1092 std::cout << "No data for leaf " << cell << std::endl;
1093 #endif
1094 return false;
1095 }
1096
1097 // Bound the distance for our parent cell center using our distance.
1098 double bound = distance + half_diagonal;
1099 if (bound < parent_bound)
1100 {
1101 parent_bound = bound;
1102 }
1103
1104 return true;
1105 }
1106 #endif
1107
1108 //----------------------------------------------------------------------------
1109 template <unsigned int D>
1110 rgtl_octree_objects_query_closest<D>
rgtl_octree_objects_query_closest(internal_type const & intern,int k)1111 ::rgtl_octree_objects_query_closest(internal_type const& intern, int k):
1112 internal_(intern), best_(k), bound_(0)
1113 {
1114 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
1115 this->checked_count_ = 0;
1116 #endif
1117 }
1118
1119 //----------------------------------------------------------------------------
1120 template <unsigned int D>
1121 int
1122 rgtl_octree_objects_query_closest<D>
query(double const p[D],int k,double bound,int * ids,double * squared_distances,double * points)1123 ::query(double const p[D], int k, double bound,
1124 int* ids, double* squared_distances, double* points)
1125 {
1126 // Store the initial squared distance bound.
1127 this->bound_ = bound;
1128
1129 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
1130 if (this->internal_.query_closest_debug_)
1131 {
1132 std::cout << "Querying point (";
1133 const char* sep = "";
1134 for (unsigned int a=0; a < D; ++a)
1135 {
1136 std::cout << sep << p[a];
1137 sep = ", ";
1138 }
1139 std::cout << ") with initial bound " << this->bound_ << std::endl;
1140 }
1141 #endif
1142
1143 // Keep track objects already tested to avoid duplicating tests.
1144 this->internal_.object_once_.reset();
1145
1146 // Recursively visit the tree starting at the root.
1147 this->query_impl(cell_location_type(), cell_index_type(), p, k);
1148
1149 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
1150 if (this->internal_.query_closest_debug_)
1151 {
1152 std::cout << " Checked " << this->checked_count_
1153 << " of " << this->internal_.number_of_objects()
1154 << " objects." << std::endl;
1155 }
1156 #endif
1157
1158 // Copy the final object id list to the output.
1159 std::vector<closest_object_entry>& best = this->best_;
1160 for (int i=0; i < k; ++i)
1161 {
1162 if (best[i].distance_squared >= 0)
1163 {
1164 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
1165 if (this->internal_.query_closest_debug_)
1166 {
1167 std::cout << i
1168 << ": id " << best[i].id
1169 << ", d " << std::sqrt(best[i].distance_squared) << std::endl;
1170 }
1171 #endif
1172 if (ids)
1173 {
1174 ids[i] = best[i].id;
1175 }
1176 if (squared_distances)
1177 {
1178 squared_distances[i] = best[i].distance_squared;
1179 }
1180 if (points)
1181 {
1182 for (unsigned int a=0; a < D; ++a)
1183 {
1184 points[i*D+a] = best[i].point[a];
1185 }
1186 }
1187 }
1188 else
1189 {
1190 return i;
1191 }
1192 }
1193 return k;
1194 }
1195
1196 //----------------------------------------------------------------------------
1197 template <unsigned int D>
1198 void
1199 rgtl_octree_objects_query_closest<D>
query_impl(cell_location_type const & cell,cell_index_type cell_index,double const p[D],int k)1200 ::query_impl(cell_location_type const& cell, cell_index_type cell_index,
1201 double const p[D], int k)
1202 {
1203 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
1204 if (this->internal_.query_closest_debug_)
1205 {
1206 std::cout << "Considering cell " << cell << std::endl;
1207 }
1208 #endif
1209
1210 // Make sure the cell intersects the current bounding sphere.
1211 double nearest_squared;
1212 this->internal_.cell_nearest(cell, p, nearest_squared);
1213 if (nearest_squared > this->bound_)
1214 {
1215 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
1216 if (this->internal_.query_closest_debug_)
1217 {
1218 std::cout << " cell is out of range: " << nearest_squared
1219 << " > " << this->bound_ << std::endl;
1220 }
1221 #endif
1222 return;
1223 }
1224
1225 #ifdef RGTL_OCTREE_OBJECTS_NODE_DT
1226 // Compute the cell center.
1227 double center[D];
1228 this->internal_.compute_center(cell, center);
1229 double center_distance = std::numeric_limits<double>::infinity();
1230 #endif
1231
1232 // Check whether this is a node or leaf.
1233 bool is_node = this->internal_.tree_.has_children(cell_index);
1234 leaf_data_type const* ld = 0;
1235 if (is_node)
1236 {
1237 #ifdef RGTL_OCTREE_OBJECTS_NODE_DT
1238 // Check if this node center provides a distance.
1239 if (node_data_type const* nd =
1240 this->internal_.tree_.get_node_data(cell_index))
1241 {
1242 center_distance = nd->distance;
1243 }
1244 #endif
1245 }
1246 else
1247 {
1248 // This is a leaf. Lookup the data.
1249 ld = this->internal_.tree_.get_leaf_data(cell_index);
1250
1251 #ifdef RGTL_OCTREE_OBJECTS_NODE_DT
1252 // Check if this leaf center provides a distance.
1253 if (ld)
1254 {
1255 center_distance = ld->distance;
1256 }
1257 #endif
1258 }
1259
1260 #ifdef RGTL_OCTREE_OBJECTS_NODE_DT
1261 // Try to reduce the current bound using the distance transform
1262 // result for this cell.
1263 if (k <= this->internal_.distance_transform_order_ &&
1264 (center_distance*center_distance) < this->bound_)
1265 {
1266 double distance_squared =
1267 this->internal_.compute_distance_squared(center, p);
1268 if (distance_squared < this->bound_)
1269 {
1270 // Compute a bound on the distance by adding the distance from
1271 // the query point to the cell center and the distance from
1272 // the cell center to its nearest object.
1273 double bound = std::fabs(center_distance) + std::sqrt(distance_squared);
1274
1275 // Shrink the squared distance bound if possible.
1276 // Increase the computed potential bound by a small fraction
1277 // in order to avoid missing an object altogether due to
1278 // rounding problems.
1279 double squared_bound = bound*bound*1.00001;
1280 if (squared_bound < this->bound_)
1281 {
1282 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
1283 if (this->internal_.query_closest_debug_)
1284 {
1285 std::cout << "Reduced current bound "
1286 << squared_bound << " < " << this->bound_
1287 << " using cell " << cell << std::endl;
1288 }
1289 #endif
1290 this->bound_ = squared_bound;
1291 }
1292 }
1293 }
1294 #endif
1295
1296 if (is_node)
1297 {
1298 // Visit the children closer to the query point first.
1299 #ifndef RGTL_OCTREE_OBJECTS_NODE_DT
1300 double center[D];
1301 this->internal_.compute_center(cell, center);
1302 #endif
1303 unsigned int child_xor = 0;
1304 for (unsigned int a=0; a < D; ++a)
1305 {
1306 if (p[a] >= center[a])
1307 {
1308 child_xor |= (1<<a);
1309 }
1310 }
1311
1312 // The cell is divided, so visit the children.
1313 for (unsigned int l = 0; l < (1<<D); ++l)
1314 {
1315 child_index_type i(l^child_xor);
1316 this->query_impl(cell.get_child(i),
1317 this->internal_.tree_.get_child(cell_index, i), p, k);
1318 }
1319 }
1320 else if (ld)
1321 {
1322 // This is a leaf. Look for objects.
1323 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
1324 if (this->internal_.query_closest_debug_)
1325 {
1326 if (ld->index_begin < ld->index_end)
1327 {
1328 std::cout << " checking objects" << std::endl;
1329 }
1330 else
1331 {
1332 std::cout << " no objects" << std::endl;
1333 }
1334 }
1335 #endif
1336 // Loop over the objects in this cell. When the objects are not
1337 // points it is possible that an object will provide a point
1338 // that is closer to the query than any point in the cell. If
1339 // this occurs we can stop testing objects in the cell.
1340 typedef typename leaf_data_type::index_type index_type;
1341 for (index_type i=ld->index_begin;
1342 i < ld->index_end && nearest_squared <= this->bound_; ++i)
1343 {
1344 // Test each object at most once.
1345 int id = this->internal_.object_ids_[i];
1346 if (this->internal_.object_once_.visit(id))
1347 {
1348 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
1349 ++checked_count_;
1350 #endif
1351
1352 // Get the squared distance to this object and use it only
1353 // if it is smaller than the current distance bound.
1354 double q[D];
1355 double distance_squared;
1356 if (this->internal_.object_closest_point(id, p, q, this->bound_) &&
1357 (distance_squared = this->internal_.compute_distance_squared(p, q),
1358 distance_squared <= this->bound_))
1359 {
1360 // Insert this object into our sorted list of k objects.
1361 std::vector<closest_object_entry>& best = this->best_;
1362 closest_object_entry obj(id, distance_squared, q);
1363 for (int j=0; j < k && obj.distance_squared >= 0; ++j)
1364 {
1365 if (best[j].distance_squared < 0 || obj < best[j])
1366 {
1367 std::swap(obj, best[j]);
1368 }
1369 }
1370
1371 // Update the current bounding sphere radius.
1372 if (best[k-1].distance_squared >= 0 &&
1373 best[k-1].distance_squared < this->bound_)
1374 {
1375 this->bound_ = best[k-1].distance_squared;
1376 #ifdef RGTL_OCTREE_OBJECTS_DEBUG_QUERY
1377 if (this->internal_.query_closest_debug_)
1378 {
1379 std::cout << " Reduced bound to " << this->bound_ << std::endl;
1380 if (nearest_squared > this->bound_ && i < ld->index_end-1)
1381 {
1382 std::cout << " Terminating cell early!" << std::endl;
1383 }
1384 }
1385 #endif
1386 }
1387 }
1388 }
1389 }
1390 }
1391 }
1392
1393 //----------------------------------------------------------------------------
1394 template <unsigned int D>
1395 int
1396 rgtl_octree_objects_internal<D>
query_sphere(double const center[D],double radius,std::vector<int> & ids) const1397 ::query_sphere(double const center[D], double radius,
1398 std::vector<int>& ids) const
1399 {
1400 // Reset the object visitation marks.
1401 this->object_once_.reset();
1402
1403 // Query the tree recursively.
1404 this->query_sphere(cell_location_type(), cell_index_type(),
1405 center, radius*radius, ids);
1406
1407 // Return the number of objects found.
1408 return static_cast<int>(ids.size());
1409 }
1410
1411 //----------------------------------------------------------------------------
1412 template <unsigned int D>
1413 void
1414 rgtl_octree_objects_internal<D>
query_sphere(cell_location_type const & cell,cell_index_type cidx,double const p[D],double radius_squared,std::vector<int> & ids) const1415 ::query_sphere(cell_location_type const& cell,
1416 cell_index_type cidx,
1417 double const p[D],
1418 double radius_squared,
1419 std::vector<int>& ids) const
1420 {
1421 // Compute the squared magnitude of vectors pointing from the sphere
1422 // center to the nearest and farthest points in the volume of the
1423 // cell.
1424 double nearest_squared;
1425 double farthest_squared;
1426 this->cell_distances(cell, p, nearest_squared, farthest_squared);
1427
1428 // If the radius of the sphere is less than the nearest point on the
1429 // cell then the entire cell is outside the sphere.
1430 if (radius_squared < nearest_squared)
1431 {
1432 // The sphere contains no part of the cell. Do nothing.
1433 }
1434 // If the radius of the sphere is greater than the farthest point on
1435 // the cell then the entire cell is contained within the sphere.
1436 else if (radius_squared > farthest_squared)
1437 {
1438 // The sphere completely contains the cell. Extract objects in
1439 // the cell and its children. By not passing the center or radius
1440 // to this method it will not bother testing the objects since
1441 // they are known to intersect the cell, which is inside the
1442 // sphere.
1443 this->extract_objects(cidx, 0, 0, ids);
1444 }
1445 // If the radius of the sphere is between the length of the nearest
1446 // and farthest vectors then the surface of the sphere must pass
1447 // through the cell.
1448 else
1449 {
1450 if (this->tree_.has_children(cidx))
1451 {
1452 // The cell is divided, so evaluate the children recursively.
1453 // Some of them may be completely inside or outside the sphere.
1454 for (child_index_type i(0); i < (1<<D); ++i)
1455 {
1456 this->query_sphere(cell.get_child(i),
1457 this->tree_.get_child(cidx, i),
1458 p, radius_squared, ids);
1459 }
1460 }
1461 else
1462 {
1463 // The cell is not divided. Extract the objects within the sphere.
1464 this->extract_objects(cidx, p, radius_squared, ids);
1465 }
1466 }
1467 }
1468
1469 //----------------------------------------------------------------------------
1470 template <unsigned int D>
1471 void
1472 rgtl_octree_objects_internal<D>
extract_objects(cell_index_type cidx,double const * center,double radius_squared,std::vector<int> & ids) const1473 ::extract_objects(cell_index_type cidx,
1474 double const* center, double radius_squared,
1475 std::vector<int>& ids) const
1476 {
1477 if (this->tree_.has_children(cidx))
1478 {
1479 // Extract objects from all children.
1480 for (child_index_type i(0); i < (1<<D); ++i)
1481 {
1482 this->extract_objects(this->tree_.get_child(cidx, i), center,
1483 radius_squared, ids);
1484 }
1485 }
1486 else if (leaf_data_type const* ld = this->tree_.get_leaf_data(cidx))
1487 {
1488 // Extract objects from this leaf.
1489 typedef typename leaf_data_type::index_type index_type;
1490 for (index_type i=ld->index_begin; i < ld->index_end; ++i)
1491 {
1492 int id = this->object_ids_[i];
1493 if (this->object_once_.visit(id))
1494 {
1495 // If a sphere is given test the object against it.
1496 if (center)
1497 {
1498 double q[D];
1499 if (this->object_closest_point(id, center, q, radius_squared))
1500 {
1501 double distance_squared =
1502 this->compute_distance_squared(center, q);
1503 if (distance_squared <= radius_squared)
1504 {
1505 ids.push_back(id);
1506 }
1507 }
1508 }
1509 else
1510 {
1511 ids.push_back(id);
1512 }
1513 }
1514 }
1515 }
1516 }
1517
1518 //----------------------------------------------------------------------------
1519 template <unsigned int D>
1520 int
1521 rgtl_octree_objects_internal<D>
query_object(int id,std::vector<int> & ids) const1522 ::query_object(int id, std::vector<int>& ids) const
1523 {
1524 // Reset the object visitation marks.
1525 this->object_once_.reset();
1526
1527 // Query the tree recursively.
1528 this->query_object(cell_location_type(), cell_index_type(), id, ids);
1529
1530 // Return the number of objects found.
1531 return static_cast<int>(ids.size());
1532 }
1533
1534 //----------------------------------------------------------------------------
1535 template <unsigned int D>
1536 void
1537 rgtl_octree_objects_internal<D>
query_object(cell_location_type const & cell,cell_index_type cidx,int idA,std::vector<int> & ids) const1538 ::query_object(cell_location_type const& cell, cell_index_type cidx,
1539 int idA, std::vector<int>& ids) const
1540 {
1541 // Get the bounding box of this cell.
1542 cell_geometry_type cell_geometry(cell, this->bounds_);
1543 if (!this->object_intersects_box(idA, cell_geometry))
1544 {
1545 return;
1546 }
1547
1548 // Search this cell for objects.
1549 if (this->tree_.has_children(cidx))
1550 {
1551 // Recursively search the children.
1552 for (child_index_type i(0); i < (1<<D); ++i)
1553 {
1554 this->query_object(cell.get_child(i),
1555 this->tree_.get_child(cidx, i), idA, ids);
1556 }
1557 }
1558 else if (leaf_data_type const* ld = this->tree_.get_leaf_data(cidx))
1559 {
1560 // Objects in this leaf are candidates.
1561 typedef typename leaf_data_type::index_type index_type;
1562 for (index_type i=ld->index_begin; i < ld->index_end; ++i)
1563 {
1564 int idB = this->object_ids_[i];
1565 if (this->object_once_.visit(idB))
1566 {
1567 // Check this object for intersection with the query object.
1568 // Skip the query object itself.
1569 if (idA != idB &&
1570 this->object_intersects_object(idA, idB))
1571 {
1572 ids.push_back(idB);
1573 }
1574 }
1575 }
1576 }
1577 }
1578
1579 //----------------------------------------------------------------------------
1580 template <unsigned int D>
rgtl_octree_objects(object_array_type const & oa)1581 rgtl_octree_objects<D>::rgtl_octree_objects(object_array_type const& oa):
1582 internal_(new internal_type(oa))
1583 {
1584 }
1585
1586 //----------------------------------------------------------------------------
1587 template <unsigned int D>
rgtl_octree_objects(object_array_type const & objs,bounds_type const & b,int ml)1588 rgtl_octree_objects<D>::rgtl_octree_objects(object_array_type const& objs,
1589 bounds_type const& b, int ml)
1590 {
1591 this->internal_ = new internal_type(objs, b, ml);
1592 }
1593
1594 //----------------------------------------------------------------------------
1595 template <unsigned int D>
~rgtl_octree_objects()1596 rgtl_octree_objects<D>::~rgtl_octree_objects()
1597 {
1598 delete this->internal_;
1599 }
1600
1601 //----------------------------------------------------------------------------
1602 template <unsigned int D>
1603 int
1604 rgtl_octree_objects<D>
query_sphere(double const center[D],double radius,std::vector<int> & ids) const1605 ::query_sphere(double const center[D], double radius,
1606 std::vector<int>& ids) const
1607 {
1608 return this->internal_->query_sphere(center, radius, ids);
1609 }
1610
1611 //----------------------------------------------------------------------------
1612 template <unsigned int D>
1613 int
1614 rgtl_octree_objects<D>
query_object(int id,std::vector<int> & ids) const1615 ::query_object(int id, std::vector<int>& ids) const
1616 {
1617 return this->internal_->query_object(id, ids);
1618 }
1619
1620 //----------------------------------------------------------------------------
1621 template <unsigned int D>
1622 int
1623 rgtl_octree_objects<D>
query_closest(double const p[D],int k,int * ids,double * squared_distances,double * points,double bound_squared) const1624 ::query_closest(double const p[D], int k, int* ids, double* squared_distances,
1625 double* points, double bound_squared) const
1626 {
1627 return this->internal_->query_closest(p, k, bound_squared,
1628 ids, squared_distances, points);
1629 }
1630
1631 //----------------------------------------------------------------------------
1632 template <unsigned int D>
1633 bool
1634 rgtl_octree_objects<D>
compute_distance_transform(int n) const1635 ::compute_distance_transform(int n) const
1636 {
1637 return this->internal_->compute_distance_transform(n);
1638 }
1639
1640 //----------------------------------------------------------------------------
1641 template <unsigned int D>
1642 void
1643 rgtl_octree_objects<D>
set_query_closest_debug(bool b)1644 ::set_query_closest_debug(bool b)
1645 {
1646 this->internal_->set_query_closest_debug(b);
1647 }
1648
1649 //----------------------------------------------------------------------------
1650 template <unsigned int D>
1651 template <class Serializer>
serialize(Serializer & sr)1652 void rgtl_octree_objects<D>::serialize(Serializer& sr)
1653 {
1654 sr & *(this->internal_);
1655 }
1656
1657 //----------------------------------------------------------------------------
1658 #undef RGTL_OCTREE_OBJECTS_INSTANTIATE
1659 #define RGTL_OCTREE_OBJECTS_INSTANTIATE( D ) \
1660 template class rgtl_octree_data_fixed< D , rgtl_octree_objects_leaf_data>; \
1661 template class rgtl_octree_objects_distance_transform< D >; \
1662 template class rgtl_octree_objects_internal< D >; \
1663 template class rgtl_octree_objects< D >; \
1664 template void rgtl_octree_objects< D > \
1665 ::serialize<rgtl_serialize_ostream>(rgtl_serialize_ostream&); \
1666 template void rgtl_octree_objects< D > \
1667 ::serialize<rgtl_serialize_istream>(rgtl_serialize_istream&)
1668
1669 #endif
1670