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