BVH.cpp
Go to the documentation of this file.
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 {
26 
27  typedef Matrix<double, Dim, 1> VectorType;
28 
29  Ball() {}
30  Ball(const VectorType &c, double r) : center(c), radius(r) {}
31 
33  double radius;
34 };
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;
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 {
96  typedef std::vector<VectorType, aligned_allocator<VectorType> > VectorTypeList;
98  typedef std::vector<BallType, aligned_allocator<BallType> > BallTypeList;
100 
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 
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 
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 
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 
196 {
197  for(int i = 0; i < g_repeat; i++) {
198 #ifdef EIGEN_TEST_PART_1
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
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
216  CALL_SUBTEST(test4.testIntersect1());
217  CALL_SUBTEST(test4.testMinimize1());
218  CALL_SUBTEST(test4.testIntersect2());
219  CALL_SUBTEST(test4.testMinimize2());
220 #endif
221  }
222 }
double minimumOnObjectObject(const BallType &b, const VectorType &v)
Definition: BVH.cpp:84
#define max(a, b)
Definition: datatypes.h:20
Ball< Dim > BallType
Definition: BVH.cpp:45
BallPointStuff(const VectorType &inP)
Definition: BVH.cpp:49
VectorType center
Definition: BVH.cpp:32
double minimumOnVolumeVolume(const BoxType &r1, const BoxType &r2)
Definition: BVH.cpp:79
BallPointStuff()
Definition: BVH.cpp:48
Scalar * b
Definition: benchVecAdd.cpp:17
void test3(double add, OptionalJacobian< 2, 1 > H={})
int calls
Definition: BVH.cpp:87
Box2d bounding_box(const Vector2d &v)
Definition: BVH_Example.cpp:9
std::vector< BallType, aligned_allocator< BallType > > BallTypeList
Definition: BVH.cpp:98
#define min(a, b)
Definition: datatypes.h:19
void testIntersect1()
Definition: BVH.cpp:101
MatrixType m2(n_dims)
AlignedBox< double, Dim > BoxType
Definition: BVH.cpp:99
void test4(OptionalJacobian< 2, 3 > H={})
double minimumOnObjectObject(const BallType &b1, const BallType &b2)
Definition: BVH.cpp:82
Minimizer::Scalar BVMinimize(const BVH &tree, Minimizer &minimizer)
Definition: BVAlgorithms.h:219
Ball(const VectorType &c, double r)
Definition: BVH.cpp:30
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
void testIntersect2()
Definition: BVH.cpp:141
Vector2 b2(4, -5)
bool intersectObjectVolume(const BallType &b, const BoxType &r)
Definition: BVH.cpp:62
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Ball< Dim > BallType
Definition: BVH.cpp:97
EIGEN_DEVICE_FUNC NonInteger exteriorDistance(const MatrixBase< Derived > &p) const
Definition: AlignedBox.h:312
bool intersectObjectObject(const BallType &b, const VectorType &v)
Definition: BVH.cpp:70
static const Point3 pt(1.0, 2.0, 3.0)
void test2(OptionalJacobian<-1,-1 > H={})
double minimumOnObjectVolume(const BallType &b, const BoxType &r)
Definition: BVH.cpp:81
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Size)
Definition: Memory.h:842
An axis aligned box.
double minimumOnVolume(const BoxType &r)
Definition: BVH.cpp:77
#define VERIFY_IS_APPROX(a, b)
std::vector< VectorType, aligned_allocator< VectorType > > VectorTypeList
Definition: BVH.cpp:96
bool intersectVolume(const BoxType &r)
Definition: BVH.cpp:52
Matrix3d m1
Definition: IOFormat.cpp:2
static int g_repeat
Definition: main.h:169
Definition: BVH.cpp:93
Array< int, Dynamic, 1 > v
bool intersectObjectObject(const BallType &b1, const BallType &b2)
Definition: BVH.cpp:63
double radius
Definition: BVH.cpp:33
bool intersectVolumeObject(const BoxType &r, const BallType &b)
Definition: BVH.cpp:61
Matrix< double, Dim, 1 > VectorType
Definition: BVH.cpp:95
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double r2
double minimumOnObject(const BallType &b)
Definition: BVH.cpp:78
void testMinimize2()
Definition: BVH.cpp:166
double minimumOnVolumeObject(const BoxType &r, const VectorType &v)
Definition: BVH.cpp:83
int count
Definition: BVH.cpp:88
void BVIntersect(const BVH &tree, Intersector &intersector)
Definition: BVAlgorithms.h:79
Definition: BVH.cpp:23
EIGEN_DEVICE_FUNC Scalar squaredExteriorDistance(const MatrixBase< Derived > &p) const
Definition: AlignedBox.h:408
bool intersectObject(const BallType &b)
Definition: BVH.cpp:53
static const double r1
#define CALL_SUBTEST(FUNC)
Definition: main.h:399
#define VERIFY(a)
Definition: main.h:380
bool intersectVolumeVolume(const BoxType &r1, const BoxType &r2)
Definition: BVH.cpp:60
AlignedBox< double, Dim > BoxType
Definition: BVH.cpp:46
Matrix< double, Dim, 1 > VectorType
Definition: BVH.cpp:44
double Scalar
Definition: BVH.cpp:43
float * p
EIGEN_DEVICE_FUNC AlignedBox intersection(const AlignedBox &b) const
Definition: AlignedBox.h:266
void testMinimize1()
Definition: BVH.cpp:120
double SQR(double x)
Definition: BVH.cpp:38
VectorType p
Definition: BVH.cpp:86
EIGEN_DECLARE_TEST(BVH)
Definition: BVH.cpp:195
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
A simple bounding volume hierarchy based on AlignedBox.
Definition: KdBVH.h:68
std::ptrdiff_t j
EIGEN_DEVICE_FUNC bool contains(const MatrixBase< Derived > &p) const
Definition: AlignedBox.h:218
double minimumOnVolumeObject(const BoxType &r, const BallType &b)
Definition: BVH.cpp:80
bool intersectVolumeObject(const BoxType &r, const VectorType &v)
Definition: BVH.cpp:69
Vector2 b1(2, -1)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:33:59