10 #ifndef KDBVH_H_INCLUDED 11 #define KDBVH_H_INCLUDED 18 template<
typename Scalar,
int Dim>
19 struct vector_int_pair
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Dim)
24 vector_int_pair(
const VectorType &v,
int i) : first(v), second(i) {}
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)
36 outBoxes.insert(outBoxes.end(), boxBegin, boxEnd);
37 eigen_assert(outBoxes.size() == objects.size());
38 EIGEN_ONLY_USED_FOR_DEBUG(objects);
42 template<
typename ObjectList,
typename VolumeList>
43 struct get_boxes_helper<ObjectList, VolumeList, int> {
44 void operator()(
const ObjectList &objects,
int,
int, VolumeList &outBoxes)
46 outBoxes.reserve(objects.size());
47 for(
int i = 0; i < (int)objects.size(); ++i)
48 outBoxes.push_back(bounding_box(objects[i]));
68 template<
typename _Scalar,
int _Dim,
typename _Object>
class KdBVH 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;
78 typedef const int *VolumeIterator;
79 typedef const Object *ObjectIterator;
84 template<
typename Iter>
KdBVH(Iter begin, Iter end) { init(begin, end, 0, 0); }
87 template<
typename OIter,
typename BIter>
KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) { init(begin, end, boxBegin, boxEnd); }
91 template<
typename Iter>
void init(Iter begin, Iter end) {
init(begin, end, 0, 0); }
95 template<
typename OIter,
typename BIter>
void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd)
101 objects.insert(objects.end(), begin, end);
102 int n =
static_cast<int>(objects.size());
108 VIPairList objCenters;
111 internal::get_boxes_helper<ObjectList, VolumeList, BIter>()(objects, boxBegin, boxEnd, objBoxes);
113 objCenters.reserve(n);
114 boxes.reserve(n - 1);
115 children.reserve(2 * n - 2);
117 for(
int i = 0; i < n; ++i)
118 objCenters.push_back(VIPair(objBoxes[i].center(), i));
120 build(objCenters, 0, n, objBoxes, 0);
124 for(
int i = 0; i < n; ++i)
125 objects[i] = tmp[objCenters[i].second];
133 EIGEN_STRONG_INLINE
void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd,
134 ObjectIterator &outOBegin, ObjectIterator &outOEnd)
const 139 outOBegin = &(objects[0]);
140 outOEnd = outOBegin + objects.size();
144 int numBoxes =
static_cast<int>(boxes.size());
147 if(children[idx + 1] < numBoxes) {
148 outVBegin = &(children[idx]);
149 outVEnd = outVBegin + 2;
152 else if(children[idx] >= numBoxes) {
154 outOBegin = &(objects[children[idx] - numBoxes]);
155 outOEnd = outOBegin + 2;
157 outVBegin = &(children[idx]);
158 outVEnd = outVBegin + 1;
159 outOBegin = &(objects[children[idx + 1] - numBoxes]);
160 outOEnd = outOBegin + 1;
171 typedef internal::vector_int_pair<Scalar, Dim> VIPair;
172 typedef std::vector<VIPair, aligned_allocator<VIPair> > VIPairList;
174 struct VectorComparator
176 VectorComparator(
int inDim) : dim(inDim) {}
177 inline bool operator()(
const VIPair &v1,
const VIPair &v2)
const {
return v1.first[dim] < v2.first[dim]; }
184 void build(VIPairList &objCenters,
int from,
int to,
const VolumeList &objBoxes,
int dim)
186 eigen_assert(to - from > 1);
188 boxes.push_back(objBoxes[objCenters[from].second].merged(objBoxes[objCenters[from + 1].second]));
189 children.push_back(from + (
int)objects.size() - 1);
190 children.push_back(from + (
int)objects.size());
192 else if(to - from == 3) {
194 std::nth_element(objCenters.begin() + from, objCenters.begin() + mid,
195 objCenters.begin() + to, VectorComparator(dim));
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);
203 int mid = from + (to - from) / 2;
204 nth_element(objCenters.begin() + from, objCenters.begin() + mid,
205 objCenters.begin() + to, VectorComparator(dim));
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);
216 std::vector<int> children;
223 #endif //KDBVH_H_INCLUDED KdBVH(Iter begin, Iter end)
Definition: KdBVH.h:84
KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd)
Definition: KdBVH.h:87
Namespace containing all symbols from the Eigen library.
void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd)
Definition: KdBVH.h:95
const Volume & getVolume(Index index) const
Definition: KdBVH.h:165
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
Index getRootIndex() const
Definition: KdBVH.h:129
void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd, ObjectIterator &outOBegin, ObjectIterator &outOEnd) const
Definition: KdBVH.h:133
A simple bounding volume hierarchy based on AlignedBox.
Definition: KdBVH.h:68
void init(Iter begin, Iter end)
Definition: KdBVH.h:91