From 4bb5221d229703a906c6fe805b73fac2496c8bea Mon Sep 17 00:00:00 2001 From: Gael Guennebaud Date: Wed, 18 Mar 2009 20:06:06 +0000 Subject: [PATCH] Add BVH module in unsupported (patch from Ilya Baran) (I thought I committed it a week ago but it seems the command failed) --- doc/CMakeLists.txt | 2 +- doc/snippets/CMakeLists.txt | 2 +- doc/unsupported_modules.dox | 3 + unsupported/CMakeLists.txt | 2 + unsupported/Eigen/AdolcForward | 2 + unsupported/Eigen/BVH | 113 ++++++++++ unsupported/Eigen/CMakeLists.txt | 2 +- unsupported/Eigen/IterativeSolvers | 4 +- unsupported/Eigen/src/BVH/BVAlgorithms.h | 276 +++++++++++++++++++++++ unsupported/Eigen/src/BVH/CMakeLists.txt | 6 + unsupported/Eigen/src/BVH/KdBVH.h | 225 ++++++++++++++++++ unsupported/Eigen/src/CMakeLists.txt | 1 + unsupported/doc/CMakeLists.txt | 2 + unsupported/doc/Doxyfile.in | 6 +- unsupported/doc/examples/BVH_Example.cpp | 45 ++++ unsupported/doc/examples/CMakeLists.txt | 17 ++ unsupported/doc/snippets/CMakeLists.txt | 25 ++ unsupported/test/BVH.cpp | 221 ++++++++++++++++++ unsupported/test/CMakeLists.txt | 4 +- 19 files changed, 952 insertions(+), 6 deletions(-) create mode 100644 unsupported/Eigen/BVH create mode 100644 unsupported/Eigen/src/BVH/BVAlgorithms.h create mode 100644 unsupported/Eigen/src/BVH/CMakeLists.txt create mode 100644 unsupported/Eigen/src/BVH/KdBVH.h create mode 100644 unsupported/doc/CMakeLists.txt create mode 100644 unsupported/doc/examples/BVH_Example.cpp create mode 100644 unsupported/doc/examples/CMakeLists.txt create mode 100644 unsupported/doc/snippets/CMakeLists.txt create mode 100644 unsupported/test/BVH.cpp diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index acbadb5bf..7aabe2882 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -60,7 +60,7 @@ add_custom_target( ) add_dependencies(doc-eigen-prerequisites all_snippets all_examples) -add_dependencies(doc-unsupported-prerequisites unsupported_examples) +add_dependencies(doc-unsupported-prerequisites unsupported_snippets unsupported_examples) add_custom_target(doc ALL COMMAND doxygen Doxyfile-unsupported COMMAND doxygen diff --git a/doc/snippets/CMakeLists.txt b/doc/snippets/CMakeLists.txt index 195ed9e99..87eae62a2 100644 --- a/doc/snippets/CMakeLists.txt +++ b/doc/snippets/CMakeLists.txt @@ -1,4 +1,4 @@ -FILE(GLOB snippets_SRCS "*.cpp" "../../unsupported/snippets/*.cpp") +FILE(GLOB snippets_SRCS "*.cpp") ADD_CUSTOM_TARGET(all_snippets) diff --git a/doc/unsupported_modules.dox b/doc/unsupported_modules.dox index 259c43dba..61ea9ada2 100644 --- a/doc/unsupported_modules.dox +++ b/doc/unsupported_modules.dox @@ -16,4 +16,7 @@ namespace Eigen { /** \ingroup Unsupported_modules * \defgroup IterativeSolvers_Module Iterative solvers module */ +/** \ingroup Unsupported_modules + * \defgroup BVH_Module BVH module */ + } // namespace Eigen \ No newline at end of file diff --git a/unsupported/CMakeLists.txt b/unsupported/CMakeLists.txt index de3e0dc9f..895fcdbed 100644 --- a/unsupported/CMakeLists.txt +++ b/unsupported/CMakeLists.txt @@ -1,6 +1,8 @@ add_subdirectory(Eigen) +add_subdirectory(doc) + if(EIGEN_BUILD_TESTS) add_subdirectory(test) endif(EIGEN_BUILD_TESTS) diff --git a/unsupported/Eigen/AdolcForward b/unsupported/Eigen/AdolcForward index 531b2ae5f..675aec3d8 100644 --- a/unsupported/Eigen/AdolcForward +++ b/unsupported/Eigen/AdolcForward @@ -167,6 +167,8 @@ protected: }; +//@} + } #endif // EIGEN_ADLOC_FORWARD diff --git a/unsupported/Eigen/BVH b/unsupported/Eigen/BVH new file mode 100644 index 000000000..4fda52b97 --- /dev/null +++ b/unsupported/Eigen/BVH @@ -0,0 +1,113 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2009 Ilya Baran +// +// 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 . + +#ifndef EIGEN_BVH_MODULE_H +#define EIGEN_BVH_MODULE_H + +#include +#include +#include +#include +#include + +namespace Eigen { + +/** \ingroup Unsupported_modules + * \defgroup BVH_Module BVH module + * \brief This module provides generic bounding volume hierarchy algorithms + * and reference tree implementations. + * + * + * \code + * #include + * \endcode + * + * A bounding volume hierarchy (BVH) can accelerate many geometric queries. This module provides a generic implementation + * of the two basic algorithms over a BVH: intersection of a query object against all objects in the hierarchy and minimization + * of a function over the objects in the hierarchy. It also provides intersection and minimization over a cartesian product of + * two BVH's. A BVH accelerates intersection by using the fact that if a query object does not intersect a volume, then it cannot + * intersect any object contained in that volume. Similarly, a BVH accelerates minimization because the minimum of a function + * over a volume is no greater than the minimum of a function over any object contained in it. + * + * Some sample queries that can be written in terms of intersection are: + * - Determine all points where a ray intersects a triangle mesh + * - Given a set of points, determine which are contained in a query sphere + * - Given a set of spheres, determine which contain the query point + * - Given a set of spheres, determine if any is completely contained in a query box (not an intersection query, + but can still be accelerated by pruning all spheres that do not intersect the query box) + * - Given a set of points, count how many pairs are \f$d\pm\epsilon\f$ apart (done by looking at the cartesian product of the set + * of points with itself) + * + * Some sample queries that can be written in terms of function minimization over a set of objects are: + * - Find the intersection between a ray and a triangle mesh closest to the ray origin (function is infinite off the ray) + * - Given a polyline and a query point, determine the closest point on the polyline to the query + * - Find the diameter of a point cloud (done by looking at the cartesian product and using negative distance as the function) + * - Determine how far two meshes are from colliding (this is also a cartesian product query) + * + * This implementation decouples the basic algorithms both from the type of hierarchy (and the types of the bounding volumes) and + * from the particulars of the query. To enable abstraction from the BVH, the BVH is required to implement a generic mechanism + * for traversal. To abstract from the query, the query is responsible for keeping track of results. + * + * To be used in the algorithms, a hierarchy must implement the following traversal mechanism (see KdBVH for a sample implementation): \code + typedef Volume //the type of bounding volume + typedef Object //the type of object in the hierarchy + typedef Index //a reference to a node in the hierarchy--typically an int or a pointer + typedef VolumeIterator //an iterator type over node children--returns Index + typedef ObjectIterator //an iterator over object (leaf) children--returns const Object & + Index getRootIndex() const //returns the index of the hierarchy root + const Volume &getVolume(Index index) const //returns the bounding volume of the node at given index + void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd, + ObjectIterator &outOBegin, ObjectIterator &outOEnd) const + //getChildren takes a node index and makes [outVBegin, outVEnd) range over its node children + //and [outOBegin, outOEnd) range over its object children + \endcode + * + * To use the hierarchy, call BVIntersect or BVMinimize, passing it a BVH (or two, for cartesian product) and a minimizer or intersector. + * For an intersection query on a single BVH, the intersector encapsulates the query and must provide two functions: + * \code + bool intersectVolume(const Volume &volume) //returns true if the query intersects the volume + bool intersectObject(const Object &object) //returns true if the intersection search should terminate immediately + \endcode + * The guarantee that BVIntersect provides is that intersectObject will be called on every object whose bounding volume + * intersects the query (but possibly on other objects too) unless the search is terminated prematurely. It is the + * responsibility of the intersectObject function to keep track of the results in whatever manner is appropriate. + * The cartesian product intersection and the BVMinimize queries are similar--see their individual documentation. + * + * \addexample BVH_Example \label How to use a BVH to find the closest pair between two point sets + * + * The following is a simple but complete example for how to use the BVH to accelerate the search for a closest red-blue point pair: + * \include BVH_Example.cpp + * Output: \verbinclude BVH_Example.out + + */ +//@{ + +#include "src/BVH/BVAlgorithms.h" +#include "src/BVH/KdBVH.h" + +//@} + +} + +#endif // EIGEN_BVH_MODULE_H diff --git a/unsupported/Eigen/CMakeLists.txt b/unsupported/Eigen/CMakeLists.txt index c06c0653a..eb54f88ac 100644 --- a/unsupported/Eigen/CMakeLists.txt +++ b/unsupported/Eigen/CMakeLists.txt @@ -1,4 +1,4 @@ -set(Eigen_HEADERS AdolcForward IterativeSolvers) +set(Eigen_HEADERS AdolcForward BVH IterativeSolvers) install(FILES ${Eigen_HEADERS} diff --git a/unsupported/Eigen/IterativeSolvers b/unsupported/Eigen/IterativeSolvers index e60ce4d1e..fb59ed0bb 100644 --- a/unsupported/Eigen/IterativeSolvers +++ b/unsupported/Eigen/IterativeSolvers @@ -25,6 +25,8 @@ #ifndef EIGEN_ITERATIVE_SOLVERS_MODULE_H #define EIGEN_ITERATIVE_SOLVERS_MODULE_H +#include + namespace Eigen { /** \ingroup Unsupported_modules @@ -38,7 +40,7 @@ namespace Eigen { * \endcode */ //@{ - + #include "src/IterativeSolvers/IterationController.h" #include "src/IterativeSolvers/ConstrainedConjGrad.h" diff --git a/unsupported/Eigen/src/BVH/BVAlgorithms.h b/unsupported/Eigen/src/BVH/BVAlgorithms.h new file mode 100644 index 000000000..eda052bee --- /dev/null +++ b/unsupported/Eigen/src/BVH/BVAlgorithms.h @@ -0,0 +1,276 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2009 Ilya Baran +// +// 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 . + +#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 +void BVIntersect(const BVH &tree, Intersector &intersector) +{ + ei_intersect_helper(tree, intersector, tree.getRootIndex()); +} + +#ifndef EIGEN_PARSED_BY_DOXYGEN +template +bool ei_intersect_helper(const BVH &tree, Intersector &intersector, typename BVH::Index root) +{ + typedef typename BVH::Index Index; + + typename BVH::VolumeIterator vBegin, vEnd; + typename BVH::ObjectIterator oBegin, oEnd; + + std::vector 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 +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 +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 +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 Helper1; + typedef ei_intersector_helper2 Helper2; + + typename BVH1::VolumeIterator vBegin1, vEnd1; + typename BVH1::ObjectIterator oBegin1, oEnd1; + typename BVH2::VolumeIterator vBegin2, vEnd2, vCur2; + typename BVH2::ObjectIterator oBegin2, oEnd2, oCur2; + + std::vector > 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 Minimizer::Scalar BVMinimize(const BVH &tree, Minimizer &minimizer) +{ + return ei_minimize_helper(tree, minimizer, tree.getRootIndex(), std::numeric_limits::max()); +} + +#ifndef EIGEN_PARSED_BY_DOXYGEN +template +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 QueueElement; //first element is priority + + typename BVH::VolumeIterator vBegin, vEnd; + typename BVH::ObjectIterator oBegin, oEnd; + std::priority_queue, std::greater > 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 +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 +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 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 Helper1; + typedef ei_minimizer_helper2 Helper2; + typedef std::pair > QueueElement; //first element is priority + + typename BVH1::VolumeIterator vBegin1, vEnd1; + typename BVH1::ObjectIterator oBegin1, oEnd1; + typename BVH2::VolumeIterator vBegin2, vEnd2, vCur2; + typename BVH2::ObjectIterator oBegin2, oEnd2, oCur2; + std::priority_queue, std::greater > todo; //smallest is at the top + + Scalar minimum = std::numeric_limits::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 diff --git a/unsupported/Eigen/src/BVH/CMakeLists.txt b/unsupported/Eigen/src/BVH/CMakeLists.txt new file mode 100644 index 000000000..b377d865c --- /dev/null +++ b/unsupported/Eigen/src/BVH/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_BVH_SRCS "*.h") + +INSTALL(FILES + ${Eigen_BVH_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/unsupported/Eigen/src/BVH COMPONENT Devel + ) diff --git a/unsupported/Eigen/src/BVH/KdBVH.h b/unsupported/Eigen/src/BVH/KdBVH.h new file mode 100644 index 000000000..4b681925c --- /dev/null +++ b/unsupported/Eigen/src/BVH/KdBVH.h @@ -0,0 +1,225 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2009 Ilya Baran +// +// 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 . + +#ifndef KDBVH_H_INCLUDED +#define KDBVH_H_INCLUDED + +//internal pair class for the BVH--used instead of std::pair because of alignment +template +struct ei_vector_int_pair +{ +EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Dim) + typedef Matrix VectorType; + + ei_vector_int_pair(const VectorType &v, int i) : first(v), second(i) {} + + VectorType first; + int second; +}; + +//these templates help the tree initializer get the bounding boxes either from a provided +//iterator range or using ei_bounding_box in a unified way +template +struct ei_get_boxes_helper { + void operator()(const std::vector &objects, BoxIter boxBegin, BoxIter boxEnd, std::vector &outBoxes) + { + outBoxes.insert(outBoxes.end(), boxBegin, boxEnd); + ei_assert(outBoxes.size() == objects.size()); + } +}; + +template +struct ei_get_boxes_helper { + void operator()(const std::vector &objects, int, int, std::vector &outBoxes) + { + outBoxes.reserve(objects.size()); + for(int i = 0; i < (int)objects.size(); ++i) + outBoxes.push_back(ei_bounding_box(objects[i])); + } +}; + + +/** \class KdBVH + * \brief A simple bounding volume hierarchy based on AlignedBox + * + * \param _Scalar The underlying scalar type of the bounding boxes + * \param _Dim The dimension of the space in which the hierarchy lives + * \param _Object The object type that lives in the hierarchy. It must have value semantics. Either ei_bounding_box(_Object) must + * be defined and return an AlignedBox<_Scalar, _Dim> or bounding boxes must be provided to the tree initializer. + * + * This class provides a simple (as opposed to optimized) implementation of a bounding volume hierarchy analogous to a Kd-tree. + * Given a sequence of objects, it computes their bounding boxes, constructs a Kd-tree of their centers + * and builds a BVH with the structure of that Kd-tree. When the elements of the tree are too expensive to be copied around, + * it is useful for _Object to be a pointer. + */ +template class KdBVH +{ +public: + enum { Dim = _Dim }; + typedef _Object Object; + typedef _Scalar Scalar; + typedef AlignedBox Volume; + typedef int Index; + typedef const int *VolumeIterator; //the iterators are just pointers into the tree's vectors + typedef const Object *ObjectIterator; + + KdBVH() {} + + /** Given an iterator range over \a Object references, constructs the BVH. Requires that ei_bounding_box(Object) return a Volume. */ + template KdBVH(Iter begin, Iter end) { init(begin, end, 0, 0); } //int is recognized by init as not being an iterator type + + /** Given an iterator range over \a Object references and an iterator range over their bounding boxes, constructs the BVH */ + template KdBVH(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) { init(begin, end, boxBegin, boxEnd); } + + /** Given an iterator range over \a Object references, constructs the BVH, overwriting whatever is in there currently. + * Requires that ei_bounding_box(Object) return a Volume. */ + template void init(Iter begin, Iter end) { init(begin, end, 0, 0); } + + /** Given an iterator range over \a Object references and an iterator range over their bounding boxes, + * constructs the BVH, overwriting whatever is in there currently. */ + template void init(OIter begin, OIter end, BIter boxBegin, BIter boxEnd) + { + objects.clear(); + boxes.clear(); + children.clear(); + + objects.insert(objects.end(), begin, end); + int n = objects.size(); + + if(n < 2) + return; //if we have at most one object, we don't need any internal nodes + + std::vector objBoxes; + std::vector objCenters; + + ei_get_boxes_helper()(objects, boxBegin, boxEnd, objBoxes); //compute the bounding boxes depending on BIter type + + objCenters.reserve(n); + boxes.reserve(n - 1); + children.reserve(2 * n - 2); + + for(int i = 0; i < n; ++i) + objCenters.push_back(VIPair(objBoxes[i].center(), i)); + + build(objCenters, 0, n, objBoxes, 0); //the recursive part of the algorithm + + std::vector tmp(n); + tmp.swap(objects); + for(int i = 0; i < n; ++i) + objects[i] = tmp[objCenters[i].second]; + } + + /** \returns the index of the root of the hierarchy */ + inline Index getRootIndex() const { return (int)boxes.size() - 1; } + + /** Given an \a index of a node, on exit, \a outVBegin and \a outVEnd range over the indices of the volume children of the node + * and \a outOBegin and \a outOEnd range over the object children of the node */ + EIGEN_STRONG_INLINE void getChildren(Index index, VolumeIterator &outVBegin, VolumeIterator &outVEnd, + ObjectIterator &outOBegin, ObjectIterator &outOEnd) const + { //inlining this function should open lots of optimization opportunities to the compiler + if(index < 0) { + outVBegin = outVEnd; + if(!objects.empty()) + outOBegin = &(objects[0]); + outOEnd = outOBegin + objects.size(); //output all objects--necessary when the tree has only one object + return; + } + + int numBoxes = boxes.size(); + + int idx = index * 2; + if(children[idx + 1] < numBoxes) { //second index is always bigger + outVBegin = &(children[idx]); + outVEnd = outVBegin + 2; + outOBegin = outOEnd; + } + else if(children[idx] >= numBoxes) { //if both children are objects + outVBegin = outVEnd; + outOBegin = &(objects[children[idx] - numBoxes]); + outOEnd = outOBegin + 2; + } else { //if the first child is a volume and the second is an object + outVBegin = &(children[idx]); + outVEnd = outVBegin + 1; + outOBegin = &(objects[children[idx + 1] - numBoxes]); + outOEnd = outOBegin + 1; + } + } + + /** \returns the bounding box of the node at \a index */ + inline const Volume &getVolume(Index index) const + { + return boxes[index]; + } + +private: + typedef ei_vector_int_pair VIPair; + typedef Matrix VectorType; + struct VectorComparator //compares vectors, or, more specificall, VIPairs along a particular dimension + { + VectorComparator(int inDim) : dim(inDim) {} + inline bool operator()(const VIPair &v1, const VIPair &v2) const { return v1.first[dim] < v2.first[dim]; } + int dim; + }; + + //Build the part of the tree between objects[from] and objects[to] (not including objects[to]). + //This routine partitions the objCenters in [from, to) along the dimension dim, recursively constructs + //the two halves, and adds their parent node. TODO: a cache-friendlier layout + void build(std::vector &objCenters, int from, int to, const std::vector &objBoxes, int dim) + { + ei_assert(to - from > 1); + if(to - from == 2) { + boxes.push_back(objBoxes[objCenters[from].second].merged(objBoxes[objCenters[from + 1].second])); + children.push_back(from + (int)objects.size() - 1); //there are objects.size() - 1 tree nodes + children.push_back(from + (int)objects.size()); + } + else if(to - from == 3) { + int mid = from + 2; + std::nth_element(objCenters.begin() + from, objCenters.begin() + mid, + objCenters.begin() + to, VectorComparator(dim)); //partition + build(objCenters, from, mid, objBoxes, (dim + 1) % Dim); + int idx1 = (int)boxes.size() - 1; + boxes.push_back(boxes[idx1].merged(objBoxes[objCenters[mid].second])); + children.push_back(idx1); + children.push_back(mid + (int)objects.size() - 1); + } + else { + int mid = from + (to - from) / 2; + nth_element(objCenters.begin() + from, objCenters.begin() + mid, + objCenters.begin() + to, VectorComparator(dim)); //partition + build(objCenters, from, mid, objBoxes, (dim + 1) % Dim); + int idx1 = (int)boxes.size() - 1; + build(objCenters, mid, to, objBoxes, (dim + 1) % Dim); + int idx2 = (int)boxes.size() - 1; + boxes.push_back(boxes[idx1].merged(boxes[idx2])); + children.push_back(idx1); + children.push_back(idx2); + } + } + + std::vector children; //children of x are children[2x] and children[2x+1], indices bigger than boxes.size() index into objects. + std::vector boxes; + std::vector objects; +}; + +#endif //KDBVH_H_INCLUDED diff --git a/unsupported/Eigen/src/CMakeLists.txt b/unsupported/Eigen/src/CMakeLists.txt index 67ef0a914..727b18cf5 100644 --- a/unsupported/Eigen/src/CMakeLists.txt +++ b/unsupported/Eigen/src/CMakeLists.txt @@ -1 +1,2 @@ ADD_SUBDIRECTORY(IterativeSolvers) +ADD_SUBDIRECTORY(BVH) diff --git a/unsupported/doc/CMakeLists.txt b/unsupported/doc/CMakeLists.txt new file mode 100644 index 000000000..e8f6985e3 --- /dev/null +++ b/unsupported/doc/CMakeLists.txt @@ -0,0 +1,2 @@ + +add_subdirectory(examples) diff --git a/unsupported/doc/Doxyfile.in b/unsupported/doc/Doxyfile.in index 83ab41334..c33986224 100644 --- a/unsupported/doc/Doxyfile.in +++ b/unsupported/doc/Doxyfile.in @@ -626,7 +626,11 @@ EXCLUDE_SYMBOLS = MatrixBase<* MapBase<* RotationBase<* Matrix<* EXAMPLE_PATH = "${Eigen_SOURCE_DIR}/doc/snippets" \ "${Eigen_BINARY_DIR}/doc/snippets" \ "${Eigen_SOURCE_DIR}/doc/examples" \ - "${Eigen_BINARY_DIR}/doc/examples" + "${Eigen_BINARY_DIR}/doc/examples" \ + "${Eigen_SOURCE_DIR}/unsupported/doc/snippets" \ + "${Eigen_BINARY_DIR}/unsupported/doc/snippets" \ + "${Eigen_SOURCE_DIR}/unsupported/doc/examples" \ + "${Eigen_BINARY_DIR}/unsupported/doc/examples" # If the value of the EXAMPLE_PATH tag contains directories, you can use the # EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp diff --git a/unsupported/doc/examples/BVH_Example.cpp b/unsupported/doc/examples/BVH_Example.cpp new file mode 100644 index 000000000..890eb535b --- /dev/null +++ b/unsupported/doc/examples/BVH_Example.cpp @@ -0,0 +1,45 @@ +#include + +using namespace Eigen; +typedef AlignedBox Box2d; + +Box2d ei_bounding_box(const Vector2d &v) { return Box2d(v, v); } //compute the bounding box of a single point + +struct PointPointMinimizer //how to compute squared distances between points and rectangles +{ + PointPointMinimizer() : calls(0) {} + typedef double Scalar; + + double minimumOnVolumeVolume(const Box2d &r1, const Box2d &r2) { ++calls; return r1.squaredExteriorDistance(r2); } + double minimumOnVolumeObject(const Box2d &r, const Vector2d &v) { ++calls; return r.squaredExteriorDistance(v); } + double minimumOnObjectVolume(const Vector2d &v, const Box2d &r) { ++calls; return r.squaredExteriorDistance(v); } + double minimumOnObjectObject(const Vector2d &v1, const Vector2d &v2) { ++calls; return (v1 - v2).squaredNorm(); } + + int calls; +}; + +int main() +{ + std::vector redPoints, bluePoints; + for(int i = 0; i < 100; ++i) { //initialize random set of red points and blue points + redPoints.push_back(Vector2d::Random()); + bluePoints.push_back(Vector2d::Random()); + } + + PointPointMinimizer minimizer; + double minDistSq = std::numeric_limits::max(); + + //brute force to find closest red-blue pair + for(int i = 0; i < (int)redPoints.size(); ++i) + for(int j = 0; j < (int)bluePoints.size(); ++j) + minDistSq = std::min(minDistSq, minimizer.minimumOnObjectObject(redPoints[i], bluePoints[j])); + std::cout << "Brute force distance = " << sqrt(minDistSq) << ", calls = " << minimizer.calls << std::endl; + + //using BVH to find closest red-blue pair + minimizer.calls = 0; + KdBVH redTree(redPoints.begin(), redPoints.end()), blueTree(bluePoints.begin(), bluePoints.end()); //construct the trees + minDistSq = BVMinimize(redTree, blueTree, minimizer); //actual BVH minimization call + std::cout << "BVH distance = " << sqrt(minDistSq) << ", calls = " << minimizer.calls << std::endl; + + return 0; +} diff --git a/unsupported/doc/examples/CMakeLists.txt b/unsupported/doc/examples/CMakeLists.txt new file mode 100644 index 000000000..af4de4b0d --- /dev/null +++ b/unsupported/doc/examples/CMakeLists.txt @@ -0,0 +1,17 @@ +FILE(GLOB examples_SRCS "*.cpp") + +ADD_CUSTOM_TARGET(unsupported_examples) + +FOREACH(example_src ${examples_SRCS}) +GET_FILENAME_COMPONENT(example ${example_src} NAME_WE) +ADD_EXECUTABLE(${example} ${example_src}) +GET_TARGET_PROPERTY(example_executable + ${example} LOCATION) +ADD_CUSTOM_COMMAND( + TARGET ${example} + POST_BUILD + COMMAND ${example_executable} + ARGS >${CMAKE_CURRENT_BINARY_DIR}/${example}.out +) +ADD_DEPENDENCIES(unsupported_examples ${example}) +ENDFOREACH(example_src) diff --git a/unsupported/doc/snippets/CMakeLists.txt b/unsupported/doc/snippets/CMakeLists.txt new file mode 100644 index 000000000..fa529d139 --- /dev/null +++ b/unsupported/doc/snippets/CMakeLists.txt @@ -0,0 +1,25 @@ +FILE(GLOB snippets_SRCS "*.cpp") + +ADD_CUSTOM_TARGET(unsupported_snippets) + +FOREACH(snippet_src ${snippets_SRCS}) + GET_FILENAME_COMPONENT(snippet ${snippet_src} NAME_WE) + SET(compile_snippet_target compile_${snippet}) + SET(compile_snippet_src ${compile_snippet_target}.cpp) + FILE(READ ${snippet_src} snippet_source_code) + CONFIGURE_FILE(${PROJECT_SOURCE_DIR}/doc/compile_snippet.cpp.in + ${CMAKE_CURRENT_BINARY_DIR}/${compile_snippet_src}) + ADD_EXECUTABLE(${compile_snippet_target} + ${CMAKE_CURRENT_BINARY_DIR}/${compile_snippet_src}) + GET_TARGET_PROPERTY(compile_snippet_executable + ${compile_snippet_target} LOCATION) + ADD_CUSTOM_COMMAND( + TARGET ${compile_snippet_target} + POST_BUILD + COMMAND ${compile_snippet_executable} + ARGS >${CMAKE_CURRENT_BINARY_DIR}/${snippet}.out + ) + ADD_DEPENDENCIES(unsupported_snippets ${compile_snippet_target}) + set_source_files_properties(${CMAKE_CURRENT_BINARY_DIR}/${compile_snippet_src} + PROPERTIES OBJECT_DEPENDS ${snippet_src}) +ENDFOREACH(snippet_src) diff --git a/unsupported/test/BVH.cpp b/unsupported/test/BVH.cpp new file mode 100644 index 000000000..445489eef --- /dev/null +++ b/unsupported/test/BVH.cpp @@ -0,0 +1,221 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2009 Ilya Baran +// +// 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 . + +#include +#include "main.h" +#include + +inline double SQR(double x) { return x * x; } + +template +struct Ball +{ +EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(double, Dim) + + typedef Matrix VectorType; + + Ball() {} + Ball(const VectorType &c, double r) : center(c), radius(r) {} + + VectorType center; + double radius; +}; + +template AlignedBox ei_bounding_box(const Matrix &v) { return AlignedBox(v); } +template AlignedBox ei_bounding_box(const Ball &b) +{ return AlignedBox(b.center.cwise() - b.radius, b.center.cwise() + b.radius); } + +template +struct BallPointStuff //this class provides functions to be both an intersector and a minimizer, both for a ball and a point and for two trees +{ + typedef double Scalar; + typedef Matrix VectorType; + typedef Ball BallType; + typedef AlignedBox BoxType; + + BallPointStuff() : calls(0), count(0) {} + BallPointStuff(const VectorType &inP) : p(inP), calls(0), count(0) {} + + + bool intersectVolume(const BoxType &r) { ++calls; return r.contains(p); } + bool intersectObject(const BallType &b) { + ++calls; + if((b.center - p).squaredNorm() < SQR(b.radius)) + ++count; + return false; //continue + } + + bool intersectVolumeVolume(const BoxType &r1, const BoxType &r2) { ++calls; return !(r1.intersection(r2)).isNull(); } + bool intersectVolumeObject(const BoxType &r, const BallType &b) { ++calls; return r.squaredExteriorDistance(b.center) < SQR(b.radius); } + bool intersectObjectVolume(const BallType &b, const BoxType &r) { ++calls; return r.squaredExteriorDistance(b.center) < SQR(b.radius); } + bool intersectObjectObject(const BallType &b1, const BallType &b2){ + ++calls; + if((b1.center - b2.center).norm() < b1.radius + b2.radius) + ++count; + return false; + } + bool intersectVolumeObject(const BoxType &r, const VectorType &v) { ++calls; return r.contains(v); } + bool intersectObjectObject(const BallType &b, const VectorType &v){ + ++calls; + if((b.center - v).squaredNorm() < SQR(b.radius)) + ++count; + return false; + } + + double minimumOnVolume(const BoxType &r) { ++calls; return r.squaredExteriorDistance(p); } + double minimumOnObject(const BallType &b) { ++calls; return std::max(0., (b.center - p).squaredNorm() - SQR(b.radius)); } + double minimumOnVolumeVolume(const BoxType &r1, const BoxType &r2) { ++calls; return r1.squaredExteriorDistance(r2); } + double minimumOnVolumeObject(const BoxType &r, const BallType &b) { ++calls; return SQR(std::max(0., r.exteriorDistance(b.center) - b.radius)); } + double minimumOnObjectVolume(const BallType &b, const BoxType &r) { ++calls; return SQR(std::max(0., r.exteriorDistance(b.center) - b.radius)); } + double minimumOnObjectObject(const BallType &b1, const BallType &b2){ ++calls; return SQR(std::max(0., (b1.center - b2.center).norm() - b1.radius - b2.radius)); } + double minimumOnVolumeObject(const BoxType &r, const VectorType &v) { ++calls; return r.squaredExteriorDistance(v); } + double minimumOnObjectObject(const BallType &b, const VectorType &v){ ++calls; return SQR(std::max(0., (b.center - v).norm() - b.radius)); } + + VectorType p; + int calls; + int count; +}; + +template +struct TreeTest +{ + typedef Matrix VectorType; + typedef Ball BallType; + typedef AlignedBox BoxType; + + void testIntersect1() + { + std::vector b; + for(int i = 0; i < 500; ++i) { + b.push_back(BallType(VectorType::Random(), 0.5 * ei_random(0., 1.))); + } + KdBVH tree(b.begin(), b.end()); + + VectorType pt = VectorType::Random(); + BallPointStuff i1(pt), i2(pt); + + for(int i = 0; i < (int)b.size(); ++i) + i1.intersectObject(b[i]); + + BVIntersect(tree, i2); + + VERIFY(i1.count == i2.count); + } + + void testMinimize1() + { + std::vector b; + for(int i = 0; i < 500; ++i) { + b.push_back(BallType(VectorType::Random(), 0.01 * ei_random(0., 1.))); + } + KdBVH tree(b.begin(), b.end()); + + VectorType pt = VectorType::Random(); + BallPointStuff i1(pt), i2(pt); + + double m1 = std::numeric_limits::max(), m2 = m1; + + for(int i = 0; i < (int)b.size(); ++i) + m1 = std::min(m1, i1.minimumOnObject(b[i])); + + m2 = BVMinimize(tree, i2); + + VERIFY_IS_APPROX(m1, m2); + } + + void testIntersect2() + { + std::vector b; + std::vector v; + + for(int i = 0; i < 50; ++i) { + b.push_back(BallType(VectorType::Random(), 0.5 * ei_random(0., 1.))); + for(int j = 0; j < 3; ++j) + v.push_back(VectorType::Random()); + } + + KdBVH tree(b.begin(), b.end()); + KdBVH vTree(v.begin(), v.end()); + + BallPointStuff i1, i2; + + for(int i = 0; i < (int)b.size(); ++i) + for(int j = 0; j < (int)v.size(); ++j) + i1.intersectObjectObject(b[i], v[j]); + + BVIntersect(tree, vTree, i2); + + VERIFY(i1.count == i2.count); + } + + void testMinimize2() + { + std::vector b; + std::vector v; + + for(int i = 0; i < 50; ++i) { + b.push_back(BallType(VectorType::Random(), 1e-7 + 1e-6 * ei_random(0., 1.))); + for(int j = 0; j < 3; ++j) + v.push_back(VectorType::Random()); + } + + KdBVH tree(b.begin(), b.end()); + KdBVH vTree(v.begin(), v.end()); + + BallPointStuff i1, i2; + + double m1 = std::numeric_limits::max(), m2 = m1; + + for(int i = 0; i < (int)b.size(); ++i) + for(int j = 0; j < (int)v.size(); ++j) + m1 = std::min(m1, i1.minimumOnObjectObject(b[i], v[j])); + + m2 = BVMinimize(tree, vTree, i2); + + VERIFY_IS_APPROX(m1, m2); + } +}; + +void test_BVH() +{ + for(int i = 0; i < g_repeat; i++) { + TreeTest<2> test2; + CALL_SUBTEST(test2.testIntersect1()); + CALL_SUBTEST(test2.testMinimize1()); + CALL_SUBTEST(test2.testIntersect2()); + CALL_SUBTEST(test2.testMinimize2()); + + TreeTest<3> test3; + CALL_SUBTEST(test3.testIntersect1()); + CALL_SUBTEST(test3.testMinimize1()); + CALL_SUBTEST(test3.testIntersect2()); + CALL_SUBTEST(test3.testMinimize2()); + + TreeTest<4> test4; + CALL_SUBTEST(test4.testIntersect1()); + CALL_SUBTEST(test4.testMinimize1()); + CALL_SUBTEST(test4.testIntersect2()); + CALL_SUBTEST(test4.testMinimize2()); + } +} diff --git a/unsupported/test/CMakeLists.txt b/unsupported/test/CMakeLists.txt index c36bcad32..2e4bde3e1 100644 --- a/unsupported/test/CMakeLists.txt +++ b/unsupported/test/CMakeLists.txt @@ -13,4 +13,6 @@ if(ADOLC_FOUND) ei_add_test(forward_adolc " " ${ADOLC_LIBRARIES}) else(ADOLC_FOUND) ei_add_property(EIGEN_MISSING_BACKENDS "Adolc") -endif(ADOLC_FOUND) \ No newline at end of file +endif(ADOLC_FOUND) + +ei_add_test(BVH)