10 #ifndef KDBVH_H_INCLUDED 11 #define KDBVH_H_INCLUDED 18 template<
typename Scalar,
int Dim>
32 template<
typename ObjectList,
typename VolumeList,
typename BoxIter>
34 void operator()(
const ObjectList &objects, BoxIter boxBegin, BoxIter boxEnd, VolumeList &outBoxes)
36 outBoxes.insert(outBoxes.end(), boxBegin, boxEnd);
41 template<
typename ObjectList,
typename VolumeList>
43 void operator()(
const ObjectList &objects,
int,
int, VolumeList &outBoxes)
45 outBoxes.reserve(objects.size());
46 for(
int i = 0; i < (int)objects.size(); ++i)
47 outBoxes.push_back(bounding_box(objects[i]));
67 template<
typename _Scalar,
int _Dim,
typename _Object>
class KdBVH 72 typedef std::vector<Object, aligned_allocator<Object> >
ObjectList;
75 typedef std::vector<Volume, aligned_allocator<Volume> >
VolumeList;
83 template<
typename Iter>
KdBVH(Iter begin, Iter end) {
init(begin, end, 0, 0); }
86 template<
typename OIter,
typename BIter>
KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) {
init(begin, end, boxBegin, boxEnd); }
90 template<
typename Iter>
void init(Iter begin, Iter end) {
init(begin, end, 0, 0); }
94 template<
typename OIter,
typename BIter>
void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd)
100 objects.insert(objects.end(), begin, end);
101 int n =
static_cast<int>(objects.size());
112 objCenters.reserve(n);
113 boxes.reserve(n - 1);
114 children.reserve(2 * n - 2);
116 for(
int i = 0; i < n; ++i)
117 objCenters.push_back(
VIPair(objBoxes[i].center(), i));
119 build(objCenters, 0, n, objBoxes, 0);
123 for(
int i = 0; i < n; ++i)
124 objects[i] = tmp[objCenters[i].
second];
133 ObjectIterator &outOBegin, ObjectIterator &outOEnd)
const 138 outOBegin = &(objects[0]);
139 outOEnd = outOBegin + objects.size();
143 int numBoxes =
static_cast<int>(boxes.size());
146 if(children[idx + 1] < numBoxes) {
147 outVBegin = &(children[idx]);
148 outVEnd = outVBegin + 2;
151 else if(children[idx] >= numBoxes) {
153 outOBegin = &(objects[children[idx] - numBoxes]);
154 outOEnd = outOBegin + 2;
156 outVBegin = &(children[idx]);
157 outVEnd = outVBegin + 1;
158 outOBegin = &(objects[children[idx + 1] - numBoxes]);
159 outOEnd = outOBegin + 1;
171 typedef std::vector<VIPair, aligned_allocator<VIPair> >
VIPairList;
183 void build(VIPairList &objCenters,
int from,
int to,
const VolumeList &objBoxes,
int dim)
187 boxes.push_back(objBoxes[objCenters[from].
second].merged(objBoxes[objCenters[from + 1].
second]));
188 children.push_back(from + (
int)objects.size() - 1);
189 children.push_back(from + (
int)objects.size());
191 else if(to - from == 3) {
193 std::nth_element(objCenters.begin() + from, objCenters.begin() + mid,
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);
202 int mid = from + (to - from) / 2;
203 nth_element(objCenters.begin() + from, objCenters.begin() + mid,
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);
222 #endif //KDBVH_H_INCLUDED
void operator()(const ObjectList &objects, int, int, VolumeList &outBoxes)
#define EIGEN_STRONG_INLINE
KdBVH(Iter begin, Iter end)
KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd)
const Object * ObjectIterator
iterative scaling algorithm to equilibrate rows and column norms in matrices
void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd)
std::vector< Volume, aligned_allocator< Volume > > VolumeList
VectorComparator(int inDim)
void build(VIPairList &objCenters, int from, int to, const VolumeList &objBoxes, int dim)
Index getRootIndex() const
EIGEN_STRONG_INLINE void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd, ObjectIterator &outOBegin, ObjectIterator &outOEnd) const
Matrix< Scalar, Dim, 1 > VectorType
const int * VolumeIterator
bool operator()(const VIPair &v1, const VIPair &v2) const
internal::vector_int_pair< Scalar, Dim > VIPair
AlignedBox< Scalar, Dim > Volume
const Volume & getVolume(Index index) const
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Size)
The matrix class, also used for vectors and row-vectors.
A simple bounding volume hierarchy based on AlignedBox.
void init(int nV, int nC, SymmetricMatrix *H, real_t *g, Matrix *A, const real_t *const lb, const real_t *const ub, const real_t *const lbA, const real_t *const ubA, int nWSR, const real_t *const x0, Options *options, int nOutputs, mxArray *plhs[])
void operator()(const ObjectList &objects, BoxIter boxBegin, BoxIter boxEnd, VolumeList &outBoxes)
std::vector< VIPair, aligned_allocator< VIPair > > VIPairList
void init(Iter begin, Iter end)
std::vector< Object, aligned_allocator< Object > > ObjectList
std::vector< int > children