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 |
|
---|
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 | }
|
---|