BVH.cpp
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2009 Ilya Baran <ibaran@mit.edu>
00005 //
00006 // Eigen is free software; you can redistribute it and/or
00007 // modify it under the terms of the GNU Lesser General Public
00008 // License as published by the Free Software Foundation; either
00009 // version 3 of the License, or (at your option) any later version.
00010 //
00011 // Alternatively, you can redistribute it and/or
00012 // modify it under the terms of the GNU General Public License as
00013 // published by the Free Software Foundation; either version 2 of
00014 // the License, or (at your option) any later version.
00015 //
00016 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00017 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00018 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00019 // GNU General Public License for more details.
00020 //
00021 // You should have received a copy of the GNU Lesser General Public
00022 // License and a copy of the GNU General Public License along with
00023 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00024 
00025 #include "main.h"
00026 #include <Eigen/StdVector>
00027 #include <unsupported/Eigen/BVH>
00028 
00029 inline double SQR(double x) { return x * x; }
00030 
00031 template<int Dim>
00032 struct Ball
00033 {
00034 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(double, Dim)
00035 
00036   typedef Matrix<double, Dim, 1> VectorType;
00037 
00038   Ball() {}
00039   Ball(const VectorType &c, double r) : center(c), radius(r) {}
00040 
00041   VectorType center;
00042   double radius;
00043 };
00044 
00045 namespace Eigen {
00046 namespace internal {
00047 
00048 template<typename Scalar, int Dim> AlignedBox<Scalar, Dim> bounding_box(const Matrix<Scalar, Dim, 1> &v) { return AlignedBox<Scalar, Dim>(v); }
00049 template<int Dim> AlignedBox<double, Dim> bounding_box(const Ball<Dim> &b)
00050 { return AlignedBox<double, Dim>(b.center.array() - b.radius, b.center.array() + b.radius); }
00051 
00052 } // end namespace internal
00053 }
00054 
00055 template<int Dim>
00056 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
00057 {
00058   typedef double Scalar;
00059   typedef Matrix<double, Dim, 1> VectorType;
00060   typedef Ball<Dim> BallType;
00061   typedef AlignedBox<double, Dim> BoxType;
00062 
00063   BallPointStuff() : calls(0), count(0) {}
00064   BallPointStuff(const VectorType &inP) : p(inP), calls(0), count(0) {}
00065 
00066 
00067   bool intersectVolume(const BoxType &r) { ++calls; return r.contains(p); }
00068   bool intersectObject(const BallType &b) {
00069     ++calls;
00070     if((b.center - p).squaredNorm() < SQR(b.radius))
00071       ++count;
00072     return false; //continue
00073   }
00074 
00075   bool intersectVolumeVolume(const BoxType &r1, const BoxType &r2) { ++calls; return !(r1.intersection(r2)).isNull(); }
00076   bool intersectVolumeObject(const BoxType &r, const BallType &b) { ++calls; return r.squaredExteriorDistance(b.center) < SQR(b.radius); }
00077   bool intersectObjectVolume(const BallType &b, const BoxType &r) { ++calls; return r.squaredExteriorDistance(b.center) < SQR(b.radius); }
00078   bool intersectObjectObject(const BallType &b1, const BallType &b2){
00079     ++calls;
00080     if((b1.center - b2.center).norm() < b1.radius + b2.radius)
00081       ++count;
00082     return false;
00083   }
00084   bool intersectVolumeObject(const BoxType &r, const VectorType &v) { ++calls; return r.contains(v); }
00085   bool intersectObjectObject(const BallType &b, const VectorType &v){
00086     ++calls;
00087     if((b.center - v).squaredNorm() < SQR(b.radius))
00088       ++count;
00089     return false;
00090   }
00091 
00092   double minimumOnVolume(const BoxType &r) { ++calls; return r.squaredExteriorDistance(p); }
00093   double minimumOnObject(const BallType &b) { ++calls; return std::max(0., (b.center - p).squaredNorm() - SQR(b.radius)); }
00094   double minimumOnVolumeVolume(const BoxType &r1, const BoxType &r2) { ++calls; return r1.squaredExteriorDistance(r2); }
00095   double minimumOnVolumeObject(const BoxType &r, const BallType &b) { ++calls; return SQR(std::max(0., r.exteriorDistance(b.center) - b.radius)); }
00096   double minimumOnObjectVolume(const BallType &b, const BoxType &r) { ++calls; return SQR(std::max(0., r.exteriorDistance(b.center) - b.radius)); }
00097   double minimumOnObjectObject(const BallType &b1, const BallType &b2){ ++calls; return SQR(std::max(0., (b1.center - b2.center).norm() - b1.radius - b2.radius)); }
00098   double minimumOnVolumeObject(const BoxType &r, const VectorType &v) { ++calls; return r.squaredExteriorDistance(v); }
00099   double minimumOnObjectObject(const BallType &b, const VectorType &v){ ++calls; return SQR(std::max(0., (b.center - v).norm() - b.radius)); }
00100 
00101   VectorType p;
00102   int calls;
00103   int count;
00104 };
00105 
00106 
00107 template<int Dim>
00108 struct TreeTest
00109 {
00110   typedef Matrix<double, Dim, 1> VectorType;
00111   typedef std::vector<VectorType, aligned_allocator<VectorType> > VectorTypeList;
00112   typedef Ball<Dim> BallType;
00113   typedef std::vector<BallType, aligned_allocator<BallType> > BallTypeList;
00114   typedef AlignedBox<double, Dim> BoxType;
00115 
00116   void testIntersect1()
00117   {
00118     BallTypeList b;
00119     for(int i = 0; i < 500; ++i) {
00120         b.push_back(BallType(VectorType::Random(), 0.5 * internal::random(0., 1.)));
00121     }
00122     KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
00123 
00124     VectorType pt = VectorType::Random();
00125     BallPointStuff<Dim> i1(pt), i2(pt);
00126 
00127     for(int i = 0; i < (int)b.size(); ++i)
00128       i1.intersectObject(b[i]);
00129 
00130     BVIntersect(tree, i2);
00131 
00132     VERIFY(i1.count == i2.count);
00133   }
00134 
00135   void testMinimize1()
00136   {
00137     BallTypeList b;
00138     for(int i = 0; i < 500; ++i) {
00139         b.push_back(BallType(VectorType::Random(), 0.01 * internal::random(0., 1.)));
00140     }
00141     KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
00142 
00143     VectorType pt = VectorType::Random();
00144     BallPointStuff<Dim> i1(pt), i2(pt);
00145 
00146     double m1 = (std::numeric_limits<double>::max)(), m2 = m1;
00147 
00148     for(int i = 0; i < (int)b.size(); ++i)
00149       m1 = (std::min)(m1, i1.minimumOnObject(b[i]));
00150 
00151     m2 = BVMinimize(tree, i2);
00152 
00153     VERIFY_IS_APPROX(m1, m2);
00154   }
00155 
00156   void testIntersect2()
00157   {
00158     BallTypeList b;
00159     VectorTypeList v;
00160 
00161     for(int i = 0; i < 50; ++i) {
00162         b.push_back(BallType(VectorType::Random(), 0.5 * internal::random(0., 1.)));
00163         for(int j = 0; j < 3; ++j)
00164             v.push_back(VectorType::Random());
00165     }
00166 
00167     KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
00168     KdBVH<double, Dim, VectorType> vTree(v.begin(), v.end());
00169 
00170     BallPointStuff<Dim> i1, i2;
00171 
00172     for(int i = 0; i < (int)b.size(); ++i)
00173         for(int j = 0; j < (int)v.size(); ++j)
00174             i1.intersectObjectObject(b[i], v[j]);
00175 
00176     BVIntersect(tree, vTree, i2);
00177 
00178     VERIFY(i1.count == i2.count);
00179   }
00180 
00181   void testMinimize2()
00182   {
00183     BallTypeList b;
00184     VectorTypeList v;
00185 
00186     for(int i = 0; i < 50; ++i) {
00187         b.push_back(BallType(VectorType::Random(), 1e-7 + 1e-6 * internal::random(0., 1.)));
00188         for(int j = 0; j < 3; ++j)
00189             v.push_back(VectorType::Random());
00190     }
00191 
00192     KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
00193     KdBVH<double, Dim, VectorType> vTree(v.begin(), v.end());
00194 
00195     BallPointStuff<Dim> i1, i2;
00196 
00197     double m1 = (std::numeric_limits<double>::max)(), m2 = m1;
00198 
00199     for(int i = 0; i < (int)b.size(); ++i)
00200         for(int j = 0; j < (int)v.size(); ++j)
00201             m1 = (std::min)(m1, i1.minimumOnObjectObject(b[i], v[j]));
00202 
00203     m2 = BVMinimize(tree, vTree, i2);
00204 
00205     VERIFY_IS_APPROX(m1, m2);
00206   }
00207 };
00208 
00209 
00210 void test_BVH()
00211 {
00212   for(int i = 0; i < g_repeat; i++) {
00213 #ifdef EIGEN_TEST_PART_1
00214     TreeTest<2> test2;
00215     CALL_SUBTEST(test2.testIntersect1());
00216     CALL_SUBTEST(test2.testMinimize1());
00217     CALL_SUBTEST(test2.testIntersect2());
00218     CALL_SUBTEST(test2.testMinimize2());
00219 #endif
00220 
00221 #ifdef EIGEN_TEST_PART_2
00222     TreeTest<3> test3;
00223     CALL_SUBTEST(test3.testIntersect1());
00224     CALL_SUBTEST(test3.testMinimize1());
00225     CALL_SUBTEST(test3.testIntersect2());
00226     CALL_SUBTEST(test3.testMinimize2());
00227 #endif
00228 
00229 #ifdef EIGEN_TEST_PART_3
00230     TreeTest<4> test4;
00231     CALL_SUBTEST(test4.testIntersect1());
00232     CALL_SUBTEST(test4.testMinimize1());
00233     CALL_SUBTEST(test4.testIntersect2());
00234     CALL_SUBTEST(test4.testMinimize2());
00235 #endif
00236   }
00237 }


libicr
Author(s): Robert Krug
autogenerated on Mon Jan 6 2014 11:32:31