1c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This file is part of Eigen, a lightweight C++ template library 2c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// for linear algebra. 3c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 4c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Copyright (C) 2009 Ilya Baran <ibaran@mit.edu> 5c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 6c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This Source Code Form is subject to the terms of the Mozilla 7c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Public License v. 2.0. If a copy of the MPL was not distributed 8c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 10c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#ifndef KDBVH_H_INCLUDED 11c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#define KDBVH_H_INCLUDED 12c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 13c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace Eigen { 14c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 15c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace internal { 16c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 17c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath//internal pair class for the BVH--used instead of std::pair because of alignment 18c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Scalar, int Dim> 19c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathstruct vector_int_pair 20c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 21c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathEIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Dim) 22c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar, Dim, 1> VectorType; 23c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 24c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath vector_int_pair(const VectorType &v, int i) : first(v), second(i) {} 25c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 26c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VectorType first; 27c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int second; 28c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 29c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 30c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath//these templates help the tree initializer get the bounding boxes either from a provided 31c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath//iterator range or using bounding_box in a unified way 32c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename ObjectList, typename VolumeList, typename BoxIter> 33c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathstruct get_boxes_helper { 34c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath void operator()(const ObjectList &objects, BoxIter boxBegin, BoxIter boxEnd, VolumeList &outBoxes) 35c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 36c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outBoxes.insert(outBoxes.end(), boxBegin, boxEnd); 37c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath eigen_assert(outBoxes.size() == objects.size()); 38c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 39c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 40c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 41c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename ObjectList, typename VolumeList> 42c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathstruct get_boxes_helper<ObjectList, VolumeList, int> { 43c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath void operator()(const ObjectList &objects, int, int, VolumeList &outBoxes) 44c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 45c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outBoxes.reserve(objects.size()); 46c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for(int i = 0; i < (int)objects.size(); ++i) 47c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outBoxes.push_back(bounding_box(objects[i])); 48c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 49c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 50c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 51c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} // end namespace internal 52c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 53c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 54c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \class KdBVH 55c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief A simple bounding volume hierarchy based on AlignedBox 56c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 57c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param _Scalar The underlying scalar type of the bounding boxes 58c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param _Dim The dimension of the space in which the hierarchy lives 59c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param _Object The object type that lives in the hierarchy. It must have value semantics. Either bounding_box(_Object) must 60c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * be defined and return an AlignedBox<_Scalar, _Dim> or bounding boxes must be provided to the tree initializer. 61c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 62c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * This class provides a simple (as opposed to optimized) implementation of a bounding volume hierarchy analogous to a Kd-tree. 63c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * Given a sequence of objects, it computes their bounding boxes, constructs a Kd-tree of their centers 64c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * and builds a BVH with the structure of that Kd-tree. When the elements of the tree are too expensive to be copied around, 65c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * it is useful for _Object to be a pointer. 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 67c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename _Scalar, int _Dim, typename _Object> class KdBVH 68c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 69c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathpublic: 70c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath enum { Dim = _Dim }; 71c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef _Object Object; 72c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef std::vector<Object, aligned_allocator<Object> > ObjectList; 73c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef _Scalar Scalar; 74c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef AlignedBox<Scalar, Dim> Volume; 75c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef std::vector<Volume, aligned_allocator<Volume> > VolumeList; 76c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef int Index; 77c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef const int *VolumeIterator; //the iterators are just pointers into the tree's vectors 78c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef const Object *ObjectIterator; 79c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 80c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath KdBVH() {} 81c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 82c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Given an iterator range over \a Object references, constructs the BVH. Requires that bounding_box(Object) return a Volume. */ 83c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename Iter> KdBVH(Iter begin, Iter end) { init(begin, end, 0, 0); } //int is recognized by init as not being an iterator type 84c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 85c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Given an iterator range over \a Object references and an iterator range over their bounding boxes, constructs the BVH */ 86c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename OIter, typename BIter> KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) { init(begin, end, boxBegin, boxEnd); } 87c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 88c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Given an iterator range over \a Object references, constructs the BVH, overwriting whatever is in there currently. 89c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * Requires that bounding_box(Object) return a Volume. */ 90c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename Iter> void init(Iter begin, Iter end) { init(begin, end, 0, 0); } 91c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 92c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Given an iterator range over \a Object references and an iterator range over their bounding boxes, 93c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * constructs the BVH, overwriting whatever is in there currently. */ 94c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename OIter, typename BIter> void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) 95c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 96c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath objects.clear(); 97c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath boxes.clear(); 98c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath children.clear(); 99c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 100c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath objects.insert(objects.end(), begin, end); 101c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int n = static_cast<int>(objects.size()); 102c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 103c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if(n < 2) 104c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return; //if we have at most one object, we don't need any internal nodes 105c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 106c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VolumeList objBoxes; 107c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VIPairList objCenters; 108c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 109c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath //compute the bounding boxes depending on BIter type 110c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath internal::get_boxes_helper<ObjectList, VolumeList, BIter>()(objects, boxBegin, boxEnd, objBoxes); 111c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 112c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath objCenters.reserve(n); 113c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath boxes.reserve(n - 1); 114c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath children.reserve(2 * n - 2); 115c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 116c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for(int i = 0; i < n; ++i) 117c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath objCenters.push_back(VIPair(objBoxes[i].center(), i)); 118c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 119c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath build(objCenters, 0, n, objBoxes, 0); //the recursive part of the algorithm 120c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 121c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ObjectList tmp(n); 122c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath tmp.swap(objects); 123c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for(int i = 0; i < n; ++i) 124c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath objects[i] = tmp[objCenters[i].second]; 125c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 126c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 127c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the index of the root of the hierarchy */ 128c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Index getRootIndex() const { return (int)boxes.size() - 1; } 129c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 130c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Given an \a index of a node, on exit, \a outVBegin and \a outVEnd range over the indices of the volume children of the node 131c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * and \a outOBegin and \a outOEnd range over the object children of the node */ 132c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_STRONG_INLINE void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd, 133c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ObjectIterator &outOBegin, ObjectIterator &outOEnd) const 134c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { //inlining this function should open lots of optimization opportunities to the compiler 135c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if(index < 0) { 136c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outVBegin = outVEnd; 137c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if(!objects.empty()) 138c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outOBegin = &(objects[0]); 139c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outOEnd = outOBegin + objects.size(); //output all objects--necessary when the tree has only one object 140c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return; 141c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 142c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 143c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int numBoxes = static_cast<int>(boxes.size()); 144c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 145c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int idx = index * 2; 146c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if(children[idx + 1] < numBoxes) { //second index is always bigger 147c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outVBegin = &(children[idx]); 148c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outVEnd = outVBegin + 2; 149c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outOBegin = outOEnd; 150c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 151c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath else if(children[idx] >= numBoxes) { //if both children are objects 152c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outVBegin = outVEnd; 153c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outOBegin = &(objects[children[idx] - numBoxes]); 154c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outOEnd = outOBegin + 2; 155c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } else { //if the first child is a volume and the second is an object 156c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outVBegin = &(children[idx]); 157c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outVEnd = outVBegin + 1; 158c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outOBegin = &(objects[children[idx + 1] - numBoxes]); 159c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath outOEnd = outOBegin + 1; 160c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 161c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 162c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 163c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the bounding box of the node at \a index */ 164c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline const Volume &getVolume(Index index) const 165c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 166c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return boxes[index]; 167c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 168c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 169c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathprivate: 170c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef internal::vector_int_pair<Scalar, Dim> VIPair; 171c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef std::vector<VIPair, aligned_allocator<VIPair> > VIPairList; 172c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar, Dim, 1> VectorType; 173c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath struct VectorComparator //compares vectors, or, more specificall, VIPairs along a particular dimension 174c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 175c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VectorComparator(int inDim) : dim(inDim) {} 176c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline bool operator()(const VIPair &v1, const VIPair &v2) const { return v1.first[dim] < v2.first[dim]; } 177c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int dim; 178c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath }; 179c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 180c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath //Build the part of the tree between objects[from] and objects[to] (not including objects[to]). 181c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath //This routine partitions the objCenters in [from, to) along the dimension dim, recursively constructs 182c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath //the two halves, and adds their parent node. TODO: a cache-friendlier layout 183c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath void build(VIPairList &objCenters, int from, int to, const VolumeList &objBoxes, int dim) 184c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 185c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath eigen_assert(to - from > 1); 186c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if(to - from == 2) { 187c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath boxes.push_back(objBoxes[objCenters[from].second].merged(objBoxes[objCenters[from + 1].second])); 188c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath children.push_back(from + (int)objects.size() - 1); //there are objects.size() - 1 tree nodes 189c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath children.push_back(from + (int)objects.size()); 190c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 191c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath else if(to - from == 3) { 192c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int mid = from + 2; 193c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath std::nth_element(objCenters.begin() + from, objCenters.begin() + mid, 194c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath objCenters.begin() + to, VectorComparator(dim)); //partition 195c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath build(objCenters, from, mid, objBoxes, (dim + 1) % Dim); 196c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int idx1 = (int)boxes.size() - 1; 197c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath boxes.push_back(boxes[idx1].merged(objBoxes[objCenters[mid].second])); 198c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath children.push_back(idx1); 199c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath children.push_back(mid + (int)objects.size() - 1); 200c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 201c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath else { 202c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int mid = from + (to - from) / 2; 203c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath nth_element(objCenters.begin() + from, objCenters.begin() + mid, 204c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath objCenters.begin() + to, VectorComparator(dim)); //partition 205c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath build(objCenters, from, mid, objBoxes, (dim + 1) % Dim); 206c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int idx1 = (int)boxes.size() - 1; 207c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath build(objCenters, mid, to, objBoxes, (dim + 1) % Dim); 208c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int idx2 = (int)boxes.size() - 1; 209c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath boxes.push_back(boxes[idx1].merged(boxes[idx2])); 210c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath children.push_back(idx1); 211c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath children.push_back(idx2); 212c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 213c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 214c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 215c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath std::vector<int> children; //children of x are children[2x] and children[2x+1], indices bigger than boxes.size() index into objects. 216c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VolumeList boxes; 217c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ObjectList objects; 218c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 219c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 220c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} // end namespace Eigen 221c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 222c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#endif //KDBVH_H_INCLUDED 223