1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2009 Ilya Baran <ibaran@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
15namespace Eigen {
16
17template<typename Scalar, int Dim> AlignedBox<Scalar, Dim> bounding_box(const Matrix<Scalar, Dim, 1> &v) { return AlignedBox<Scalar, Dim>(v); }
18
19}
20
21
22template<int Dim>
23struct Ball
24{
25EIGEN_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};
35template<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
38inline double SQR(double x) { return x * x; }
39
40template<int Dim>
41struct 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
92template<int Dim>
93struct 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
195void 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