blob: 47c49be7f0cd2a93683ba373178d433b3ada5e7a [file] [log] [blame]
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Ilya Baran <ibaran@mit.edu>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_BVALGORITHMS_H
#define EIGEN_BVALGORITHMS_H
/** Given a BVH, runs the query encapsulated by \a intersector.
* The Intersector type must provide the following members: \code
bool intersectVolume(const BVH::Volume &volume) //returns true if volume intersects the query
bool intersectObject(const BVH::Object &object) //returns true if the search should terminate immediately
\endcode
*/
template<typename BVH, typename Intersector>
void BVIntersect(const BVH &tree, Intersector &intersector)
{
ei_intersect_helper(tree, intersector, tree.getRootIndex());
}
#ifndef EIGEN_PARSED_BY_DOXYGEN
template<typename BVH, typename Intersector>
bool ei_intersect_helper(const BVH &tree, Intersector &intersector, typename BVH::Index root)
{
typedef typename BVH::Index Index;
typedef typename BVH::VolumeIterator VolIter;
typedef typename BVH::ObjectIterator ObjIter;
VolIter vBegin = VolIter(), vEnd = VolIter();
ObjIter oBegin = ObjIter(), oEnd = ObjIter();
std::vector<Index> todo(1, root);
while(!todo.empty()) {
tree.getChildren(todo.back(), vBegin, vEnd, oBegin, oEnd);
todo.pop_back();
for(; vBegin != vEnd; ++vBegin) //go through child volumes
if(intersector.intersectVolume(tree.getVolume(*vBegin)))
todo.push_back(*vBegin);
for(; oBegin != oEnd; ++oBegin) //go through child objects
if(intersector.intersectObject(*oBegin))
return true; //intersector said to stop query
}
return false;
}
#endif //not EIGEN_PARSED_BY_DOXYGEN
template<typename Volume1, typename Object1, typename Object2, typename Intersector>
struct ei_intersector_helper1
{
ei_intersector_helper1(const Object2 &inStored, Intersector &in) : stored(inStored), intersector(in) {}
bool intersectVolume(const Volume1 &vol) { return intersector.intersectVolumeObject(vol, stored); }
bool intersectObject(const Object1 &obj) { return intersector.intersectObjectObject(obj, stored); }
Object2 stored;
Intersector &intersector;
};
template<typename Volume2, typename Object2, typename Object1, typename Intersector>
struct ei_intersector_helper2
{
ei_intersector_helper2(const Object1 &inStored, Intersector &in) : stored(inStored), intersector(in) {}
bool intersectVolume(const Volume2 &vol) { return intersector.intersectObjectVolume(stored, vol); }
bool intersectObject(const Object2 &obj) { return intersector.intersectObjectObject(stored, obj); }
Object1 stored;
Intersector &intersector;
};
/** Given two BVH's, runs the query on their cartesian product encapsulated by \a intersector.
* The Intersector type must provide the following members: \code
bool intersectVolumeVolume(const BVH1::Volume &v1, const BVH2::Volume &v2) //returns true if product of volumes intersects the query
bool intersectVolumeObject(const BVH1::Volume &v1, const BVH2::Object &o2) //returns true if the volume-object product intersects the query
bool intersectObjectVolume(const BVH1::Object &o1, const BVH2::Volume &v2) //returns true if the volume-object product intersects the query
bool intersectObjectObject(const BVH1::Object &o1, const BVH2::Object &o2) //returns true if the search should terminate immediately
\endcode
*/
template<typename BVH1, typename BVH2, typename Intersector>
void BVIntersect(const BVH1 &tree1, const BVH2 &tree2, Intersector &intersector) //TODO: tandem descent when it makes sense
{
typedef typename BVH1::Index Index1;
typedef typename BVH2::Index Index2;
typedef ei_intersector_helper1<typename BVH1::Volume, typename BVH1::Object, typename BVH2::Object, Intersector> Helper1;
typedef ei_intersector_helper2<typename BVH2::Volume, typename BVH2::Object, typename BVH1::Object, Intersector> Helper2;
typedef typename BVH1::VolumeIterator VolIter1;
typedef typename BVH1::ObjectIterator ObjIter1;
typedef typename BVH2::VolumeIterator VolIter2;
typedef typename BVH2::ObjectIterator ObjIter2;
VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1();
ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1();
VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2();
ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2();
std::vector<std::pair<Index1, Index2> > todo(1, std::make_pair(tree1.getRootIndex(), tree2.getRootIndex()));
while(!todo.empty()) {
tree1.getChildren(todo.back().first, vBegin1, vEnd1, oBegin1, oEnd1);
tree2.getChildren(todo.back().second, vBegin2, vEnd2, oBegin2, oEnd2);
todo.pop_back();
for(; vBegin1 != vEnd1; ++vBegin1) { //go through child volumes of first tree
const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1);
for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
if(intersector.intersectVolumeVolume(vol1, tree2.getVolume(*vCur2)))
todo.push_back(std::make_pair(*vBegin1, *vCur2));
}
for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
Helper1 helper(*oCur2, intersector);
if(ei_intersect_helper(tree1, helper, *vBegin1))
return; //intersector said to stop query
}
}
for(; oBegin1 != oEnd1; ++oBegin1) { //go through child objects of first tree
for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
Helper2 helper(*oBegin1, intersector);
if(ei_intersect_helper(tree2, helper, *vCur2))
return; //intersector said to stop query
}
for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
if(intersector.intersectObjectObject(*oBegin1, *oCur2))
return; //intersector said to stop query
}
}
}
}
/** Given a BVH, runs the query encapsulated by \a minimizer.
* \returns the minimum value.
* The Minimizer type must provide the following members: \code
typedef Scalar //the numeric type of what is being minimized--not necessarily the Scalar type of the BVH (if it has one)
Scalar minimumOnVolume(const BVH::Volume &volume)
Scalar minimumOnObject(const BVH::Object &object)
\endcode
*/
template<typename BVH, typename Minimizer>
typename Minimizer::Scalar BVMinimize(const BVH &tree, Minimizer &minimizer)
{
return ei_minimize_helper(tree, minimizer, tree.getRootIndex(), std::numeric_limits<typename Minimizer::Scalar>::max());
}
#ifndef EIGEN_PARSED_BY_DOXYGEN
template<typename BVH, typename Minimizer>
typename Minimizer::Scalar ei_minimize_helper(const BVH &tree, Minimizer &minimizer, typename BVH::Index root, typename Minimizer::Scalar minimum)
{
typedef typename Minimizer::Scalar Scalar;
typedef typename BVH::Index Index;
typedef std::pair<Scalar, Index> QueueElement; //first element is priority
typedef typename BVH::VolumeIterator VolIter;
typedef typename BVH::ObjectIterator ObjIter;
VolIter vBegin = VolIter(), vEnd = VolIter();
ObjIter oBegin = ObjIter(), oEnd = ObjIter();
std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement> > todo; //smallest is at the top
todo.push(std::make_pair(Scalar(), root));
while(!todo.empty()) {
tree.getChildren(todo.top().second, vBegin, vEnd, oBegin, oEnd);
todo.pop();
for(; oBegin != oEnd; ++oBegin) //go through child objects
minimum = std::min(minimum, minimizer.minimumOnObject(*oBegin));
for(; vBegin != vEnd; ++vBegin) { //go through child volumes
Scalar val = minimizer.minimumOnVolume(tree.getVolume(*vBegin));
if(val < minimum)
todo.push(std::make_pair(val, *vBegin));
}
}
return minimum;
}
#endif //not EIGEN_PARSED_BY_DOXYGEN
template<typename Volume1, typename Object1, typename Object2, typename Minimizer>
struct ei_minimizer_helper1
{
typedef typename Minimizer::Scalar Scalar;
ei_minimizer_helper1(const Object2 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {}
Scalar minimumOnVolume(const Volume1 &vol) { return minimizer.minimumOnVolumeObject(vol, stored); }
Scalar minimumOnObject(const Object1 &obj) { return minimizer.minimumOnObjectObject(obj, stored); }
Object2 stored;
Minimizer &minimizer;
};
template<typename Volume2, typename Object2, typename Object1, typename Minimizer>
struct ei_minimizer_helper2
{
typedef typename Minimizer::Scalar Scalar;
ei_minimizer_helper2(const Object1 &inStored, Minimizer &m) : stored(inStored), minimizer(m) {}
Scalar minimumOnVolume(const Volume2 &vol) { return minimizer.minimumOnObjectVolume(stored, vol); }
Scalar minimumOnObject(const Object2 &obj) { return minimizer.minimumOnObjectObject(stored, obj); }
Object1 stored;
Minimizer &minimizer;
};
/** Given two BVH's, runs the query on their cartesian product encapsulated by \a minimizer.
* \returns the minimum value.
* The Minimizer type must provide the following members: \code
typedef Scalar //the numeric type of what is being minimized--not necessarily the Scalar type of the BVH (if it has one)
Scalar minimumOnVolumeVolume(const BVH1::Volume &v1, const BVH2::Volume &v2)
Scalar minimumOnVolumeObject(const BVH1::Volume &v1, const BVH2::Object &o2)
Scalar minimumOnObjectVolume(const BVH1::Object &o1, const BVH2::Volume &v2)
Scalar minimumOnObjectObject(const BVH1::Object &o1, const BVH2::Object &o2)
\endcode
*/
template<typename BVH1, typename BVH2, typename Minimizer>
typename Minimizer::Scalar BVMinimize(const BVH1 &tree1, const BVH2 &tree2, Minimizer &minimizer)
{
typedef typename Minimizer::Scalar Scalar;
typedef typename BVH1::Index Index1;
typedef typename BVH2::Index Index2;
typedef ei_minimizer_helper1<typename BVH1::Volume, typename BVH1::Object, typename BVH2::Object, Minimizer> Helper1;
typedef ei_minimizer_helper2<typename BVH2::Volume, typename BVH2::Object, typename BVH1::Object, Minimizer> Helper2;
typedef std::pair<Scalar, std::pair<Index1, Index2> > QueueElement; //first element is priority
typedef typename BVH1::VolumeIterator VolIter1;
typedef typename BVH1::ObjectIterator ObjIter1;
typedef typename BVH2::VolumeIterator VolIter2;
typedef typename BVH2::ObjectIterator ObjIter2;
VolIter1 vBegin1 = VolIter1(), vEnd1 = VolIter1();
ObjIter1 oBegin1 = ObjIter1(), oEnd1 = ObjIter1();
VolIter2 vBegin2 = VolIter2(), vEnd2 = VolIter2(), vCur2 = VolIter2();
ObjIter2 oBegin2 = ObjIter2(), oEnd2 = ObjIter2(), oCur2 = ObjIter2();
std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement> > todo; //smallest is at the top
Scalar minimum = std::numeric_limits<Scalar>::max();
todo.push(std::make_pair(Scalar(), std::make_pair(tree1.getRootIndex(), tree2.getRootIndex())));
while(!todo.empty()) {
tree1.getChildren(todo.top().second.first, vBegin1, vEnd1, oBegin1, oEnd1);
tree2.getChildren(todo.top().second.second, vBegin2, vEnd2, oBegin2, oEnd2);
todo.pop();
for(; oBegin1 != oEnd1; ++oBegin1) { //go through child objects of first tree
for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
minimum = std::min(minimum, minimizer.minimumOnObjectObject(*oBegin1, *oCur2));
}
for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
Helper2 helper(*oBegin1, minimizer);
minimum = std::min(minimum, ei_minimize_helper(tree2, helper, *vCur2, minimum));
}
}
for(; vBegin1 != vEnd1; ++vBegin1) { //go through child volumes of first tree
const typename BVH1::Volume &vol1 = tree1.getVolume(*vBegin1);
for(oCur2 = oBegin2; oCur2 != oEnd2; ++oCur2) {//go through child objects of second tree
Helper1 helper(*oCur2, minimizer);
minimum = std::min(minimum, ei_minimize_helper(tree1, helper, *vBegin1, minimum));
}
for(vCur2 = vBegin2; vCur2 != vEnd2; ++vCur2) { //go through child volumes of second tree
Scalar val = minimizer.minimumOnVolumeVolume(vol1, tree2.getVolume(*vCur2));
if(val < minimum)
todo.push(std::make_pair(val, std::make_pair(*vBegin1, *vCur2)));
}
}
}
return minimum;
}
#endif // EIGEN_BVALGORITHMS_H