KdBVH.h
Go to the documentation of this file.
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2009 Ilya Baran <ibaran@mit.edu>
5 //
6 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9 
10 #ifndef KDBVH_H_INCLUDED
11 #define KDBVH_H_INCLUDED
12 
13 namespace Eigen {
14 
15 namespace internal {
16 
17 //internal pair class for the BVH--used instead of std::pair because of alignment
18 template<typename Scalar, int Dim>
20 {
22  typedef Matrix<Scalar, Dim, 1> VectorType;
23 
24  vector_int_pair(const VectorType &v, int i) : first(v), second(i) {}
25 
27  int second;
28 };
29 
30 //these templates help the tree initializer get the bounding boxes either from a provided
31 //iterator range or using bounding_box in a unified way
32 template<typename ObjectList, typename VolumeList, typename BoxIter>
34  void operator()(const ObjectList &objects, BoxIter boxBegin, BoxIter boxEnd, VolumeList &outBoxes)
35  {
36  outBoxes.insert(outBoxes.end(), boxBegin, boxEnd);
37  eigen_assert(outBoxes.size() == objects.size());
39  }
40 };
41 
42 template<typename ObjectList, typename VolumeList>
43 struct get_boxes_helper<ObjectList, VolumeList, int> {
44  void operator()(const ObjectList &objects, int, int, VolumeList &outBoxes)
45  {
46  outBoxes.reserve(objects.size());
47  for(int i = 0; i < (int)objects.size(); ++i)
48  outBoxes.push_back(bounding_box(objects[i]));
49  }
50 };
51 
52 } // end namespace internal
53 
54 
68 template<typename _Scalar, int _Dim, typename _Object> class KdBVH
69 {
70 public:
71  enum { Dim = _Dim };
72  typedef _Object Object;
73  typedef std::vector<Object, aligned_allocator<Object> > ObjectList;
74  typedef _Scalar Scalar;
76  typedef std::vector<Volume, aligned_allocator<Volume> > VolumeList;
77  typedef int Index;
78  typedef const int *VolumeIterator; //the iterators are just pointers into the tree's vectors
79  typedef const Object *ObjectIterator;
80 
81  KdBVH() {}
82 
84  template<typename Iter> KdBVH(Iter begin, Iter end) { init(begin, end, 0, 0); } //int is recognized by init as not being an iterator type
85 
87  template<typename OIter, typename BIter> KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) { init(begin, end, boxBegin, boxEnd); }
88 
91  template<typename Iter> void init(Iter begin, Iter end) { init(begin, end, 0, 0); }
92 
95  template<typename OIter, typename BIter> void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd)
96  {
97  objects.clear();
98  boxes.clear();
99  children.clear();
100 
101  objects.insert(objects.end(), begin, end);
102  int n = static_cast<int>(objects.size());
103 
104  if(n < 2)
105  return; //if we have at most one object, we don't need any internal nodes
106 
107  VolumeList objBoxes;
108  VIPairList objCenters;
109 
110  //compute the bounding boxes depending on BIter type
111  internal::get_boxes_helper<ObjectList, VolumeList, BIter>()(objects, boxBegin, boxEnd, objBoxes);
112 
113  objCenters.reserve(n);
114  boxes.reserve(n - 1);
115  children.reserve(2 * n - 2);
116 
117  for(int i = 0; i < n; ++i)
118  objCenters.push_back(VIPair(objBoxes[i].center(), i));
119 
120  build(objCenters, 0, n, objBoxes, 0); //the recursive part of the algorithm
121 
122  ObjectList tmp(n);
123  tmp.swap(objects);
124  for(int i = 0; i < n; ++i)
125  objects[i] = tmp[objCenters[i].second];
126  }
127 
129  inline Index getRootIndex() const { return (int)boxes.size() - 1; }
130 
133  EIGEN_STRONG_INLINE void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd,
134  ObjectIterator &outOBegin, ObjectIterator &outOEnd) const
135  { //inlining this function should open lots of optimization opportunities to the compiler
136  if(index < 0) {
137  outVBegin = outVEnd;
138  if(!objects.empty())
139  outOBegin = &(objects[0]);
140  outOEnd = outOBegin + objects.size(); //output all objects--necessary when the tree has only one object
141  return;
142  }
143 
144  int numBoxes = static_cast<int>(boxes.size());
145 
146  int idx = index * 2;
147  if(children[idx + 1] < numBoxes) { //second index is always bigger
148  outVBegin = &(children[idx]);
149  outVEnd = outVBegin + 2;
150  outOBegin = outOEnd;
151  }
152  else if(children[idx] >= numBoxes) { //if both children are objects
153  outVBegin = outVEnd;
154  outOBegin = &(objects[children[idx] - numBoxes]);
155  outOEnd = outOBegin + 2;
156  } else { //if the first child is a volume and the second is an object
157  outVBegin = &(children[idx]);
158  outVEnd = outVBegin + 1;
159  outOBegin = &(objects[children[idx + 1] - numBoxes]);
160  outOEnd = outOBegin + 1;
161  }
162  }
163 
165  inline const Volume &getVolume(Index index) const
166  {
167  return boxes[index];
168  }
169 
170 private:
172  typedef std::vector<VIPair, aligned_allocator<VIPair> > VIPairList;
174  struct VectorComparator //compares vectors, or more specifically, VIPairs along a particular dimension
175  {
176  VectorComparator(int inDim) : dim(inDim) {}
177  inline bool operator()(const VIPair &v1, const VIPair &v2) const { return v1.first[dim] < v2.first[dim]; }
178  int dim;
179  };
180 
181  //Build the part of the tree between objects[from] and objects[to] (not including objects[to]).
182  //This routine partitions the objCenters in [from, to) along the dimension dim, recursively constructs
183  //the two halves, and adds their parent node. TODO: a cache-friendlier layout
184  void build(VIPairList &objCenters, int from, int to, const VolumeList &objBoxes, int dim)
185  {
186  eigen_assert(to - from > 1);
187  if(to - from == 2) {
188  boxes.push_back(objBoxes[objCenters[from].second].merged(objBoxes[objCenters[from + 1].second]));
189  children.push_back(from + (int)objects.size() - 1); //there are objects.size() - 1 tree nodes
190  children.push_back(from + (int)objects.size());
191  }
192  else if(to - from == 3) {
193  int mid = from + 2;
194  std::nth_element(objCenters.begin() + from, objCenters.begin() + mid,
195  objCenters.begin() + to, VectorComparator(dim)); //partition
196  build(objCenters, from, mid, objBoxes, (dim + 1) % Dim);
197  int idx1 = (int)boxes.size() - 1;
198  boxes.push_back(boxes[idx1].merged(objBoxes[objCenters[mid].second]));
199  children.push_back(idx1);
200  children.push_back(mid + (int)objects.size() - 1);
201  }
202  else {
203  int mid = from + (to - from) / 2;
204  nth_element(objCenters.begin() + from, objCenters.begin() + mid,
205  objCenters.begin() + to, VectorComparator(dim)); //partition
206  build(objCenters, from, mid, objBoxes, (dim + 1) % Dim);
207  int idx1 = (int)boxes.size() - 1;
208  build(objCenters, mid, to, objBoxes, (dim + 1) % Dim);
209  int idx2 = (int)boxes.size() - 1;
210  boxes.push_back(boxes[idx1].merged(boxes[idx2]));
211  children.push_back(idx1);
212  children.push_back(idx2);
213  }
214  }
215 
216  std::vector<int> children; //children of x are children[2x] and children[2x+1], indices bigger than boxes.size() index into objects.
217  VolumeList boxes;
218  ObjectList objects;
219 };
220 
221 } // end namespace Eigen
222 
223 #endif //KDBVH_H_INCLUDED
EIGEN_STRONG_INLINE void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd, ObjectIterator &outOBegin, ObjectIterator &outOEnd) const
Definition: KdBVH.h:133
_Scalar Scalar
Definition: KdBVH.h:74
SCALAR Scalar
Definition: bench_gemm.cpp:46
void operator()(const ObjectList &objects, int, int, VolumeList &outBoxes)
Definition: KdBVH.h:44
#define EIGEN_STRONG_INLINE
Definition: Macros.h:917
Vector v2
ObjectList objects
Definition: KdBVH.h:218
KdBVH(Iter begin, Iter end)
Definition: KdBVH.h:84
KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd)
Definition: KdBVH.h:87
Vector v1
Box2d bounding_box(const Vector2d &v)
Definition: BVH_Example.cpp:9
def build
Definition: noxfile.py:86
const Object * ObjectIterator
Definition: KdBVH.h:79
int n
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd)
Definition: KdBVH.h:95
std::vector< Volume, aligned_allocator< Volume > > VolumeList
Definition: KdBVH.h:76
_Object Object
Definition: KdBVH.h:72
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Size)
Definition: Memory.h:842
An axis aligned box.
bool operator()(const VIPair &v1, const VIPair &v2) const
Definition: KdBVH.h:177
void build(VIPairList &objCenters, int from, int to, const VolumeList &objBoxes, int dim)
Definition: KdBVH.h:184
Matrix< Scalar, Dim, 1 > VectorType
Definition: KdBVH.h:173
const Volume & getVolume(Index index) const
Definition: KdBVH.h:165
#define eigen_assert(x)
Definition: Macros.h:1037
Array< int, Dynamic, 1 > v
const int * VolumeIterator
Definition: KdBVH.h:78
internal::vector_int_pair< Scalar, Dim > VIPair
Definition: KdBVH.h:171
AlignedBox< Scalar, Dim > Volume
Definition: KdBVH.h:75
VolumeList boxes
Definition: KdBVH.h:217
int Index
Definition: KdBVH.h:77
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:1882
static EIGEN_DEPRECATED const end_t end
Index getRootIndex() const
Definition: KdBVH.h:129
The matrix class, also used for vectors and row-vectors.
A simple bounding volume hierarchy based on AlignedBox.
Definition: KdBVH.h:68
void operator()(const ObjectList &objects, BoxIter boxBegin, BoxIter boxEnd, VolumeList &outBoxes)
Definition: KdBVH.h:34
std::vector< VIPair, aligned_allocator< VIPair > > VIPairList
Definition: KdBVH.h:172
void init(Iter begin, Iter end)
Definition: KdBVH.h:91
#define EIGEN_ONLY_USED_FOR_DEBUG(x)
Definition: Macros.h:1049
std::vector< Object, aligned_allocator< Object > > ObjectList
Definition: KdBVH.h:73
std::vector< int > children
Definition: KdBVH.h:216


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:29