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>
19 struct vector_int_pair
20 {
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Dim)
22   typedef Matrix<Scalar, Dim, 1> VectorType;
23 
vector_int_pairvector_int_pair24   vector_int_pair(const VectorType &v, int i) : first(v), second(i) {}
25 
26   VectorType first;
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>
33 struct get_boxes_helper {
operatorget_boxes_helper34   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());
38   }
39 };
40 
41 template<typename ObjectList, typename VolumeList>
42 struct get_boxes_helper<ObjectList, VolumeList, int> {
43   void operator()(const ObjectList &objects, int, int, VolumeList &outBoxes)
44   {
45     outBoxes.reserve(objects.size());
46     for(int i = 0; i < (int)objects.size(); ++i)
47       outBoxes.push_back(bounding_box(objects[i]));
48   }
49 };
50 
51 } // end namespace internal
52 
53 
54 /** \class KdBVH
55  *  \brief A simple bounding volume hierarchy based on AlignedBox
56  *
57  *  \param _Scalar The underlying scalar type of the bounding boxes
58  *  \param _Dim The dimension of the space in which the hierarchy lives
59  *  \param _Object The object type that lives in the hierarchy.  It must have value semantics.  Either bounding_box(_Object) must
60  *                 be defined and return an AlignedBox<_Scalar, _Dim> or bounding boxes must be provided to the tree initializer.
61  *
62  *  This class provides a simple (as opposed to optimized) implementation of a bounding volume hierarchy analogous to a Kd-tree.
63  *  Given a sequence of objects, it computes their bounding boxes, constructs a Kd-tree of their centers
64  *  and builds a BVH with the structure of that Kd-tree.  When the elements of the tree are too expensive to be copied around,
65  *  it is useful for _Object to be a pointer.
66  */
67 template<typename _Scalar, int _Dim, typename _Object> class KdBVH
68 {
69 public:
70   enum { Dim = _Dim };
71   typedef _Object Object;
72   typedef std::vector<Object, aligned_allocator<Object> > ObjectList;
73   typedef _Scalar Scalar;
74   typedef AlignedBox<Scalar, Dim> Volume;
75   typedef std::vector<Volume, aligned_allocator<Volume> > VolumeList;
76   typedef int Index;
77   typedef const int *VolumeIterator; //the iterators are just pointers into the tree's vectors
78   typedef const Object *ObjectIterator;
79 
80   KdBVH() {}
81 
82   /** Given an iterator range over \a Object references, constructs the BVH.  Requires that bounding_box(Object) return a Volume. */
83   template<typename Iter> KdBVH(Iter begin, Iter end) { init(begin, end, 0, 0); } //int is recognized by init as not being an iterator type
84 
85   /** Given an iterator range over \a Object references and an iterator range over their bounding boxes, constructs the BVH */
86   template<typename OIter, typename BIter> KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) { init(begin, end, boxBegin, boxEnd); }
87 
88   /** Given an iterator range over \a Object references, constructs the BVH, overwriting whatever is in there currently.
89     * Requires that bounding_box(Object) return a Volume. */
90   template<typename Iter> void init(Iter begin, Iter end) { init(begin, end, 0, 0); }
91 
92   /** Given an iterator range over \a Object references and an iterator range over their bounding boxes,
93     * constructs the BVH, overwriting whatever is in there currently. */
94   template<typename OIter, typename BIter> void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd)
95   {
96     objects.clear();
97     boxes.clear();
98     children.clear();
99 
100     objects.insert(objects.end(), begin, end);
101     int n = static_cast<int>(objects.size());
102 
103     if(n < 2)
104       return; //if we have at most one object, we don't need any internal nodes
105 
106     VolumeList objBoxes;
107     VIPairList objCenters;
108 
109     //compute the bounding boxes depending on BIter type
110     internal::get_boxes_helper<ObjectList, VolumeList, BIter>()(objects, boxBegin, boxEnd, objBoxes);
111 
112     objCenters.reserve(n);
113     boxes.reserve(n - 1);
114     children.reserve(2 * n - 2);
115 
116     for(int i = 0; i < n; ++i)
117       objCenters.push_back(VIPair(objBoxes[i].center(), i));
118 
119     build(objCenters, 0, n, objBoxes, 0); //the recursive part of the algorithm
120 
121     ObjectList tmp(n);
122     tmp.swap(objects);
123     for(int i = 0; i < n; ++i)
124       objects[i] = tmp[objCenters[i].second];
125   }
126 
127   /** \returns the index of the root of the hierarchy */
128   inline Index getRootIndex() const { return (int)boxes.size() - 1; }
129 
130   /** Given an \a index of a node, on exit, \a outVBegin and \a outVEnd range over the indices of the volume children of the node
131     * and \a outOBegin and \a outOEnd range over the object children of the node */
132   EIGEN_STRONG_INLINE void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd,
133                                        ObjectIterator &outOBegin, ObjectIterator &outOEnd) const
134   { //inlining this function should open lots of optimization opportunities to the compiler
135     if(index < 0) {
136       outVBegin = outVEnd;
137       if(!objects.empty())
138         outOBegin = &(objects[0]);
139       outOEnd = outOBegin + objects.size(); //output all objects--necessary when the tree has only one object
140       return;
141     }
142 
143     int numBoxes = static_cast<int>(boxes.size());
144 
145     int idx = index * 2;
146     if(children[idx + 1] < numBoxes) { //second index is always bigger
147       outVBegin = &(children[idx]);
148       outVEnd = outVBegin + 2;
149       outOBegin = outOEnd;
150     }
151     else if(children[idx] >= numBoxes) { //if both children are objects
152       outVBegin = outVEnd;
153       outOBegin = &(objects[children[idx] - numBoxes]);
154       outOEnd = outOBegin + 2;
155     } else { //if the first child is a volume and the second is an object
156       outVBegin = &(children[idx]);
157       outVEnd = outVBegin + 1;
158       outOBegin = &(objects[children[idx + 1] - numBoxes]);
159       outOEnd = outOBegin + 1;
160     }
161   }
162 
163   /** \returns the bounding box of the node at \a index */
164   inline const Volume &getVolume(Index index) const
165   {
166     return boxes[index];
167   }
168 
169 private:
170   typedef internal::vector_int_pair<Scalar, Dim> VIPair;
171   typedef std::vector<VIPair, aligned_allocator<VIPair> > VIPairList;
172   typedef Matrix<Scalar, Dim, 1> VectorType;
173   struct VectorComparator //compares vectors, or, more specificall, VIPairs along a particular dimension
174   {
175     VectorComparator(int inDim) : dim(inDim) {}
176     inline bool operator()(const VIPair &v1, const VIPair &v2) const { return v1.first[dim] < v2.first[dim]; }
177     int dim;
178   };
179 
180   //Build the part of the tree between objects[from] and objects[to] (not including objects[to]).
181   //This routine partitions the objCenters in [from, to) along the dimension dim, recursively constructs
182   //the two halves, and adds their parent node.  TODO: a cache-friendlier layout
183   void build(VIPairList &objCenters, int from, int to, const VolumeList &objBoxes, int dim)
184   {
185     eigen_assert(to - from > 1);
186     if(to - from == 2) {
187       boxes.push_back(objBoxes[objCenters[from].second].merged(objBoxes[objCenters[from + 1].second]));
188       children.push_back(from + (int)objects.size() - 1); //there are objects.size() - 1 tree nodes
189       children.push_back(from + (int)objects.size());
190     }
191     else if(to - from == 3) {
192       int mid = from + 2;
193       std::nth_element(objCenters.begin() + from, objCenters.begin() + mid,
194                         objCenters.begin() + to, VectorComparator(dim)); //partition
195       build(objCenters, from, mid, objBoxes, (dim + 1) % Dim);
196       int idx1 = (int)boxes.size() - 1;
197       boxes.push_back(boxes[idx1].merged(objBoxes[objCenters[mid].second]));
198       children.push_back(idx1);
199       children.push_back(mid + (int)objects.size() - 1);
200     }
201     else {
202       int mid = from + (to - from) / 2;
203       nth_element(objCenters.begin() + from, objCenters.begin() + mid,
204                   objCenters.begin() + to, VectorComparator(dim)); //partition
205       build(objCenters, from, mid, objBoxes, (dim + 1) % Dim);
206       int idx1 = (int)boxes.size() - 1;
207       build(objCenters, mid, to, objBoxes, (dim + 1) % Dim);
208       int idx2 = (int)boxes.size() - 1;
209       boxes.push_back(boxes[idx1].merged(boxes[idx2]));
210       children.push_back(idx1);
211       children.push_back(idx2);
212     }
213   }
214 
215   std::vector<int> children; //children of x are children[2x] and children[2x+1], indices bigger than boxes.size() index into objects.
216   VolumeList boxes;
217   ObjectList objects;
218 };
219 
220 } // end namespace Eigen
221 
222 #endif //KDBVH_H_INCLUDED
223