00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef KDBVH_H_INCLUDED
00026 #define KDBVH_H_INCLUDED
00027
00028 namespace Eigen {
00029
00030 namespace internal {
00031
00032
00033 template<typename Scalar, int Dim>
00034 struct vector_int_pair
00035 {
00036 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Dim)
00037 typedef Matrix<Scalar, Dim, 1> VectorType;
00038
00039 vector_int_pair(const VectorType &v, int i) : first(v), second(i) {}
00040
00041 VectorType first;
00042 int second;
00043 };
00044
00045
00046
00047 template<typename ObjectList, typename VolumeList, typename BoxIter>
00048 struct get_boxes_helper {
00049 void operator()(const ObjectList &objects, BoxIter boxBegin, BoxIter boxEnd, VolumeList &outBoxes)
00050 {
00051 outBoxes.insert(outBoxes.end(), boxBegin, boxEnd);
00052 eigen_assert(outBoxes.size() == objects.size());
00053 }
00054 };
00055
00056 template<typename ObjectList, typename VolumeList>
00057 struct get_boxes_helper<ObjectList, VolumeList, int> {
00058 void operator()(const ObjectList &objects, int, int, VolumeList &outBoxes)
00059 {
00060 outBoxes.reserve(objects.size());
00061 for(int i = 0; i < (int)objects.size(); ++i)
00062 outBoxes.push_back(bounding_box(objects[i]));
00063 }
00064 };
00065
00066 }
00067
00068
00082 template<typename _Scalar, int _Dim, typename _Object> class KdBVH
00083 {
00084 public:
00085 enum { Dim = _Dim };
00086 typedef _Object Object;
00087 typedef std::vector<Object, aligned_allocator<Object> > ObjectList;
00088 typedef _Scalar Scalar;
00089 typedef AlignedBox<Scalar, Dim> Volume;
00090 typedef std::vector<Volume, aligned_allocator<Volume> > VolumeList;
00091 typedef int Index;
00092 typedef const int *VolumeIterator;
00093 typedef const Object *ObjectIterator;
00094
00095 KdBVH() {}
00096
00098 template<typename Iter> KdBVH(Iter begin, Iter end) { init(begin, end, 0, 0); }
00099
00101 template<typename OIter, typename BIter> KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) { init(begin, end, boxBegin, boxEnd); }
00102
00105 template<typename Iter> void init(Iter begin, Iter end) { init(begin, end, 0, 0); }
00106
00109 template<typename OIter, typename BIter> void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd)
00110 {
00111 objects.clear();
00112 boxes.clear();
00113 children.clear();
00114
00115 objects.insert(objects.end(), begin, end);
00116 int n = static_cast<int>(objects.size());
00117
00118 if(n < 2)
00119 return;
00120
00121 VolumeList objBoxes;
00122 VIPairList objCenters;
00123
00124
00125 internal::get_boxes_helper<ObjectList, VolumeList, BIter>()(objects, boxBegin, boxEnd, objBoxes);
00126
00127 objCenters.reserve(n);
00128 boxes.reserve(n - 1);
00129 children.reserve(2 * n - 2);
00130
00131 for(int i = 0; i < n; ++i)
00132 objCenters.push_back(VIPair(objBoxes[i].center(), i));
00133
00134 build(objCenters, 0, n, objBoxes, 0);
00135
00136 ObjectList tmp(n);
00137 tmp.swap(objects);
00138 for(int i = 0; i < n; ++i)
00139 objects[i] = tmp[objCenters[i].second];
00140 }
00141
00143 inline Index getRootIndex() const { return (int)boxes.size() - 1; }
00144
00147 EIGEN_STRONG_INLINE void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd,
00148 ObjectIterator &outOBegin, ObjectIterator &outOEnd) const
00149 {
00150 if(index < 0) {
00151 outVBegin = outVEnd;
00152 if(!objects.empty())
00153 outOBegin = &(objects[0]);
00154 outOEnd = outOBegin + objects.size();
00155 return;
00156 }
00157
00158 int numBoxes = static_cast<int>(boxes.size());
00159
00160 int idx = index * 2;
00161 if(children[idx + 1] < numBoxes) {
00162 outVBegin = &(children[idx]);
00163 outVEnd = outVBegin + 2;
00164 outOBegin = outOEnd;
00165 }
00166 else if(children[idx] >= numBoxes) {
00167 outVBegin = outVEnd;
00168 outOBegin = &(objects[children[idx] - numBoxes]);
00169 outOEnd = outOBegin + 2;
00170 } else {
00171 outVBegin = &(children[idx]);
00172 outVEnd = outVBegin + 1;
00173 outOBegin = &(objects[children[idx + 1] - numBoxes]);
00174 outOEnd = outOBegin + 1;
00175 }
00176 }
00177
00179 inline const Volume &getVolume(Index index) const
00180 {
00181 return boxes[index];
00182 }
00183
00184 private:
00185 typedef internal::vector_int_pair<Scalar, Dim> VIPair;
00186 typedef std::vector<VIPair, aligned_allocator<VIPair> > VIPairList;
00187 typedef Matrix<Scalar, Dim, 1> VectorType;
00188 struct VectorComparator
00189 {
00190 VectorComparator(int inDim) : dim(inDim) {}
00191 inline bool operator()(const VIPair &v1, const VIPair &v2) const { return v1.first[dim] < v2.first[dim]; }
00192 int dim;
00193 };
00194
00195
00196
00197
00198 void build(VIPairList &objCenters, int from, int to, const VolumeList &objBoxes, int dim)
00199 {
00200 eigen_assert(to - from > 1);
00201 if(to - from == 2) {
00202 boxes.push_back(objBoxes[objCenters[from].second].merged(objBoxes[objCenters[from + 1].second]));
00203 children.push_back(from + (int)objects.size() - 1);
00204 children.push_back(from + (int)objects.size());
00205 }
00206 else if(to - from == 3) {
00207 int mid = from + 2;
00208 std::nth_element(objCenters.begin() + from, objCenters.begin() + mid,
00209 objCenters.begin() + to, VectorComparator(dim));
00210 build(objCenters, from, mid, objBoxes, (dim + 1) % Dim);
00211 int idx1 = (int)boxes.size() - 1;
00212 boxes.push_back(boxes[idx1].merged(objBoxes[objCenters[mid].second]));
00213 children.push_back(idx1);
00214 children.push_back(mid + (int)objects.size() - 1);
00215 }
00216 else {
00217 int mid = from + (to - from) / 2;
00218 nth_element(objCenters.begin() + from, objCenters.begin() + mid,
00219 objCenters.begin() + to, VectorComparator(dim));
00220 build(objCenters, from, mid, objBoxes, (dim + 1) % Dim);
00221 int idx1 = (int)boxes.size() - 1;
00222 build(objCenters, mid, to, objBoxes, (dim + 1) % Dim);
00223 int idx2 = (int)boxes.size() - 1;
00224 boxes.push_back(boxes[idx1].merged(boxes[idx2]));
00225 children.push_back(idx1);
00226 children.push_back(idx2);
00227 }
00228 }
00229
00230 std::vector<int> children;
00231 VolumeList boxes;
00232 ObjectList objects;
00233 };
00234
00235 }
00236
00237 #endif //KDBVH_H_INCLUDED