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


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 11:57:54