25 #ifndef KDBVH_H_INCLUDED
26 #define KDBVH_H_INCLUDED
33 template<
typename Scalar,
int Dim>
34 struct vector_int_pair
36 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Dim)
37 typedef Matrix<Scalar, Dim, 1> VectorType;
39 vector_int_pair(const VectorType &v,
int i) : first(v), second(i) {}
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)
51 outBoxes.insert(outBoxes.end(), boxBegin, boxEnd);
52 eigen_assert(outBoxes.size() == objects.size());
56 template<
typename ObjectList,
typename VolumeList>
57 struct get_boxes_helper<ObjectList, VolumeList, int> {
58 void operator()(
const ObjectList &objects,
int,
int, VolumeList &outBoxes)
60 outBoxes.reserve(objects.size());
61 for(
int i = 0; i < (int)objects.size(); ++i)
62 outBoxes.push_back(bounding_box(objects[i]));
82 template<
typename _Scalar,
int _Dim,
typename _Object>
class KdBVH
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;
92 typedef const int *VolumeIterator;
93 typedef const Object *ObjectIterator;
98 template<
typename Iter>
KdBVH(Iter begin, Iter end) {
init(begin, end, 0, 0); }
101 template<
typename OIter,
typename BIter>
KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) {
init(begin, end, boxBegin, boxEnd); }
105 template<
typename Iter>
void init(Iter begin, Iter end) {
init(begin, end, 0, 0); }
109 template<
typename OIter,
typename BIter>
void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd)
115 objects.insert(objects.end(), begin, end);
116 int n =
static_cast<int>(objects.size());
122 VIPairList objCenters;
125 internal::get_boxes_helper<ObjectList, VolumeList, BIter>()(objects, boxBegin, boxEnd, objBoxes);
127 objCenters.reserve(n);
128 boxes.reserve(n - 1);
129 children.reserve(2 * n - 2);
131 for(
int i = 0; i < n; ++i)
132 objCenters.push_back(VIPair(objBoxes[i].center(), i));
134 build(objCenters, 0, n, objBoxes, 0);
138 for(
int i = 0; i < n; ++i)
139 objects[i] = tmp[objCenters[i].second];
147 EIGEN_STRONG_INLINE
void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd,
148 ObjectIterator &outOBegin, ObjectIterator &outOEnd)
const
153 outOBegin = &(objects[0]);
154 outOEnd = outOBegin + objects.size();
158 int numBoxes =
static_cast<int>(boxes.size());
161 if(children[idx + 1] < numBoxes) {
162 outVBegin = &(children[idx]);
163 outVEnd = outVBegin + 2;
166 else if(children[idx] >= numBoxes) {
168 outOBegin = &(objects[children[idx] - numBoxes]);
169 outOEnd = outOBegin + 2;
171 outVBegin = &(children[idx]);
172 outVEnd = outVBegin + 1;
173 outOBegin = &(objects[children[idx + 1] - numBoxes]);
174 outOEnd = outOBegin + 1;
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
190 VectorComparator(
int inDim) : dim(inDim) {}
191 inline bool operator()(
const VIPair &v1,
const VIPair &v2)
const {
return v1.first[dim] < v2.first[dim]; }
198 void build(VIPairList &objCenters,
int from,
int to,
const VolumeList &objBoxes,
int dim)
200 eigen_assert(to - from > 1);
202 boxes.push_back(objBoxes[objCenters[from].second].merged(objBoxes[objCenters[from + 1].second]));
203 children.push_back(from + (
int)objects.size() - 1);
204 children.push_back(from + (
int)objects.size());
206 else if(to - from == 3) {
208 std::nth_element(objCenters.begin() + from, objCenters.begin() + mid,
209 objCenters.begin() + to, VectorComparator(dim));
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);
217 int mid = from + (to - from) / 2;
218 nth_element(objCenters.begin() + from, objCenters.begin() + mid,
219 objCenters.begin() + to, VectorComparator(dim));
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);
230 std::vector<int> children;
237 #endif //KDBVH_H_INCLUDED