BVAlgorithms.h
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 EIGEN_BVALGORITHMS_H
00026 #define EIGEN_BVALGORITHMS_H
00027 
00028 namespace Eigen { 
00029 
00030 namespace internal {
00031 
00032 #ifndef EIGEN_PARSED_BY_DOXYGEN
00033 template<typename BVH, typename Intersector>
00034 bool intersect_helper(const BVH &tree, Intersector &intersector, typename BVH::Index root)
00035 {
00036   typedef typename BVH::Index Index;
00037   typedef typename BVH::VolumeIterator VolIter;
00038   typedef typename BVH::ObjectIterator ObjIter;
00039 
00040   VolIter vBegin = VolIter(), vEnd = VolIter();
00041   ObjIter oBegin = ObjIter(), oEnd = ObjIter();
00042 
00043   std::vector<Index> todo(1, root);
00044 
00045   while(!todo.empty()) {
00046     tree.getChildren(todo.back(), vBegin, vEnd, oBegin, oEnd);
00047     todo.pop_back();
00048 
00049     for(; vBegin != vEnd; ++vBegin) //go through child volumes
00050       if(intersector.intersectVolume(tree.getVolume(*vBegin)))
00051         todo.push_back(*vBegin);
00052 
00053     for(; oBegin != oEnd; ++oBegin) //go through child objects
00054       if(intersector.intersectObject(*oBegin))
00055         return true; //intersector said to stop query
00056   }
00057   return false;
00058 }
00059 #endif //not EIGEN_PARSED_BY_DOXYGEN
00060 
00061 template<typename Volume1, typename Object1, typename Object2, typename Intersector>
00062 struct intersector_helper1
00063 {
00064   intersector_helper1(const Object2 &inStored, Intersector &in) : stored(inStored), intersector(in) {}
00065   bool intersectVolume(const Volume1 &vol) { return intersector.intersectVolumeObject(vol, stored); }
00066   bool intersectObject(const Object1 &obj) { return intersector.intersectObjectObject(obj, stored); }
00067   Object2 stored;
00068   Intersector &intersector;
00069 private:
00070   intersector_helper1& operator=(const intersector_helper1&);
00071 };
00072 
00073 template<typename Volume2, typename Object2, typename Object1, typename Intersector>
00074 struct intersector_helper2
00075 {
00076   intersector_helper2(const Object1 &inStored, Intersector &in) : stored(inStored), intersector(in) {}
00077   bool intersectVolume(const Volume2 &vol) { return intersector.intersectObjectVolume(stored, vol); }
00078   bool intersectObject(const Object2 &obj) { return intersector.intersectObjectObject(stored, obj); }
00079   Object1 stored;
00080   Intersector &intersector;
00081 private:
00082   intersector_helper2& operator=(const intersector_helper2&);
00083 };
00084 
00085 } // end namespace internal
00086 
00093 template<typename BVH, typename Intersector>
00094 void BVIntersect(const BVH &tree, Intersector &intersector)
00095 {
00096   internal::intersect_helper(tree, intersector, tree.getRootIndex());
00097 }
00098 
00107 template<typename BVH1, typename BVH2, typename Intersector>
00108 void BVIntersect(const BVH1 &tree1, const BVH2 &tree2, Intersector &intersector) //TODO: tandem descent when it makes sense
00109 {
00110   typedef typename BVH1::Index Index1;
00111   typedef typename BVH2::Index Index2;
00112   typedef internal::intersector_helper1<typename BVH1::Volume, typename BVH1::Object, typename BVH2::Object, Intersector> Helper1;
00113   typedef internal::intersector_helper2<typename BVH2::Volume, typename BVH2::Object, typename BVH1::Object, Intersector> Helper2;
00114   typedef typename BVH1::VolumeIterator VolIter1;
00115   typedef typename BVH1::ObjectIterator ObjIter1;
00116   typedef typename BVH2::VolumeIterator VolIter2;
00117   typedef typename BVH2::ObjectIterator ObjIter2;
00118 
00119   VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1();
00120   ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1();
00121   VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2();
00122   ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2();
00123 
00124   std::vector<std::pair<Index1, Index2> > todo(1, std::make_pair(tree1.getRootIndex(), tree2.getRootIndex()));
00125 
00126   while(!todo.empty()) {
00127     tree1.getChildren(todo.back().first, vBegin1, vEnd1, oBegin1, oEnd1);
00128     tree2.getChildren(todo.back().second, vBegin2, vEnd2, oBegin2, oEnd2);
00129     todo.pop_back();
00130 
00131     for(; vBegin1 != vEnd1; ++vBegin1) { //go through child volumes of first tree
00132       const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1);
00133       for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
00134         if(intersector.intersectVolumeVolume(vol1, tree2.getVolume(*vCur2)))
00135           todo.push_back(std::make_pair(*vBegin1, *vCur2));
00136       }
00137 
00138       for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
00139         Helper1 helper(*oCur2, intersector);
00140         if(internal::intersect_helper(tree1, helper, *vBegin1))
00141           return; //intersector said to stop query
00142       }
00143     }
00144 
00145     for(; oBegin1 != oEnd1; ++oBegin1) { //go through child objects of first tree
00146       for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
00147         Helper2 helper(*oBegin1, intersector);
00148         if(internal::intersect_helper(tree2, helper, *vCur2))
00149           return; //intersector said to stop query
00150       }
00151 
00152       for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
00153         if(intersector.intersectObjectObject(*oBegin1, *oCur2))
00154           return; //intersector said to stop query
00155       }
00156     }
00157   }
00158 }
00159 
00160 namespace internal {
00161 
00162 #ifndef EIGEN_PARSED_BY_DOXYGEN
00163 template<typename BVH, typename Minimizer>
00164 typename Minimizer::Scalar minimize_helper(const BVH &tree, Minimizer &minimizer, typename BVH::Index root, typename Minimizer::Scalar minimum)
00165 {
00166   typedef typename Minimizer::Scalar Scalar;
00167   typedef typename BVH::Index Index;
00168   typedef std::pair<Scalar, Index> QueueElement; //first element is priority
00169   typedef typename BVH::VolumeIterator VolIter;
00170   typedef typename BVH::ObjectIterator ObjIter;
00171 
00172   VolIter vBegin = VolIter(), vEnd = VolIter();
00173   ObjIter oBegin = ObjIter(), oEnd = ObjIter();
00174   std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement> > todo; //smallest is at the top
00175 
00176   todo.push(std::make_pair(Scalar(), root));
00177 
00178   while(!todo.empty()) {
00179     tree.getChildren(todo.top().second, vBegin, vEnd, oBegin, oEnd);
00180     todo.pop();
00181 
00182     for(; oBegin != oEnd; ++oBegin) //go through child objects
00183       minimum = (std::min)(minimum, minimizer.minimumOnObject(*oBegin));
00184 
00185     for(; vBegin != vEnd; ++vBegin) { //go through child volumes
00186       Scalar val = minimizer.minimumOnVolume(tree.getVolume(*vBegin));
00187       if(val < minimum)
00188         todo.push(std::make_pair(val, *vBegin));
00189     }
00190   }
00191 
00192   return minimum;
00193 }
00194 #endif //not EIGEN_PARSED_BY_DOXYGEN
00195 
00196 
00197 template<typename Volume1, typename Object1, typename Object2, typename Minimizer>
00198 struct minimizer_helper1
00199 {
00200   typedef typename Minimizer::Scalar Scalar;
00201   minimizer_helper1(const Object2 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {}
00202   Scalar minimumOnVolume(const Volume1 &vol) { return minimizer.minimumOnVolumeObject(vol, stored); }
00203   Scalar minimumOnObject(const Object1 &obj) { return minimizer.minimumOnObjectObject(obj, stored); }
00204   Object2 stored;
00205   Minimizer &minimizer;
00206 private:
00207   minimizer_helper1& operator=(const minimizer_helper1&) {}
00208 };
00209 
00210 template<typename Volume2, typename Object2, typename Object1, typename Minimizer>
00211 struct minimizer_helper2
00212 {
00213   typedef typename Minimizer::Scalar Scalar;
00214   minimizer_helper2(const Object1 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {}
00215   Scalar minimumOnVolume(const Volume2 &vol) { return minimizer.minimumOnObjectVolume(stored, vol); }
00216   Scalar minimumOnObject(const Object2 &obj) { return minimizer.minimumOnObjectObject(stored, obj); }
00217   Object1 stored;
00218   Minimizer &minimizer;
00219 private:
00220   minimizer_helper2& operator=(const minimizer_helper2&);
00221 };
00222 
00223 } // end namespace internal
00224 
00233 template<typename BVH, typename Minimizer>
00234 typename Minimizer::Scalar BVMinimize(const BVH &tree, Minimizer &minimizer)
00235 {
00236   return internal::minimize_helper(tree, minimizer, tree.getRootIndex(), (std::numeric_limits<typename Minimizer::Scalar>::max)());
00237 }
00238 
00249 template<typename BVH1, typename BVH2, typename Minimizer>
00250 typename Minimizer::Scalar BVMinimize(const BVH1 &tree1, const BVH2 &tree2, Minimizer &minimizer)
00251 {
00252   typedef typename Minimizer::Scalar Scalar;
00253   typedef typename BVH1::Index Index1;
00254   typedef typename BVH2::Index Index2;
00255   typedef internal::minimizer_helper1<typename BVH1::Volume, typename BVH1::Object, typename BVH2::Object, Minimizer> Helper1;
00256   typedef internal::minimizer_helper2<typename BVH2::Volume, typename BVH2::Object, typename BVH1::Object, Minimizer> Helper2;
00257   typedef std::pair<Scalar, std::pair<Index1, Index2> > QueueElement; //first element is priority
00258   typedef typename BVH1::VolumeIterator VolIter1;
00259   typedef typename BVH1::ObjectIterator ObjIter1;
00260   typedef typename BVH2::VolumeIterator VolIter2;
00261   typedef typename BVH2::ObjectIterator ObjIter2;
00262 
00263   VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1();
00264   ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1();
00265   VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2();
00266   ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2();
00267   std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement> > todo; //smallest is at the top
00268 
00269   Scalar minimum = (std::numeric_limits<Scalar>::max)();
00270   todo.push(std::make_pair(Scalar(), std::make_pair(tree1.getRootIndex(), tree2.getRootIndex())));
00271 
00272   while(!todo.empty()) {
00273     tree1.getChildren(todo.top().second.first, vBegin1, vEnd1, oBegin1, oEnd1);
00274     tree2.getChildren(todo.top().second.second, vBegin2, vEnd2, oBegin2, oEnd2);
00275     todo.pop();
00276 
00277     for(; oBegin1 != oEnd1; ++oBegin1) { //go through child objects of first tree
00278       for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
00279         minimum = (std::min)(minimum, minimizer.minimumOnObjectObject(*oBegin1, *oCur2));
00280       }
00281 
00282       for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
00283         Helper2 helper(*oBegin1, minimizer);
00284         minimum = (std::min)(minimum, internal::minimize_helper(tree2, helper, *vCur2, minimum));
00285       }
00286     }
00287 
00288     for(; vBegin1 != vEnd1; ++vBegin1) { //go through child volumes of first tree
00289       const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1);
00290 
00291       for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
00292         Helper1 helper(*oCur2, minimizer);
00293         minimum = (std::min)(minimum, internal::minimize_helper(tree1, helper, *vBegin1, minimum));
00294       }
00295 
00296       for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
00297         Scalar val = minimizer.minimumOnVolumeVolume(vol1, tree2.getVolume(*vCur2));
00298         if(val < minimum)
00299           todo.push(std::make_pair(val, std::make_pair(*vBegin1, *vCur2)));
00300       }
00301     }
00302   }
00303   return minimum;
00304 }
00305 
00306 } // end namespace Eigen
00307 
00308 #endif // EIGEN_BVALGORITHMS_H