KdBVH.h
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 // Eigen is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // Alternatively, you can redistribute it and/or
12 // modify it under the terms of the GNU General Public License as
13 // published by the Free Software Foundation; either version 2 of
14 // the License, or (at your option) any later version.
15 //
16 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
17 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
19 // GNU General Public License for more details.
20 //
21 // You should have received a copy of the GNU Lesser General Public
22 // License and a copy of the GNU General Public License along with
23 // Eigen. If not, see <http://www.gnu.org/licenses/>.
24 
25 #ifndef KDBVH_H_INCLUDED
26 #define KDBVH_H_INCLUDED
27 
28 namespace Eigen {
29 
30 namespace internal {
31 
32 //internal pair class for the BVH--used instead of std::pair because of alignment
33 template<typename Scalar, int Dim>
34 struct vector_int_pair
35 {
36 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Dim)
37  typedef Matrix<Scalar, Dim, 1> VectorType;
38 
39  vector_int_pair(const VectorType &v, int i) : first(v), second(i) {}
40 
41  VectorType first;
42  int second;
43 };
44 
45 //these templates help the tree initializer get the bounding boxes either from a provided
46 //iterator range or using bounding_box in a unified way
47 template<typename ObjectList, typename VolumeList, typename BoxIter>
48 struct get_boxes_helper {
49  void operator()(const ObjectList &objects, BoxIter boxBegin, BoxIter boxEnd, VolumeList &outBoxes)
50  {
51  outBoxes.insert(outBoxes.end(), boxBegin, boxEnd);
52  eigen_assert(outBoxes.size() == objects.size());
53  }
54 };
55 
56 template<typename ObjectList, typename VolumeList>
57 struct get_boxes_helper<ObjectList, VolumeList, int> {
58  void operator()(const ObjectList &objects, int, int, VolumeList &outBoxes)
59  {
60  outBoxes.reserve(objects.size());
61  for(int i = 0; i < (int)objects.size(); ++i)
62  outBoxes.push_back(bounding_box(objects[i]));
63  }
64 };
65 
66 } // end namespace internal
67 
68 
82 template<typename _Scalar, int _Dim, typename _Object> class KdBVH
83 {
84 public:
85  enum { Dim = _Dim };
86  typedef _Object Object;
87  typedef std::vector<Object, aligned_allocator<Object> > ObjectList;
88  typedef _Scalar Scalar;
89  typedef AlignedBox<Scalar, Dim> Volume;
90  typedef std::vector<Volume, aligned_allocator<Volume> > VolumeList;
91  typedef int Index;
92  typedef const int *VolumeIterator; //the iterators are just pointers into the tree's vectors
93  typedef const Object *ObjectIterator;
94 
95  KdBVH() {}
96 
98  template<typename Iter> KdBVH(Iter begin, Iter end) { init(begin, end, 0, 0); } //int is recognized by init as not being an iterator type
99 
101  template<typename OIter, typename BIter> KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) { init(begin, end, boxBegin, boxEnd); }
102 
105  template<typename Iter> void init(Iter begin, Iter end) { init(begin, end, 0, 0); }
106 
109  template<typename OIter, typename BIter> void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd)
110  {
111  objects.clear();
112  boxes.clear();
113  children.clear();
114 
115  objects.insert(objects.end(), begin, end);
116  int n = static_cast<int>(objects.size());
117 
118  if(n < 2)
119  return; //if we have at most one object, we don't need any internal nodes
120 
121  VolumeList objBoxes;
122  VIPairList objCenters;
123 
124  //compute the bounding boxes depending on BIter type
125  internal::get_boxes_helper<ObjectList, VolumeList, BIter>()(objects, boxBegin, boxEnd, objBoxes);
126 
127  objCenters.reserve(n);
128  boxes.reserve(n - 1);
129  children.reserve(2 * n - 2);
130 
131  for(int i = 0; i < n; ++i)
132  objCenters.push_back(VIPair(objBoxes[i].center(), i));
133 
134  build(objCenters, 0, n, objBoxes, 0); //the recursive part of the algorithm
135 
136  ObjectList tmp(n);
137  tmp.swap(objects);
138  for(int i = 0; i < n; ++i)
139  objects[i] = tmp[objCenters[i].second];
140  }
141 
143  inline Index getRootIndex() const { return (int)boxes.size() - 1; }
144 
147  EIGEN_STRONG_INLINE void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd,
148  ObjectIterator &outOBegin, ObjectIterator &outOEnd) const
149  { //inlining this function should open lots of optimization opportunities to the compiler
150  if(index < 0) {
151  outVBegin = outVEnd;
152  if(!objects.empty())
153  outOBegin = &(objects[0]);
154  outOEnd = outOBegin + objects.size(); //output all objects--necessary when the tree has only one object
155  return;
156  }
157 
158  int numBoxes = static_cast<int>(boxes.size());
159 
160  int idx = index * 2;
161  if(children[idx + 1] < numBoxes) { //second index is always bigger
162  outVBegin = &(children[idx]);
163  outVEnd = outVBegin + 2;
164  outOBegin = outOEnd;
165  }
166  else if(children[idx] >= numBoxes) { //if both children are objects
167  outVBegin = outVEnd;
168  outOBegin = &(objects[children[idx] - numBoxes]);
169  outOEnd = outOBegin + 2;
170  } else { //if the first child is a volume and the second is an object
171  outVBegin = &(children[idx]);
172  outVEnd = outVBegin + 1;
173  outOBegin = &(objects[children[idx + 1] - numBoxes]);
174  outOEnd = outOBegin + 1;
175  }
176  }
177 
179  inline const Volume &getVolume(Index index) const
180  {
181  return boxes[index];
182  }
183 
184 private:
185  typedef internal::vector_int_pair<Scalar, Dim> VIPair;
186  typedef std::vector<VIPair, aligned_allocator<VIPair> > VIPairList;
187  typedef Matrix<Scalar, Dim, 1> VectorType;
188  struct VectorComparator //compares vectors, or, more specificall, VIPairs along a particular dimension
189  {
190  VectorComparator(int inDim) : dim(inDim) {}
191  inline bool operator()(const VIPair &v1, const VIPair &v2) const { return v1.first[dim] < v2.first[dim]; }
192  int dim;
193  };
194 
195  //Build the part of the tree between objects[from] and objects[to] (not including objects[to]).
196  //This routine partitions the objCenters in [from, to) along the dimension dim, recursively constructs
197  //the two halves, and adds their parent node. TODO: a cache-friendlier layout
198  void build(VIPairList &objCenters, int from, int to, const VolumeList &objBoxes, int dim)
199  {
200  eigen_assert(to - from > 1);
201  if(to - from == 2) {
202  boxes.push_back(objBoxes[objCenters[from].second].merged(objBoxes[objCenters[from + 1].second]));
203  children.push_back(from + (int)objects.size() - 1); //there are objects.size() - 1 tree nodes
204  children.push_back(from + (int)objects.size());
205  }
206  else if(to - from == 3) {
207  int mid = from + 2;
208  std::nth_element(objCenters.begin() + from, objCenters.begin() + mid,
209  objCenters.begin() + to, VectorComparator(dim)); //partition
210  build(objCenters, from, mid, objBoxes, (dim + 1) % Dim);
211  int idx1 = (int)boxes.size() - 1;
212  boxes.push_back(boxes[idx1].merged(objBoxes[objCenters[mid].second]));
213  children.push_back(idx1);
214  children.push_back(mid + (int)objects.size() - 1);
215  }
216  else {
217  int mid = from + (to - from) / 2;
218  nth_element(objCenters.begin() + from, objCenters.begin() + mid,
219  objCenters.begin() + to, VectorComparator(dim)); //partition
220  build(objCenters, from, mid, objBoxes, (dim + 1) % Dim);
221  int idx1 = (int)boxes.size() - 1;
222  build(objCenters, mid, to, objBoxes, (dim + 1) % Dim);
223  int idx2 = (int)boxes.size() - 1;
224  boxes.push_back(boxes[idx1].merged(boxes[idx2]));
225  children.push_back(idx1);
226  children.push_back(idx2);
227  }
228  }
229 
230  std::vector<int> children; //children of x are children[2x] and children[2x+1], indices bigger than boxes.size() index into objects.
231  VolumeList boxes;
232  ObjectList objects;
233 };
234 
235 } // end namespace Eigen
236 
237 #endif //KDBVH_H_INCLUDED