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
15 namespace Eigen {
16
bounding_box(const Matrix<Scalar,Dim,1> & v)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
BallBall29 Ball() {}
BallBall30 Ball(const VectorType &c, double r) : center(c), radius(r) {}
31
32 VectorType center;
33 double radius;
34 };
bounding_box(const Ball<Dim> & b)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
SQR(double x)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
BallPointStuffBallPointStuff48 BallPointStuff() : calls(0), count(0) {}
BallPointStuffBallPointStuff49 BallPointStuff(const VectorType &inP) : p(inP), calls(0), count(0) {}
50
51
intersectVolumeBallPointStuff52 bool intersectVolume(const BoxType &r) { ++calls; return r.contains(p); }
intersectObjectBallPointStuff53 bool intersectObject(const BallType &b) {
54 ++calls;
55 if((b.center - p).squaredNorm() < SQR(b.radius))
56 ++count;
57 return false; //continue
58 }
59
intersectVolumeVolumeBallPointStuff60 bool intersectVolumeVolume(const BoxType &r1, const BoxType &r2) { ++calls; return !(r1.intersection(r2)).isNull(); }
intersectVolumeObjectBallPointStuff61 bool intersectVolumeObject(const BoxType &r, const BallType &b) { ++calls; return r.squaredExteriorDistance(b.center) < SQR(b.radius); }
intersectObjectVolumeBallPointStuff62 bool intersectObjectVolume(const BallType &b, const BoxType &r) { ++calls; return r.squaredExteriorDistance(b.center) < SQR(b.radius); }
intersectObjectObjectBallPointStuff63 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 }
intersectVolumeObjectBallPointStuff69 bool intersectVolumeObject(const BoxType &r, const VectorType &v) { ++calls; return r.contains(v); }
intersectObjectObjectBallPointStuff70 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
minimumOnVolumeBallPointStuff77 double minimumOnVolume(const BoxType &r) { ++calls; return r.squaredExteriorDistance(p); }
minimumOnObjectBallPointStuff78 double minimumOnObject(const BallType &b) { ++calls; return (std::max)(0., (b.center - p).squaredNorm() - SQR(b.radius)); }
minimumOnVolumeVolumeBallPointStuff79 double minimumOnVolumeVolume(const BoxType &r1, const BoxType &r2) { ++calls; return r1.squaredExteriorDistance(r2); }
minimumOnVolumeObjectBallPointStuff80 double minimumOnVolumeObject(const BoxType &r, const BallType &b) { ++calls; return SQR((std::max)(0., r.exteriorDistance(b.center) - b.radius)); }
minimumOnObjectVolumeBallPointStuff81 double minimumOnObjectVolume(const BallType &b, const BoxType &r) { ++calls; return SQR((std::max)(0., r.exteriorDistance(b.center) - b.radius)); }
minimumOnObjectObjectBallPointStuff82 double minimumOnObjectObject(const BallType &b1, const BallType &b2){ ++calls; return SQR((std::max)(0., (b1.center - b2.center).norm() - b1.radius - b2.radius)); }
minimumOnVolumeObjectBallPointStuff83 double minimumOnVolumeObject(const BoxType &r, const VectorType &v) { ++calls; return r.squaredExteriorDistance(v); }
minimumOnObjectObjectBallPointStuff84 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
testIntersect1TreeTest101 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
testMinimize1TreeTest120 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
testIntersect2TreeTest141 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
testMinimize2TreeTest166 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
test_BVH()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