KdBVH.h
Go to the documentation of this file.
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 // Eigen is free software; you can redistribute it and/or
00007 // modify it under the terms of the GNU Lesser General Public
00008 // License as published by the Free Software Foundation; either
00009 // version 3 of the License, or (at your option) any later version.
00010 //
00011 // Alternatively, you can redistribute it and/or
00012 // modify it under the terms of the GNU General Public License as
00013 // published by the Free Software Foundation; either version 2 of
00014 // the License, or (at your option) any later version.
00015 //
00016 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00017 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00018 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00019 // GNU General Public License for more details.
00020 //
00021 // You should have received a copy of the GNU Lesser General Public
00022 // License and a copy of the GNU General Public License along with
00023 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00024 
00025 #ifndef KDBVH_H_INCLUDED
00026 #define KDBVH_H_INCLUDED
00027 
00028 namespace internal {
00029 
00030 //internal pair class for the BVH--used instead of std::pair because of alignment
00031 template<typename Scalar, int Dim>
00032 struct vector_int_pair
00033 {
00034 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Dim)
00035   typedef Matrix<Scalar, Dim, 1> VectorType;
00036 
00037   vector_int_pair(const VectorType &v, int i) : first(v), second(i) {}
00038 
00039   VectorType first;
00040   int second;
00041 };
00042 
00043 //these templates help the tree initializer get the bounding boxes either from a provided
00044 //iterator range or using bounding_box in a unified way
00045 template<typename ObjectList, typename VolumeList, typename BoxIter>
00046 struct get_boxes_helper {
00047   void operator()(const ObjectList &objects, BoxIter boxBegin, BoxIter boxEnd, VolumeList &outBoxes)
00048   {
00049     outBoxes.insert(outBoxes.end(), boxBegin, boxEnd);
00050     eigen_assert(outBoxes.size() == objects.size());
00051   }
00052 };
00053 
00054 template<typename ObjectList, typename VolumeList>
00055 struct get_boxes_helper<ObjectList, VolumeList, int> {
00056   void operator()(const ObjectList &objects, int, int, VolumeList &outBoxes)
00057   {
00058     outBoxes.reserve(objects.size());
00059     for(int i = 0; i < (int)objects.size(); ++i)
00060       outBoxes.push_back(bounding_box(objects[i]));
00061   }
00062 };
00063 
00064 } // end namespace internal
00065 
00066 
00080 template<typename _Scalar, int _Dim, typename _Object> class KdBVH
00081 {
00082 public:
00083   enum { Dim = _Dim };
00084   typedef _Object Object;
00085   typedef std::vector<Object, aligned_allocator<Object> > ObjectList;
00086   typedef _Scalar Scalar;
00087   typedef AlignedBox<Scalar, Dim> Volume;
00088   typedef std::vector<Volume, aligned_allocator<Volume> > VolumeList;
00089   typedef int Index;
00090   typedef const int *VolumeIterator; //the iterators are just pointers into the tree's vectors
00091   typedef const Object *ObjectIterator;
00092 
00093   KdBVH() {}
00094 
00096   template<typename Iter> KdBVH(Iter begin, Iter end) { init(begin, end, 0, 0); } //int is recognized by init as not being an iterator type
00097 
00099   template<typename OIter, typename BIter> KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) { init(begin, end, boxBegin, boxEnd); }
00100 
00103   template<typename Iter> void init(Iter begin, Iter end) { init(begin, end, 0, 0); }
00104 
00107   template<typename OIter, typename BIter> void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd)
00108   {
00109     objects.clear();
00110     boxes.clear();
00111     children.clear();
00112 
00113     objects.insert(objects.end(), begin, end);
00114     int n = static_cast<int>(objects.size());
00115 
00116     if(n < 2)
00117       return; //if we have at most one object, we don't need any internal nodes
00118 
00119     VolumeList objBoxes;
00120     VIPairList objCenters;
00121 
00122     //compute the bounding boxes depending on BIter type
00123     internal::get_boxes_helper<ObjectList, VolumeList, BIter>()(objects, boxBegin, boxEnd, objBoxes);
00124 
00125     objCenters.reserve(n);
00126     boxes.reserve(n - 1);
00127     children.reserve(2 * n - 2);
00128 
00129     for(int i = 0; i < n; ++i)
00130       objCenters.push_back(VIPair(objBoxes[i].center(), i));
00131 
00132     build(objCenters, 0, n, objBoxes, 0); //the recursive part of the algorithm
00133 
00134     ObjectList tmp(n);
00135     tmp.swap(objects);
00136     for(int i = 0; i < n; ++i)
00137       objects[i] = tmp[objCenters[i].second];
00138   }
00139 
00141   inline Index getRootIndex() const { return (int)boxes.size() - 1; }
00142 
00145   EIGEN_STRONG_INLINE void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd,
00146                                        ObjectIterator &outOBegin, ObjectIterator &outOEnd) const
00147   { //inlining this function should open lots of optimization opportunities to the compiler
00148     if(index < 0) {
00149       outVBegin = outVEnd;
00150       if(!objects.empty())
00151         outOBegin = &(objects[0]);
00152       outOEnd = outOBegin + objects.size(); //output all objects--necessary when the tree has only one object
00153       return;
00154     }
00155 
00156     int numBoxes = static_cast<int>(boxes.size());
00157 
00158     int idx = index * 2;
00159     if(children[idx + 1] < numBoxes) { //second index is always bigger
00160       outVBegin = &(children[idx]);
00161       outVEnd = outVBegin + 2;
00162       outOBegin = outOEnd;
00163     }
00164     else if(children[idx] >= numBoxes) { //if both children are objects
00165       outVBegin = outVEnd;
00166       outOBegin = &(objects[children[idx] - numBoxes]);
00167       outOEnd = outOBegin + 2;
00168     } else { //if the first child is a volume and the second is an object
00169       outVBegin = &(children[idx]);
00170       outVEnd = outVBegin + 1;
00171       outOBegin = &(objects[children[idx + 1] - numBoxes]);
00172       outOEnd = outOBegin + 1;
00173     }
00174   }
00175 
00177   inline const Volume &getVolume(Index index) const
00178   {
00179     return boxes[index];
00180   }
00181 
00182 private:
00183   typedef internal::vector_int_pair<Scalar, Dim> VIPair;
00184   typedef std::vector<VIPair, aligned_allocator<VIPair> > VIPairList;
00185   typedef Matrix<Scalar, Dim, 1> VectorType;
00186   struct VectorComparator //compares vectors, or, more specificall, VIPairs along a particular dimension
00187   {
00188     VectorComparator(int inDim) : dim(inDim) {}
00189     inline bool operator()(const VIPair &v1, const VIPair &v2) const { return v1.first[dim] < v2.first[dim]; }
00190     int dim;
00191   };
00192 
00193   //Build the part of the tree between objects[from] and objects[to] (not including objects[to]).
00194   //This routine partitions the objCenters in [from, to) along the dimension dim, recursively constructs
00195   //the two halves, and adds their parent node.  TODO: a cache-friendlier layout
00196   void build(VIPairList &objCenters, int from, int to, const VolumeList &objBoxes, int dim)
00197   {
00198     eigen_assert(to - from > 1);
00199     if(to - from == 2) {
00200       boxes.push_back(objBoxes[objCenters[from].second].merged(objBoxes[objCenters[from + 1].second]));
00201       children.push_back(from + (int)objects.size() - 1); //there are objects.size() - 1 tree nodes
00202       children.push_back(from + (int)objects.size());
00203     }
00204     else if(to - from == 3) {
00205       int mid = from + 2;
00206       std::nth_element(objCenters.begin() + from, objCenters.begin() + mid,
00207                         objCenters.begin() + to, VectorComparator(dim)); //partition
00208       build(objCenters, from, mid, objBoxes, (dim + 1) % Dim);
00209       int idx1 = (int)boxes.size() - 1;
00210       boxes.push_back(boxes[idx1].merged(objBoxes[objCenters[mid].second]));
00211       children.push_back(idx1);
00212       children.push_back(mid + (int)objects.size() - 1);
00213     }
00214     else {
00215       int mid = from + (to - from) / 2;
00216       nth_element(objCenters.begin() + from, objCenters.begin() + mid,
00217                   objCenters.begin() + to, VectorComparator(dim)); //partition
00218       build(objCenters, from, mid, objBoxes, (dim + 1) % Dim);
00219       int idx1 = (int)boxes.size() - 1;
00220       build(objCenters, mid, to, objBoxes, (dim + 1) % Dim);
00221       int idx2 = (int)boxes.size() - 1;
00222       boxes.push_back(boxes[idx1].merged(boxes[idx2]));
00223       children.push_back(idx1);
00224       children.push_back(idx2);
00225     }
00226   }
00227 
00228   std::vector<int> children; //children of x are children[2x] and children[2x+1], indices bigger than boxes.size() index into objects.
00229   VolumeList boxes;
00230   ObjectList objects;
00231 };
00232 
00233 #endif //KDBVH_H_INCLUDED


libicr
Author(s): Robert Krug
autogenerated on Mon Jan 6 2014 11:32:53