Home | History | Annotate | Download | only in test
      1 // This file is part of Eigen, a lightweight C++ template library
      2 // for linear algebra.
      3 //
      4 // Copyright (C) 2009 Ilya Baran <ibaran (at) mit.edu>
      5 //
      6 // This Source Code Form is subject to the terms of the Mozilla
      7 // Public License v. 2.0. If a copy of the MPL was not distributed
      8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
      9 
     10 #include "main.h"
     11 #include <Eigen/StdVector>
     12 #include <Eigen/Geometry>
     13 #include <unsupported/Eigen/BVH>
     14 
     15 namespace Eigen {
     16 
     17 template<typename Scalar, int Dim> AlignedBox<Scalar, Dim> bounding_box(const Matrix<Scalar, Dim, 1> &v) { return AlignedBox<Scalar, Dim>(v); }
     18 
     19 }
     20 
     21 
     22 template<int Dim>
     23 struct Ball
     24 {
     25 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(double, Dim)
     26 
     27   typedef Matrix<double, Dim, 1> VectorType;
     28 
     29   Ball() {}
     30   Ball(const VectorType &c, double r) : center(c), radius(r) {}
     31 
     32   VectorType center;
     33   double radius;
     34 };
     35 template<int Dim> AlignedBox<double, Dim> bounding_box(const Ball<Dim> &b)
     36 { return AlignedBox<double, Dim>(b.center.array() - b.radius, b.center.array() + b.radius); }
     37 
     38 inline double SQR(double x) { return x * x; }
     39 
     40 template<int Dim>
     41 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
     42 {
     43   typedef double Scalar;
     44   typedef Matrix<double, Dim, 1> VectorType;
     45   typedef Ball<Dim> BallType;
     46   typedef AlignedBox<double, Dim> BoxType;
     47 
     48   BallPointStuff() : calls(0), count(0) {}
     49   BallPointStuff(const VectorType &inP) : p(inP), calls(0), count(0) {}
     50 
     51 
     52   bool intersectVolume(const BoxType &r) { ++calls; return r.contains(p); }
     53   bool intersectObject(const BallType &b) {
     54     ++calls;
     55     if((b.center - p).squaredNorm() < SQR(b.radius))
     56       ++count;
     57     return false; //continue
     58   }
     59 
     60   bool intersectVolumeVolume(const BoxType &r1, const BoxType &r2) { ++calls; return !(r1.intersection(r2)).isNull(); }
     61   bool intersectVolumeObject(const BoxType &r, const BallType &b) { ++calls; return r.squaredExteriorDistance(b.center) < SQR(b.radius); }
     62   bool intersectObjectVolume(const BallType &b, const BoxType &r) { ++calls; return r.squaredExteriorDistance(b.center) < SQR(b.radius); }
     63   bool intersectObjectObject(const BallType &b1, const BallType &b2){
     64     ++calls;
     65     if((b1.center - b2.center).norm() < b1.radius + b2.radius)
     66       ++count;
     67     return false;
     68   }
     69   bool intersectVolumeObject(const BoxType &r, const VectorType &v) { ++calls; return r.contains(v); }
     70   bool intersectObjectObject(const BallType &b, const VectorType &v){
     71     ++calls;
     72     if((b.center - v).squaredNorm() < SQR(b.radius))
     73       ++count;
     74     return false;
     75   }
     76 
     77   double minimumOnVolume(const BoxType &r) { ++calls; return r.squaredExteriorDistance(p); }
     78   double minimumOnObject(const BallType &b) { ++calls; return (std::max)(0., (b.center - p).squaredNorm() - SQR(b.radius)); }
     79   double minimumOnVolumeVolume(const BoxType &r1, const BoxType &r2) { ++calls; return r1.squaredExteriorDistance(r2); }
     80   double minimumOnVolumeObject(const BoxType &r, const BallType &b) { ++calls; return SQR((std::max)(0., r.exteriorDistance(b.center) - b.radius)); }
     81   double minimumOnObjectVolume(const BallType &b, const BoxType &r) { ++calls; return SQR((std::max)(0., r.exteriorDistance(b.center) - b.radius)); }
     82   double minimumOnObjectObject(const BallType &b1, const BallType &b2){ ++calls; return SQR((std::max)(0., (b1.center - b2.center).norm() - b1.radius - b2.radius)); }
     83   double minimumOnVolumeObject(const BoxType &r, const VectorType &v) { ++calls; return r.squaredExteriorDistance(v); }
     84   double minimumOnObjectObject(const BallType &b, const VectorType &v){ ++calls; return SQR((std::max)(0., (b.center - v).norm() - b.radius)); }
     85 
     86   VectorType p;
     87   int calls;
     88   int count;
     89 };
     90 
     91 
     92 template<int Dim>
     93 struct TreeTest
     94 {
     95   typedef Matrix<double, Dim, 1> VectorType;
     96   typedef std::vector<VectorType, aligned_allocator<VectorType> > VectorTypeList;
     97   typedef Ball<Dim> BallType;
     98   typedef std::vector<BallType, aligned_allocator<BallType> > BallTypeList;
     99   typedef AlignedBox<double, Dim> BoxType;
    100 
    101   void testIntersect1()
    102   {
    103     BallTypeList b;
    104     for(int i = 0; i < 500; ++i) {
    105         b.push_back(BallType(VectorType::Random(), 0.5 * internal::random(0., 1.)));
    106     }
    107     KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
    108 
    109     VectorType pt = VectorType::Random();
    110     BallPointStuff<Dim> i1(pt), i2(pt);
    111 
    112     for(int i = 0; i < (int)b.size(); ++i)
    113       i1.intersectObject(b[i]);
    114 
    115     BVIntersect(tree, i2);
    116 
    117     VERIFY(i1.count == i2.count);
    118   }
    119 
    120   void testMinimize1()
    121   {
    122     BallTypeList b;
    123     for(int i = 0; i < 500; ++i) {
    124         b.push_back(BallType(VectorType::Random(), 0.01 * internal::random(0., 1.)));
    125     }
    126     KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
    127 
    128     VectorType pt = VectorType::Random();
    129     BallPointStuff<Dim> i1(pt), i2(pt);
    130 
    131     double m1 = (std::numeric_limits<double>::max)(), m2 = m1;
    132 
    133     for(int i = 0; i < (int)b.size(); ++i)
    134       m1 = (std::min)(m1, i1.minimumOnObject(b[i]));
    135 
    136     m2 = BVMinimize(tree, i2);
    137 
    138     VERIFY_IS_APPROX(m1, m2);
    139   }
    140 
    141   void testIntersect2()
    142   {
    143     BallTypeList b;
    144     VectorTypeList v;
    145 
    146     for(int i = 0; i < 50; ++i) {
    147         b.push_back(BallType(VectorType::Random(), 0.5 * internal::random(0., 1.)));
    148         for(int j = 0; j < 3; ++j)
    149             v.push_back(VectorType::Random());
    150     }
    151 
    152     KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
    153     KdBVH<double, Dim, VectorType> vTree(v.begin(), v.end());
    154 
    155     BallPointStuff<Dim> i1, i2;
    156 
    157     for(int i = 0; i < (int)b.size(); ++i)
    158         for(int j = 0; j < (int)v.size(); ++j)
    159             i1.intersectObjectObject(b[i], v[j]);
    160 
    161     BVIntersect(tree, vTree, i2);
    162 
    163     VERIFY(i1.count == i2.count);
    164   }
    165 
    166   void testMinimize2()
    167   {
    168     BallTypeList b;
    169     VectorTypeList v;
    170 
    171     for(int i = 0; i < 50; ++i) {
    172         b.push_back(BallType(VectorType::Random(), 1e-7 + 1e-6 * internal::random(0., 1.)));
    173         for(int j = 0; j < 3; ++j)
    174             v.push_back(VectorType::Random());
    175     }
    176 
    177     KdBVH<double, Dim, BallType> tree(b.begin(), b.end());
    178     KdBVH<double, Dim, VectorType> vTree(v.begin(), v.end());
    179 
    180     BallPointStuff<Dim> i1, i2;
    181 
    182     double m1 = (std::numeric_limits<double>::max)(), m2 = m1;
    183 
    184     for(int i = 0; i < (int)b.size(); ++i)
    185         for(int j = 0; j < (int)v.size(); ++j)
    186             m1 = (std::min)(m1, i1.minimumOnObjectObject(b[i], v[j]));
    187 
    188     m2 = BVMinimize(tree, vTree, i2);
    189 
    190     VERIFY_IS_APPROX(m1, m2);
    191   }
    192 };
    193 
    194 
    195 void test_BVH()
    196 {
    197   for(int i = 0; i < g_repeat; i++) {
    198 #ifdef EIGEN_TEST_PART_1
    199     TreeTest<2> test2;
    200     CALL_SUBTEST(test2.testIntersect1());
    201     CALL_SUBTEST(test2.testMinimize1());
    202     CALL_SUBTEST(test2.testIntersect2());
    203     CALL_SUBTEST(test2.testMinimize2());
    204 #endif
    205 
    206 #ifdef EIGEN_TEST_PART_2
    207     TreeTest<3> test3;
    208     CALL_SUBTEST(test3.testIntersect1());
    209     CALL_SUBTEST(test3.testMinimize1());
    210     CALL_SUBTEST(test3.testIntersect2());
    211     CALL_SUBTEST(test3.testMinimize2());
    212 #endif
    213 
    214 #ifdef EIGEN_TEST_PART_3
    215     TreeTest<4> test4;
    216     CALL_SUBTEST(test4.testIntersect1());
    217     CALL_SUBTEST(test4.testMinimize1());
    218     CALL_SUBTEST(test4.testIntersect2());
    219     CALL_SUBTEST(test4.testMinimize2());
    220 #endif
    221   }
    222 }
    223