1 // This file is part of Eigen, a lightweight C++ template library 2 // for linear algebra. 3 // 4 // Copyright (C) 2009 Ilya Baran <ibaran (at) mit.edu> 5 // 6 // This Source Code Form is subject to the terms of the Mozilla 7 // Public License v. 2.0. If a copy of the MPL was not distributed 8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 10 #ifndef EIGEN_BVALGORITHMS_H 11 #define EIGEN_BVALGORITHMS_H 12 13 namespace Eigen { 14 15 namespace internal { 16 17 #ifndef EIGEN_PARSED_BY_DOXYGEN 18 template<typename BVH, typename Intersector> 19 bool intersect_helper(const BVH &tree, Intersector &intersector, typename BVH::Index root) 20 { 21 typedef typename BVH::Index Index; 22 typedef typename BVH::VolumeIterator VolIter; 23 typedef typename BVH::ObjectIterator ObjIter; 24 25 VolIter vBegin = VolIter(), vEnd = VolIter(); 26 ObjIter oBegin = ObjIter(), oEnd = ObjIter(); 27 28 std::vector<Index> todo(1, root); 29 30 while(!todo.empty()) { 31 tree.getChildren(todo.back(), vBegin, vEnd, oBegin, oEnd); 32 todo.pop_back(); 33 34 for(; vBegin != vEnd; ++vBegin) //go through child volumes 35 if(intersector.intersectVolume(tree.getVolume(*vBegin))) 36 todo.push_back(*vBegin); 37 38 for(; oBegin != oEnd; ++oBegin) //go through child objects 39 if(intersector.intersectObject(*oBegin)) 40 return true; //intersector said to stop query 41 } 42 return false; 43 } 44 #endif //not EIGEN_PARSED_BY_DOXYGEN 45 46 template<typename Volume1, typename Object1, typename Object2, typename Intersector> 47 struct intersector_helper1 48 { 49 intersector_helper1(const Object2 &inStored, Intersector &in) : stored(inStored), intersector(in) {} 50 bool intersectVolume(const Volume1 &vol) { return intersector.intersectVolumeObject(vol, stored); } 51 bool intersectObject(const Object1 &obj) { return intersector.intersectObjectObject(obj, stored); } 52 Object2 stored; 53 Intersector &intersector; 54 private: 55 intersector_helper1& operator=(const intersector_helper1&); 56 }; 57 58 template<typename Volume2, typename Object2, typename Object1, typename Intersector> 59 struct intersector_helper2 60 { 61 intersector_helper2(const Object1 &inStored, Intersector &in) : stored(inStored), intersector(in) {} 62 bool intersectVolume(const Volume2 &vol) { return intersector.intersectObjectVolume(stored, vol); } 63 bool intersectObject(const Object2 &obj) { return intersector.intersectObjectObject(stored, obj); } 64 Object1 stored; 65 Intersector &intersector; 66 private: 67 intersector_helper2& operator=(const intersector_helper2&); 68 }; 69 70 } // end namespace internal 71 72 /** Given a BVH, runs the query encapsulated by \a intersector. 73 * The Intersector type must provide the following members: \code 74 bool intersectVolume(const BVH::Volume &volume) //returns true if volume intersects the query 75 bool intersectObject(const BVH::Object &object) //returns true if the search should terminate immediately 76 \endcode 77 */ 78 template<typename BVH, typename Intersector> 79 void BVIntersect(const BVH &tree, Intersector &intersector) 80 { 81 internal::intersect_helper(tree, intersector, tree.getRootIndex()); 82 } 83 84 /** Given two BVH's, runs the query on their Cartesian product encapsulated by \a intersector. 85 * The Intersector type must provide the following members: \code 86 bool intersectVolumeVolume(const BVH1::Volume &v1, const BVH2::Volume &v2) //returns true if product of volumes intersects the query 87 bool intersectVolumeObject(const BVH1::Volume &v1, const BVH2::Object &o2) //returns true if the volume-object product intersects the query 88 bool intersectObjectVolume(const BVH1::Object &o1, const BVH2::Volume &v2) //returns true if the volume-object product intersects the query 89 bool intersectObjectObject(const BVH1::Object &o1, const BVH2::Object &o2) //returns true if the search should terminate immediately 90 \endcode 91 */ 92 template<typename BVH1, typename BVH2, typename Intersector> 93 void BVIntersect(const BVH1 &tree1, const BVH2 &tree2, Intersector &intersector) //TODO: tandem descent when it makes sense 94 { 95 typedef typename BVH1::Index Index1; 96 typedef typename BVH2::Index Index2; 97 typedef internal::intersector_helper1<typename BVH1::Volume, typename BVH1::Object, typename BVH2::Object, Intersector> Helper1; 98 typedef internal::intersector_helper2<typename BVH2::Volume, typename BVH2::Object, typename BVH1::Object, Intersector> Helper2; 99 typedef typename BVH1::VolumeIterator VolIter1; 100 typedef typename BVH1::ObjectIterator ObjIter1; 101 typedef typename BVH2::VolumeIterator VolIter2; 102 typedef typename BVH2::ObjectIterator ObjIter2; 103 104 VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1(); 105 ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1(); 106 VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2(); 107 ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2(); 108 109 std::vector<std::pair<Index1, Index2> > todo(1, std::make_pair(tree1.getRootIndex(), tree2.getRootIndex())); 110 111 while(!todo.empty()) { 112 tree1.getChildren(todo.back().first, vBegin1, vEnd1, oBegin1, oEnd1); 113 tree2.getChildren(todo.back().second, vBegin2, vEnd2, oBegin2, oEnd2); 114 todo.pop_back(); 115 116 for(; vBegin1 != vEnd1; ++vBegin1) { //go through child volumes of first tree 117 const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1); 118 for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree 119 if(intersector.intersectVolumeVolume(vol1, tree2.getVolume(*vCur2))) 120 todo.push_back(std::make_pair(*vBegin1, *vCur2)); 121 } 122 123 for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree 124 Helper1 helper(*oCur2, intersector); 125 if(internal::intersect_helper(tree1, helper, *vBegin1)) 126 return; //intersector said to stop query 127 } 128 } 129 130 for(; oBegin1 != oEnd1; ++oBegin1) { //go through child objects of first tree 131 for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree 132 Helper2 helper(*oBegin1, intersector); 133 if(internal::intersect_helper(tree2, helper, *vCur2)) 134 return; //intersector said to stop query 135 } 136 137 for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree 138 if(intersector.intersectObjectObject(*oBegin1, *oCur2)) 139 return; //intersector said to stop query 140 } 141 } 142 } 143 } 144 145 namespace internal { 146 147 #ifndef EIGEN_PARSED_BY_DOXYGEN 148 template<typename BVH, typename Minimizer> 149 typename Minimizer::Scalar minimize_helper(const BVH &tree, Minimizer &minimizer, typename BVH::Index root, typename Minimizer::Scalar minimum) 150 { 151 typedef typename Minimizer::Scalar Scalar; 152 typedef typename BVH::Index Index; 153 typedef std::pair<Scalar, Index> QueueElement; //first element is priority 154 typedef typename BVH::VolumeIterator VolIter; 155 typedef typename BVH::ObjectIterator ObjIter; 156 157 VolIter vBegin = VolIter(), vEnd = VolIter(); 158 ObjIter oBegin = ObjIter(), oEnd = ObjIter(); 159 std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement> > todo; //smallest is at the top 160 161 todo.push(std::make_pair(Scalar(), root)); 162 163 while(!todo.empty()) { 164 tree.getChildren(todo.top().second, vBegin, vEnd, oBegin, oEnd); 165 todo.pop(); 166 167 for(; oBegin != oEnd; ++oBegin) //go through child objects 168 minimum = (std::min)(minimum, minimizer.minimumOnObject(*oBegin)); 169 170 for(; vBegin != vEnd; ++vBegin) { //go through child volumes 171 Scalar val = minimizer.minimumOnVolume(tree.getVolume(*vBegin)); 172 if(val < minimum) 173 todo.push(std::make_pair(val, *vBegin)); 174 } 175 } 176 177 return minimum; 178 } 179 #endif //not EIGEN_PARSED_BY_DOXYGEN 180 181 182 template<typename Volume1, typename Object1, typename Object2, typename Minimizer> 183 struct minimizer_helper1 184 { 185 typedef typename Minimizer::Scalar Scalar; 186 minimizer_helper1(const Object2 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {} 187 Scalar minimumOnVolume(const Volume1 &vol) { return minimizer.minimumOnVolumeObject(vol, stored); } 188 Scalar minimumOnObject(const Object1 &obj) { return minimizer.minimumOnObjectObject(obj, stored); } 189 Object2 stored; 190 Minimizer &minimizer; 191 private: 192 minimizer_helper1& operator=(const minimizer_helper1&); 193 }; 194 195 template<typename Volume2, typename Object2, typename Object1, typename Minimizer> 196 struct minimizer_helper2 197 { 198 typedef typename Minimizer::Scalar Scalar; 199 minimizer_helper2(const Object1 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {} 200 Scalar minimumOnVolume(const Volume2 &vol) { return minimizer.minimumOnObjectVolume(stored, vol); } 201 Scalar minimumOnObject(const Object2 &obj) { return minimizer.minimumOnObjectObject(stored, obj); } 202 Object1 stored; 203 Minimizer &minimizer; 204 private: 205 minimizer_helper2& operator=(const minimizer_helper2&); 206 }; 207 208 } // end namespace internal 209 210 /** Given a BVH, runs the query encapsulated by \a minimizer. 211 * \returns the minimum value. 212 * The Minimizer type must provide the following members: \code 213 typedef Scalar //the numeric type of what is being minimized--not necessarily the Scalar type of the BVH (if it has one) 214 Scalar minimumOnVolume(const BVH::Volume &volume) 215 Scalar minimumOnObject(const BVH::Object &object) 216 \endcode 217 */ 218 template<typename BVH, typename Minimizer> 219 typename Minimizer::Scalar BVMinimize(const BVH &tree, Minimizer &minimizer) 220 { 221 return internal::minimize_helper(tree, minimizer, tree.getRootIndex(), (std::numeric_limits<typename Minimizer::Scalar>::max)()); 222 } 223 224 /** Given two BVH's, runs the query on their cartesian product encapsulated by \a minimizer. 225 * \returns the minimum value. 226 * The Minimizer type must provide the following members: \code 227 typedef Scalar //the numeric type of what is being minimized--not necessarily the Scalar type of the BVH (if it has one) 228 Scalar minimumOnVolumeVolume(const BVH1::Volume &v1, const BVH2::Volume &v2) 229 Scalar minimumOnVolumeObject(const BVH1::Volume &v1, const BVH2::Object &o2) 230 Scalar minimumOnObjectVolume(const BVH1::Object &o1, const BVH2::Volume &v2) 231 Scalar minimumOnObjectObject(const BVH1::Object &o1, const BVH2::Object &o2) 232 \endcode 233 */ 234 template<typename BVH1, typename BVH2, typename Minimizer> 235 typename Minimizer::Scalar BVMinimize(const BVH1 &tree1, const BVH2 &tree2, Minimizer &minimizer) 236 { 237 typedef typename Minimizer::Scalar Scalar; 238 typedef typename BVH1::Index Index1; 239 typedef typename BVH2::Index Index2; 240 typedef internal::minimizer_helper1<typename BVH1::Volume, typename BVH1::Object, typename BVH2::Object, Minimizer> Helper1; 241 typedef internal::minimizer_helper2<typename BVH2::Volume, typename BVH2::Object, typename BVH1::Object, Minimizer> Helper2; 242 typedef std::pair<Scalar, std::pair<Index1, Index2> > QueueElement; //first element is priority 243 typedef typename BVH1::VolumeIterator VolIter1; 244 typedef typename BVH1::ObjectIterator ObjIter1; 245 typedef typename BVH2::VolumeIterator VolIter2; 246 typedef typename BVH2::ObjectIterator ObjIter2; 247 248 VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1(); 249 ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1(); 250 VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2(); 251 ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2(); 252 std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement> > todo; //smallest is at the top 253 254 Scalar minimum = (std::numeric_limits<Scalar>::max)(); 255 todo.push(std::make_pair(Scalar(), std::make_pair(tree1.getRootIndex(), tree2.getRootIndex()))); 256 257 while(!todo.empty()) { 258 tree1.getChildren(todo.top().second.first, vBegin1, vEnd1, oBegin1, oEnd1); 259 tree2.getChildren(todo.top().second.second, vBegin2, vEnd2, oBegin2, oEnd2); 260 todo.pop(); 261 262 for(; oBegin1 != oEnd1; ++oBegin1) { //go through child objects of first tree 263 for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree 264 minimum = (std::min)(minimum, minimizer.minimumOnObjectObject(*oBegin1, *oCur2)); 265 } 266 267 for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree 268 Helper2 helper(*oBegin1, minimizer); 269 minimum = (std::min)(minimum, internal::minimize_helper(tree2, helper, *vCur2, minimum)); 270 } 271 } 272 273 for(; vBegin1 != vEnd1; ++vBegin1) { //go through child volumes of first tree 274 const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1); 275 276 for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree 277 Helper1 helper(*oCur2, minimizer); 278 minimum = (std::min)(minimum, internal::minimize_helper(tree1, helper, *vBegin1, minimum)); 279 } 280 281 for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree 282 Scalar val = minimizer.minimumOnVolumeVolume(vol1, tree2.getVolume(*vCur2)); 283 if(val < minimum) 284 todo.push(std::make_pair(val, std::make_pair(*vBegin1, *vCur2))); 285 } 286 } 287 } 288 return minimum; 289 } 290 291 } // end namespace Eigen 292 293 #endif // EIGEN_BVALGORITHMS_H 294
