src/BVH/BVAlgorithms.h

0001 // This file is part of Eigen, a lightweight C++ template library
0002 // for linear algebra.
0003 //
0004 // Copyright (C) 2009 Ilya Baran <ibaran@mit.edu>
0005 //
0006 // This Source Code Form is subject to the terms of the Mozilla
0007 // Public License v. 2.0. If a copy of the MPL was not distributed
0008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
0009
0010 #ifndef EIGEN_BVALGORITHMS_H
0011 #define EIGEN_BVALGORITHMS_H
0012
0013 namespace Eigen {
0014
0015 namespace internal {
0016
0017 #ifndef EIGEN_PARSED_BY_DOXYGEN
0018 template<typename BVH, typename Intersector>
0019 bool intersect_helper(const BVH &tree, Intersector &intersector, typename BVH::Index root)
0020 {
0021   typedef typename BVH::Index Index;
0022   typedef typename BVH::VolumeIterator VolIter;
0023   typedef typename BVH::ObjectIterator ObjIter;
0024
0025   VolIter vBegin = VolIter(), vEnd = VolIter();
0026   ObjIter oBegin = ObjIter(), oEnd = ObjIter();
0027
0028   std::vector<Index> todo(1, root);
0029
0030   while(!todo.empty()) {
0031     tree.getChildren(todo.back(), vBegin, vEnd, oBegin, oEnd);
0032     todo.pop_back();
0033
0034     for(; vBegin != vEnd; ++vBegin) //go through child volumes
0035       if(intersector.intersectVolume(tree.getVolume(*vBegin)))
0036         todo.push_back(*vBegin);
0037
0038     for(; oBegin != oEnd; ++oBegin) //go through child objects
0039       if(intersector.intersectObject(*oBegin))
0040         return true; //intersector said to stop query
0041   }
0042   return false;
0043 }
0044 #endif //not EIGEN_PARSED_BY_DOXYGEN
0045
0046 template<typename Volume1, typename Object1, typename Object2, typename Intersector>
0047 struct intersector_helper1
0048 {
0049   intersector_helper1(const Object2 &inStored, Intersector &in) : stored(inStored), intersector(in) {}
0050   bool intersectVolume(const Volume1 &vol) { return intersector.intersectVolumeObject(vol, stored); }
0051   bool intersectObject(const Object1 &obj) { return intersector.intersectObjectObject(obj, stored); }
0052   Object2 stored;
0053   Intersector &intersector;
0054 private:
0055   intersector_helper1& operator=(const intersector_helper1&);
0056 };
0057
0058 template<typename Volume2, typename Object2, typename Object1, typename Intersector>
0059 struct intersector_helper2
0060 {
0061   intersector_helper2(const Object1 &inStored, Intersector &in) : stored(inStored), intersector(in) {}
0062   bool intersectVolume(const Volume2 &vol) { return intersector.intersectObjectVolume(stored, vol); }
0063   bool intersectObject(const Object2 &obj) { return intersector.intersectObjectObject(stored, obj); }
0064   Object1 stored;
0065   Intersector &intersector;
0066 private:
0067   intersector_helper2& operator=(const intersector_helper2&);
0068 };
0069
0070 } // end namespace internal
0071
0072 /**  Given a BVH, runs the query encapsulated by \a intersector.
0073   *  The Intersector type must provide the following members: \code
0074      bool intersectVolume(const BVH::Volume &volume) //returns true if volume intersects the query
0075      bool intersectObject(const BVH::Object &object) //returns true if the search should terminate immediately
0076   \endcode
0077   */
0078 template<typename BVH, typename Intersector>
0079 void BVIntersect(const BVH &tree, Intersector &intersector)
0080 {
0081   internal::intersect_helper(tree, intersector, tree.getRootIndex());
0082 }
0083
0084 /**  Given two BVH's, runs the query on their Cartesian product encapsulated by \a intersector.
0085   *  The Intersector type must provide the following members: \code
0086      bool intersectVolumeVolume(const BVH1::Volume &v1, const BVH2::Volume &v2) //returns true if product of volumes intersects the query
0087      bool intersectVolumeObject(const BVH1::Volume &v1, const BVH2::Object &o2) //returns true if the volume-object product intersects the query
0088      bool intersectObjectVolume(const BVH1::Object &o1, const BVH2::Volume &v2) //returns true if the volume-object product intersects the query
0089      bool intersectObjectObject(const BVH1::Object &o1, const BVH2::Object &o2) //returns true if the search should terminate immediately
0090   \endcode
0091   */
0092 template<typename BVH1, typename BVH2, typename Intersector>
0093 void BVIntersect(const BVH1 &tree1, const BVH2 &tree2, Intersector &intersector) //TODO: tandem descent when it makes sense
0094 {
0095   typedef typename BVH1::Index Index1;
0096   typedef typename BVH2::Index Index2;
0097   typedef internal::intersector_helper1<typename BVH1::Volume, typename BVH1::Object, typename BVH2::Object, Intersector> Helper1;
0098   typedef internal::intersector_helper2<typename BVH2::Volume, typename BVH2::Object, typename BVH1::Object, Intersector> Helper2;
0099   typedef typename BVH1::VolumeIterator VolIter1;
0100   typedef typename BVH1::ObjectIterator ObjIter1;
0101   typedef typename BVH2::VolumeIterator VolIter2;
0102   typedef typename BVH2::ObjectIterator ObjIter2;
0103
0104   VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1();
0105   ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1();
0106   VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2();
0107   ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2();
0108
0109   std::vector<std::pair<Index1, Index2> > todo(1, std::make_pair(tree1.getRootIndex(), tree2.getRootIndex()));
0110
0111   while(!todo.empty()) {
0112     tree1.getChildren(todo.back().first, vBegin1, vEnd1, oBegin1, oEnd1);
0113     tree2.getChildren(todo.back().second, vBegin2, vEnd2, oBegin2, oEnd2);
0114     todo.pop_back();
0115
0116     for(; vBegin1 != vEnd1; ++vBegin1) { //go through child volumes of first tree
0117       const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1);
0118       for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
0119         if(intersector.intersectVolumeVolume(vol1, tree2.getVolume(*vCur2)))
0120           todo.push_back(std::make_pair(*vBegin1, *vCur2));
0121       }
0122
0123       for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
0124         Helper1 helper(*oCur2, intersector);
0125         if(internal::intersect_helper(tree1, helper, *vBegin1))
0126           return; //intersector said to stop query
0127       }
0128     }
0129
0130     for(; oBegin1 != oEnd1; ++oBegin1) { //go through child objects of first tree
0131       for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
0132         Helper2 helper(*oBegin1, intersector);
0133         if(internal::intersect_helper(tree2, helper, *vCur2))
0134           return; //intersector said to stop query
0135       }
0136
0137       for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
0138         if(intersector.intersectObjectObject(*oBegin1, *oCur2))
0139           return; //intersector said to stop query
0140       }
0141     }
0142   }
0143 }
0144
0145 namespace internal {
0146
0147 #ifndef EIGEN_PARSED_BY_DOXYGEN
0148 template<typename BVH, typename Minimizer>
0149 typename Minimizer::Scalar minimize_helper(const BVH &tree, Minimizer &minimizer, typename BVH::Index root, typename Minimizer::Scalar minimum)
0150 {
0151   typedef typename Minimizer::Scalar Scalar;
0152   typedef typename BVH::Index Index;
0153   typedef std::pair<Scalar, Index> QueueElement; //first element is priority
0154   typedef typename BVH::VolumeIterator VolIter;
0155   typedef typename BVH::ObjectIterator ObjIter;
0156
0157   VolIter vBegin = VolIter(), vEnd = VolIter();
0158   ObjIter oBegin = ObjIter(), oEnd = ObjIter();
0159   std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement> > todo; //smallest is at the top
0160
0161   todo.push(std::make_pair(Scalar(), root));
0162
0163   while(!todo.empty()) {
0164     tree.getChildren(todo.top().second, vBegin, vEnd, oBegin, oEnd);
0165     todo.pop();
0166
0167     for(; oBegin != oEnd; ++oBegin) //go through child objects
0168       minimum = (std::min)(minimum, minimizer.minimumOnObject(*oBegin));
0169
0170     for(; vBegin != vEnd; ++vBegin) { //go through child volumes
0171       Scalar val = minimizer.minimumOnVolume(tree.getVolume(*vBegin));
0172       if(val < minimum)
0173         todo.push(std::make_pair(val, *vBegin));
0174     }
0175   }
0176
0177   return minimum;
0178 }
0179 #endif //not EIGEN_PARSED_BY_DOXYGEN
0180
0181
0182 template<typename Volume1, typename Object1, typename Object2, typename Minimizer>
0183 struct minimizer_helper1
0184 {
0185   typedef typename Minimizer::Scalar Scalar;
0186   minimizer_helper1(const Object2 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {}
0187   Scalar minimumOnVolume(const Volume1 &vol) { return minimizer.minimumOnVolumeObject(vol, stored); }
0188   Scalar minimumOnObject(const Object1 &obj) { return minimizer.minimumOnObjectObject(obj, stored); }
0189   Object2 stored;
0190   Minimizer &minimizer;
0191 private:
0192   minimizer_helper1& operator=(const minimizer_helper1&);
0193 };
0194
0195 template<typename Volume2, typename Object2, typename Object1, typename Minimizer>
0196 struct minimizer_helper2
0197 {
0198   typedef typename Minimizer::Scalar Scalar;
0199   minimizer_helper2(const Object1 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {}
0200   Scalar minimumOnVolume(const Volume2 &vol) { return minimizer.minimumOnObjectVolume(stored, vol); }
0201   Scalar minimumOnObject(const Object2 &obj) { return minimizer.minimumOnObjectObject(stored, obj); }
0202   Object1 stored;
0203   Minimizer &minimizer;
0204 private:
0205   minimizer_helper2& operator=(const minimizer_helper2&);
0206 };
0207
0208 } // end namespace internal
0209
0210 /**  Given a BVH, runs the query encapsulated by \a minimizer.
0211   *  \returns the minimum value.
0212   *  The Minimizer type must provide the following members: \code
0213      typedef Scalar //the numeric type of what is being minimized--not necessarily the Scalar type of the BVH (if it has one)
0214      Scalar minimumOnVolume(const BVH::Volume &volume)
0215      Scalar minimumOnObject(const BVH::Object &object)
0216   \endcode
0217   */
0218 template<typename BVH, typename Minimizer>
0219 typename Minimizer::Scalar BVMinimize(const BVH &tree, Minimizer &minimizer)
0220 {
0221   return internal::minimize_helper(tree, minimizer, tree.getRootIndex(), (std::numeric_limits<typename Minimizer::Scalar>::max)());
0222 }
0223
0224 /**  Given two BVH's, runs the query on their cartesian product encapsulated by \a minimizer.
0225   *  \returns the minimum value.
0226   *  The Minimizer type must provide the following members: \code
0227      typedef Scalar //the numeric type of what is being minimized--not necessarily the Scalar type of the BVH (if it has one)
0228      Scalar minimumOnVolumeVolume(const BVH1::Volume &v1, const BVH2::Volume &v2)
0229      Scalar minimumOnVolumeObject(const BVH1::Volume &v1, const BVH2::Object &o2)
0230      Scalar minimumOnObjectVolume(const BVH1::Object &o1, const BVH2::Volume &v2)
0231      Scalar minimumOnObjectObject(const BVH1::Object &o1, const BVH2::Object &o2)
0232   \endcode
0233   */
0234 template<typename BVH1, typename BVH2, typename Minimizer>
0235 typename Minimizer::Scalar BVMinimize(const BVH1 &tree1, const BVH2 &tree2, Minimizer &minimizer)
0236 {
0237   typedef typename Minimizer::Scalar Scalar;
0238   typedef typename BVH1::Index Index1;
0239   typedef typename BVH2::Index Index2;
0240   typedef internal::minimizer_helper1<typename BVH1::Volume, typename BVH1::Object, typename BVH2::Object, Minimizer> Helper1;
0241   typedef internal::minimizer_helper2<typename BVH2::Volume, typename BVH2::Object, typename BVH1::Object, Minimizer> Helper2;
0242   typedef std::pair<Scalar, std::pair<Index1, Index2> > QueueElement; //first element is priority
0243   typedef typename BVH1::VolumeIterator VolIter1;
0244   typedef typename BVH1::ObjectIterator ObjIter1;
0245   typedef typename BVH2::VolumeIterator VolIter2;
0246   typedef typename BVH2::ObjectIterator ObjIter2;
0247
0248   VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1();
0249   ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1();
0250   VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2();
0251   ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2();
0252   std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement> > todo; //smallest is at the top
0253
0254   Scalar minimum = (std::numeric_limits<Scalar>::max)();
0255   todo.push(std::make_pair(Scalar(), std::make_pair(tree1.getRootIndex(), tree2.getRootIndex())));
0256
0257   while(!todo.empty()) {
0258     tree1.getChildren(todo.top().second.first, vBegin1, vEnd1, oBegin1, oEnd1);
0259     tree2.getChildren(todo.top().second.second, vBegin2, vEnd2, oBegin2, oEnd2);
0260     todo.pop();
0261
0262     for(; oBegin1 != oEnd1; ++oBegin1) { //go through child objects of first tree
0263       for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
0264         minimum = (std::min)(minimum, minimizer.minimumOnObjectObject(*oBegin1, *oCur2));
0265       }
0266
0267       for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
0268         Helper2 helper(*oBegin1, minimizer);
0269         minimum = (std::min)(minimum, internal::minimize_helper(tree2, helper, *vCur2, minimum));
0270       }
0271     }
0272
0273     for(; vBegin1 != vEnd1; ++vBegin1) { //go through child volumes of first tree
0274       const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1);
0275
0276       for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
0277         Helper1 helper(*oCur2, minimizer);
0278         minimum = (std::min)(minimum, internal::minimize_helper(tree1, helper, *vBegin1, minimum));
0279       }
0280
0281       for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
0282         Scalar val = minimizer.minimumOnVolumeVolume(vol1, tree2.getVolume(*vCur2));
0283         if(val < minimum)
0284           todo.push(std::make_pair(val, std::make_pair(*vBegin1, *vCur2)));
0285       }
0286     }
0287   }
0288   return minimum;
0289 }
0290
0291 } // end namespace Eigen
0292
0293 #endif // EIGEN_BVALGORITHMS_H