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 |
|
---|
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
|
---|