$treeview $search $mathjax
Eigen-unsupported
3.2.5
$projectbrief
|
$projectbrief
|
$searchbox |
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2009 Ilya Baran <ibaran@mit.edu> 00005 // 00006 // This Source Code Form is subject to the terms of the Mozilla 00007 // Public License v. 2.0. If a copy of the MPL was not distributed 00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 00009 00010 #ifndef KDBVH_H_INCLUDED 00011 #define KDBVH_H_INCLUDED 00012 00013 namespace Eigen { 00014 00015 namespace internal { 00016 00017 //internal pair class for the BVH--used instead of std::pair because of alignment 00018 template<typename Scalar, int Dim> 00019 struct vector_int_pair 00020 { 00021 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Dim) 00022 typedef Matrix<Scalar, Dim, 1> VectorType; 00023 00024 vector_int_pair(const VectorType &v, int i) : first(v), second(i) {} 00025 00026 VectorType first; 00027 int second; 00028 }; 00029 00030 //these templates help the tree initializer get the bounding boxes either from a provided 00031 //iterator range or using bounding_box in a unified way 00032 template<typename ObjectList, typename VolumeList, typename BoxIter> 00033 struct get_boxes_helper { 00034 void operator()(const ObjectList &objects, BoxIter boxBegin, BoxIter boxEnd, VolumeList &outBoxes) 00035 { 00036 outBoxes.insert(outBoxes.end(), boxBegin, boxEnd); 00037 eigen_assert(outBoxes.size() == objects.size()); 00038 } 00039 }; 00040 00041 template<typename ObjectList, typename VolumeList> 00042 struct get_boxes_helper<ObjectList, VolumeList, int> { 00043 void operator()(const ObjectList &objects, int, int, VolumeList &outBoxes) 00044 { 00045 outBoxes.reserve(objects.size()); 00046 for(int i = 0; i < (int)objects.size(); ++i) 00047 outBoxes.push_back(bounding_box(objects[i])); 00048 } 00049 }; 00050 00051 } // end namespace internal 00052 00053 00067 template<typename _Scalar, int _Dim, typename _Object> class KdBVH 00068 { 00069 public: 00070 enum { Dim = _Dim }; 00071 typedef _Object Object; 00072 typedef std::vector<Object, aligned_allocator<Object> > ObjectList; 00073 typedef _Scalar Scalar; 00074 typedef AlignedBox<Scalar, Dim> Volume; 00075 typedef std::vector<Volume, aligned_allocator<Volume> > VolumeList; 00076 typedef int Index; 00077 typedef const int *VolumeIterator; //the iterators are just pointers into the tree's vectors 00078 typedef const Object *ObjectIterator; 00079 00080 KdBVH() {} 00081 00083 template<typename Iter> KdBVH(Iter begin, Iter end) { init(begin, end, 0, 0); } //int is recognized by init as not being an iterator type 00084 00086 template<typename OIter, typename BIter> KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) { init(begin, end, boxBegin, boxEnd); } 00087 00090 template<typename Iter> void init(Iter begin, Iter end) { init(begin, end, 0, 0); } 00091 00094 template<typename OIter, typename BIter> void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) 00095 { 00096 objects.clear(); 00097 boxes.clear(); 00098 children.clear(); 00099 00100 objects.insert(objects.end(), begin, end); 00101 int n = static_cast<int>(objects.size()); 00102 00103 if(n < 2) 00104 return; //if we have at most one object, we don't need any internal nodes 00105 00106 VolumeList objBoxes; 00107 VIPairList objCenters; 00108 00109 //compute the bounding boxes depending on BIter type 00110 internal::get_boxes_helper<ObjectList, VolumeList, BIter>()(objects, boxBegin, boxEnd, objBoxes); 00111 00112 objCenters.reserve(n); 00113 boxes.reserve(n - 1); 00114 children.reserve(2 * n - 2); 00115 00116 for(int i = 0; i < n; ++i) 00117 objCenters.push_back(VIPair(objBoxes[i].center(), i)); 00118 00119 build(objCenters, 0, n, objBoxes, 0); //the recursive part of the algorithm 00120 00121 ObjectList tmp(n); 00122 tmp.swap(objects); 00123 for(int i = 0; i < n; ++i) 00124 objects[i] = tmp[objCenters[i].second]; 00125 } 00126 00128 inline Index getRootIndex() const { return (int)boxes.size() - 1; } 00129 00132 EIGEN_STRONG_INLINE void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd, 00133 ObjectIterator &outOBegin, ObjectIterator &outOEnd) const 00134 { //inlining this function should open lots of optimization opportunities to the compiler 00135 if(index < 0) { 00136 outVBegin = outVEnd; 00137 if(!objects.empty()) 00138 outOBegin = &(objects[0]); 00139 outOEnd = outOBegin + objects.size(); //output all objects--necessary when the tree has only one object 00140 return; 00141 } 00142 00143 int numBoxes = static_cast<int>(boxes.size()); 00144 00145 int idx = index * 2; 00146 if(children[idx + 1] < numBoxes) { //second index is always bigger 00147 outVBegin = &(children[idx]); 00148 outVEnd = outVBegin + 2; 00149 outOBegin = outOEnd; 00150 } 00151 else if(children[idx] >= numBoxes) { //if both children are objects 00152 outVBegin = outVEnd; 00153 outOBegin = &(objects[children[idx] - numBoxes]); 00154 outOEnd = outOBegin + 2; 00155 } else { //if the first child is a volume and the second is an object 00156 outVBegin = &(children[idx]); 00157 outVEnd = outVBegin + 1; 00158 outOBegin = &(objects[children[idx + 1] - numBoxes]); 00159 outOEnd = outOBegin + 1; 00160 } 00161 } 00162 00164 inline const Volume &getVolume(Index index) const 00165 { 00166 return boxes[index]; 00167 } 00168 00169 private: 00170 typedef internal::vector_int_pair<Scalar, Dim> VIPair; 00171 typedef std::vector<VIPair, aligned_allocator<VIPair> > VIPairList; 00172 typedef Matrix<Scalar, Dim, 1> VectorType; 00173 struct VectorComparator //compares vectors, or, more specificall, VIPairs along a particular dimension 00174 { 00175 VectorComparator(int inDim) : dim(inDim) {} 00176 inline bool operator()(const VIPair &v1, const VIPair &v2) const { return v1.first[dim] < v2.first[dim]; } 00177 int dim; 00178 }; 00179 00180 //Build the part of the tree between objects[from] and objects[to] (not including objects[to]). 00181 //This routine partitions the objCenters in [from, to) along the dimension dim, recursively constructs 00182 //the two halves, and adds their parent node. TODO: a cache-friendlier layout 00183 void build(VIPairList &objCenters, int from, int to, const VolumeList &objBoxes, int dim) 00184 { 00185 eigen_assert(to - from > 1); 00186 if(to - from == 2) { 00187 boxes.push_back(objBoxes[objCenters[from].second].merged(objBoxes[objCenters[from + 1].second])); 00188 children.push_back(from + (int)objects.size() - 1); //there are objects.size() - 1 tree nodes 00189 children.push_back(from + (int)objects.size()); 00190 } 00191 else if(to - from == 3) { 00192 int mid = from + 2; 00193 std::nth_element(objCenters.begin() + from, objCenters.begin() + mid, 00194 objCenters.begin() + to, VectorComparator(dim)); //partition 00195 build(objCenters, from, mid, objBoxes, (dim + 1) % Dim); 00196 int idx1 = (int)boxes.size() - 1; 00197 boxes.push_back(boxes[idx1].merged(objBoxes[objCenters[mid].second])); 00198 children.push_back(idx1); 00199 children.push_back(mid + (int)objects.size() - 1); 00200 } 00201 else { 00202 int mid = from + (to - from) / 2; 00203 nth_element(objCenters.begin() + from, objCenters.begin() + mid, 00204 objCenters.begin() + to, VectorComparator(dim)); //partition 00205 build(objCenters, from, mid, objBoxes, (dim + 1) % Dim); 00206 int idx1 = (int)boxes.size() - 1; 00207 build(objCenters, mid, to, objBoxes, (dim + 1) % Dim); 00208 int idx2 = (int)boxes.size() - 1; 00209 boxes.push_back(boxes[idx1].merged(boxes[idx2])); 00210 children.push_back(idx1); 00211 children.push_back(idx2); 00212 } 00213 } 00214 00215 std::vector<int> children; //children of x are children[2x] and children[2x+1], indices bigger than boxes.size() index into objects. 00216 VolumeList boxes; 00217 ObjectList objects; 00218 }; 00219 00220 } // end namespace Eigen 00221 00222 #endif //KDBVH_H_INCLUDED