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 // 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


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:37:43