1 // This file is part of Eigen, a lightweight C++ template library 2 // for linear algebra. 3 // 4 // Copyright (C) 2009 Ilya Baran <ibaran (at) 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 24 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 { 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()); 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
