test_point_inclusion.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
37 #include <geometric_shapes/bodies.h>
38 #include <geometric_shapes/shape_operations.h>
39 #include <geometric_shapes/body_operations.h>
40 #include <boost/filesystem.hpp>
41 #include <gtest/gtest.h>
42 #include "resources/config.h"
43 
44 // We expect surface points are counted inside.
45 #define EXPECT_SURF EXPECT_TRUE
46 
47 // split length into the largest number elem, such that sqrt(elem^2 + elem^2) <= length
48 double largestComponentForLength2D(const double length)
49 {
50  // HACK: sqrt(2) / 2 is a problem due to rounding errors since the distance
51  // is computed as 1.0000000000000002 . In such case we subtract an
52  // epsilon.
53 
54  double sq2 = sqrt(length / 2);
55  while (sq2 * sq2 + sq2 * sq2 > length)
56  sq2 -= std::numeric_limits<double>::epsilon();
57  return sq2;
58 }
59 
60 Eigen::Isometry3d getRandomPose(random_numbers::RandomNumberGenerator& g)
61 {
62  const Eigen::Vector3d t(g.uniformReal(-100, 100), g.uniformReal(-100, 100), g.uniformReal(-100, 100));
63 
64  double quat[4];
65  g.quaternion(quat);
66  const Eigen::Quaterniond r({ quat[3], quat[0], quat[1], quat[2] });
67 
68  return Eigen::Isometry3d::TranslationType(t) * r;
69 }
70 
71 TEST(SpherePointContainment, Basic)
72 {
73  shapes::Sphere shape(1.0);
74  bodies::Sphere sphere(&shape);
75 
76  // clang-format off
77  // zero
78  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.00)));
79  // general point outside
80  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(1.00, 1.00, 1.00)));
81 
82  // near single-axis maximum
83  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.99, 0.00, 0.00)));
84  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(1.00, 0.00, 0.00)));
85  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(1.01, 0.00, 0.00)));
86  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.00, 0.99, 0.00)));
87  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(0.00, 1.00, 0.00)));
88  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.00, 1.01, 0.00)));
89  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.99)));
90  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.00)));
91  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.01)));
92 
93  // near two-axis maximum
94  const double sq2e = largestComponentForLength2D(1.0);
95  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.70, 0.70, 0.00)));
96  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(sq2e, sq2e, 0.00)));
97  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.71, 0.71, 0.00)));
98  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.70, 0.00, 0.70)));
99  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(sq2e, 0.00, sq2e)));
100  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.71, 0.00, 0.71)));
101  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.00, 0.70, 0.70)));
102  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(0.00, sq2e, sq2e)));
103  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.00, 0.71, 0.71)));
104 
105  // near three-axis maximum
106  const double sq3 = sqrt(3) / 3;
107  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.57, 0.57, 0.57)));
108  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(sq3 , sq3 , sq3 )));
109  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.58, 0.58, 0.58)));
110 
111  // near three-axis maximum with translation
112  Eigen::Isometry3d pose;
113  pose.setIdentity();
114  pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0);
115  sphere.setPose(pose);
116 
117  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(1.57, 0.57, 0.57)));
118  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(1+sq3,sq3 , sq3 )));
119  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(1.58, 0.58, 0.58)));
120 
121  pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0);
122  sphere.setPose(pose);
123  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.57, 1.57, 0.57)));
124  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(sq3 ,1+sq3, sq3 )));
125  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.58, 1.58, 0.58)));
126 
127  pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0);
128  sphere.setPose(pose);
129  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.57, 0.57, 1.57)));
130  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(sq3 , sq3, 1+sq3)));
131  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.58, 0.58, 1.58)));
132  // clang-format on
133 }
134 
135 TEST(SpherePointContainment, SimpleInside)
136 {
137  shapes::Sphere shape(1.0);
138  bodies::Body* sphere = new bodies::Sphere(&shape);
139  sphere->setScale(1.05);
140  EXPECT_TRUE(sphere->containsPoint(0, 0, 1.0));
141 
142  random_numbers::RandomNumberGenerator r(0);
143  Eigen::Vector3d p;
144  for (int i = 0; i < 1000; ++i)
145  {
146  const Eigen::Isometry3d pos = getRandomPose(r);
147  sphere->setPose(pos);
148  sphere->setScale(r.uniformReal(0.1, 100.0));
149  sphere->setPadding(r.uniformReal(-0.001, 10.0));
150 
151  EXPECT_TRUE(sphere->samplePointInside(r, 100, p));
152  EXPECT_TRUE(sphere->containsPoint(p));
153  }
154  delete sphere;
155 }
156 
157 TEST(SpherePointContainment, SimpleOutside)
158 {
159  shapes::Sphere shape(1.0);
160  bodies::Body* sphere = new bodies::Sphere(&shape);
161  sphere->setScale(0.95);
162  bool contains = sphere->containsPoint(0, 0, 1.0);
163  delete sphere;
164  EXPECT_FALSE(contains);
165 }
166 
167 TEST(SpherePointContainment, ComplexInside)
168 {
169  shapes::Sphere shape(1.0);
170  bodies::Body* sphere = new bodies::Sphere(&shape);
171  sphere->setScale(0.95);
172  Eigen::Isometry3d pose;
173  pose.setIdentity();
174  pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
175  sphere->setPose(pose);
176  bool contains = sphere->containsPoint(0.5, 1, 1.0);
177  delete sphere;
178  EXPECT_TRUE(contains);
179 }
180 
181 TEST(SpherePointContainment, ComplexOutside)
182 {
183  shapes::Sphere shape(1.0);
184  bodies::Body* sphere = new bodies::Sphere(&shape);
185  sphere->setScale(0.95);
186  Eigen::Isometry3d pose;
187  pose.setIdentity();
188  pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
189  sphere->setPose(pose);
190  bool contains = sphere->containsPoint(0.5, 0.0, 0.0);
191  delete sphere;
192  EXPECT_FALSE(contains);
193 }
194 
195 TEST(BoxPointContainment, Basic)
196 {
197  shapes::Box shape(2.0, 2.0, 2.0);
198  bodies::Box box(&shape);
199 
200  // clang-format off
201  // zero
202  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.00)));
203  // general point outside
204  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(2.00, 2.00, 2.00)));
205 
206  // near single-axis maximum
207  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 0.00, 0.00)));
208  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 0.00, 0.00)));
209  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 0.00, 0.00)));
210  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.00, 0.99, 0.00)));
211  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(0.00, 1.00, 0.00)));
212  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(0.00, 1.01, 0.00)));
213  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.99)));
214  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.00)));
215  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.01)));
216 
217  // near two-axis maximum
218  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 0.99, 0.00)));
219  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 1.00, 0.00)));
220  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 1.01, 0.00)));
221  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 0.00, 0.99)));
222  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 0.00, 1.00)));
223  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 0.00, 1.01)));
224  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.00, 0.99, 0.99)));
225  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(0.00, 1.00, 1.00)));
226  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(0.00, 1.01, 1.01)));
227 
228  // near three-axis maximum
229  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 0.99, 0.99)));
230  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 1.00, 1.00)));
231  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 1.01, 1.01)));
232 
233  // near three-axis maximum with translation
234  Eigen::Isometry3d pose;
235  pose.setIdentity();
236  pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0);
237  box.setPose(pose);
238 
239  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(1.99, 0.99, 0.99)));
240  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(2.00, 1.00, 1.00)));
241  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(2.01, 1.01, 1.01)));
242 
243  pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0);
244  box.setPose(pose);
245  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 1.99, 0.99)));
246  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 2.00, 1.00)));
247  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 2.01, 1.01)));
248 
249  pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0);
250  box.setPose(pose);
251  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 0.99, 1.99)));
252  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 1.00, 2.00)));
253  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 1.01, 2.01)));
254  // clang-format on
255 }
256 
257 TEST(BoxPointContainment, SimpleInside)
258 {
259  shapes::Box shape(1.0, 2.0, 3.0);
260  bodies::Body* box = new bodies::Box(&shape);
261  box->setScale(0.95);
262  bool contains = box->containsPoint(0, 0, 1.0);
263  EXPECT_TRUE(contains);
264 
265  random_numbers::RandomNumberGenerator r;
266  Eigen::Vector3d p;
267  EXPECT_TRUE(box->samplePointInside(r, 100, p));
268  EXPECT_TRUE(box->containsPoint(p));
269 
270  delete box;
271 }
272 
273 TEST(BoxPointContainment, SimpleOutside)
274 {
275  shapes::Box shape(1.0, 2.0, 3.0);
276  bodies::Body* box = new bodies::Box(&shape);
277  box->setScale(0.95);
278  bool contains = box->containsPoint(0, 0, 3.0);
279  delete box;
280  EXPECT_FALSE(contains);
281 }
282 
283 TEST(BoxPointContainment, ComplexInside)
284 {
285  shapes::Box shape(1.0, 1.0, 1.0);
286  bodies::Body* box = new bodies::Box(&shape);
287  box->setScale(1.01);
288  Eigen::Isometry3d pose(Eigen::AngleAxisd(M_PI / 3.0, Eigen::Vector3d::UnitX()));
289  pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
290  box->setPose(pose);
291 
292  bool contains = box->containsPoint(1.5, 1.0, 1.5);
293  EXPECT_TRUE(contains);
294 
295  delete box;
296 }
297 
298 TEST(BoxPointContainment, Sampled)
299 {
300  shapes::Box shape(1.0, 2.0, 3.0);
301  bodies::Box box(&shape);
302 
303  random_numbers::RandomNumberGenerator r(0);
304  Eigen::Vector3d p;
305  for (int i = 0; i < 1000; ++i)
306  {
307  const Eigen::Isometry3d pos = getRandomPose(r);
308  box.setPose(pos);
309  box.setScale(r.uniformReal(0.1, 100.0));
310  box.setPadding(r.uniformReal(-0.001, 10.0));
311 
312  EXPECT_TRUE(box.samplePointInside(r, 100, p));
313  EXPECT_TRUE(box.containsPoint(p));
314  }
315 }
316 
317 TEST(BoxPointContainment, ComplexOutside)
318 {
319  shapes::Box shape(1.0, 1.0, 1.0);
320  bodies::Body* box = new bodies::Box(&shape);
321  box->setScale(1.01);
322  Eigen::Isometry3d pose(Eigen::AngleAxisd(M_PI / 3.0, Eigen::Vector3d::UnitX()));
323  pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
324  box->setPose(pose);
325 
326  bool contains = box->containsPoint(1.5, 1.5, 1.5);
327  delete box;
328  EXPECT_FALSE(contains);
329 }
330 
331 TEST(CylinderPointContainment, Basic)
332 {
333  shapes::Cylinder shape(1.0, 4.0);
334  bodies::Cylinder cylinder(&shape);
335 
336  // clang-format off
337  // zero
338  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.00)));
339  // general point outside
340  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(1.00, 1.00, 4.00)));
341 
342  // near single-axis maximum
343  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.99, 0.00, 0.00)));
344  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(1.00, 0.00, 0.00)));
345  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(1.01, 0.00, 0.00)));
346  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.00, 0.99, 0.00)));
347  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(0.00, 1.00, 0.00)));
348  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.00, 1.01, 0.00)));
349  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.99)));
350  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(0.00, 0.00, 2.00)));
351  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.00, 0.00, 2.01)));
352 
353  // near two-axis maximum
354  const double sq2 = sqrt(2) / 2;
355  const double sq2e = largestComponentForLength2D(1);
356 
357  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.70, 0.70, 0.00)));
358  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(sq2e, sq2e, 0.00)));
359  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.71, 0.71, 0.00)));
360  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.99, 0.00, 1.99)));
361  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(1.00, 0.00, 2.00)));
362  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(1.01, 0.00, 2.01)));
363  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.00, 0.99, 1.99)));
364  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(0.00, 1.00, 2.00)));
365  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.00, 1.01, 2.01)));
366 
367  // near three-axis maximum
368  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.70, 0.70, 1.99)));
369  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(sq2e, sq2e, 2.00)));
370  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.71, 0.71, 2.01)));
371 
372  // near three-axis maximum with translation
373  Eigen::Isometry3d pose;
374  pose.setIdentity();
375  pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0);
376  cylinder.setPose(pose);
377 
378  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(1.70, 0.70, 1.99)));
379  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(1+sq2,sq2 , 2.00)));
380  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(1.71, 0.71, 2.01)));
381 
382  pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0);
383  cylinder.setPose(pose);
384  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.70, 1.70, 1.99)));
385  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(sq2 ,1+sq2, 2.00)));
386  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.71, 1.71, 2.01)));
387 
388  pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0);
389  cylinder.setPose(pose);
390  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.70, 0.70, 2.99)));
391  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(sq2e, sq2e, 3.00)));
392  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.71, 0.71, 3.01)));
393  // clang-format on
394 }
395 
396 TEST(CylinderPointContainment, SimpleInside)
397 {
398  shapes::Cylinder shape(1.0, 4.0);
399  bodies::Body* cylinder = new bodies::Cylinder(&shape);
400  cylinder->setScale(1.05);
401  bool contains = cylinder->containsPoint(0, 0, 2.0);
402  delete cylinder;
403  EXPECT_TRUE(contains);
404 }
405 
406 TEST(CylinderPointContainment, SimpleOutside)
407 {
408  shapes::Cylinder shape(1.0, 4.0);
409  bodies::Body* cylinder = new bodies::Cylinder(&shape);
410  cylinder->setScale(0.95);
411  bool contains = cylinder->containsPoint(0, 0, 2.0);
412  delete cylinder;
413  EXPECT_FALSE(contains);
414 }
415 
416 TEST(CylinderPointContainment, CylinderPadding)
417 {
418  shapes::Cylinder shape(1.0, 4.0);
419  bodies::Body* cylinder = new bodies::Cylinder(&shape);
420  bool contains = cylinder->containsPoint(0, 1.01, 0);
421  EXPECT_FALSE(contains);
422  cylinder->setPadding(.02);
423  contains = cylinder->containsPoint(0, 1.01, 0);
424  EXPECT_TRUE(contains);
425  cylinder->setPadding(0.0);
426  bodies::BoundingSphere bsphere;
427  cylinder->computeBoundingSphere(bsphere);
428  EXPECT_TRUE(bsphere.radius > 2.0);
429 
430  delete cylinder;
431 }
432 
433 TEST(CylinderPointContainment, Sampled)
434 {
435  shapes::Cylinder shape(1.0, 4.0);
436  bodies::Cylinder cylinder(&shape);
437 
438  random_numbers::RandomNumberGenerator r(0);
439  Eigen::Vector3d p;
440  for (int i = 0; i < 1000; ++i)
441  {
442  const Eigen::Isometry3d pos = getRandomPose(r);
443  cylinder.setPose(pos);
444  cylinder.setScale(r.uniformReal(0.1, 100.0));
445  cylinder.setPadding(r.uniformReal(-0.001, 10.0));
446 
447  EXPECT_TRUE(cylinder.samplePointInside(r, 100, p));
448  EXPECT_TRUE(cylinder.containsPoint(p));
449  }
450 }
451 
452 TEST(MeshPointContainment, Basic)
453 {
454  // clang-format off
455  shapes::Mesh *ms = shapes::createMeshFromResource("file://" +
456  (boost::filesystem::path(TEST_RESOURCES_DIR) / "/box.dae").string());
457  ASSERT_TRUE(ms != nullptr);
458  bodies::ConvexMesh cubeMesh(ms);
459  cubeMesh.setScale(1.5);
460  cubeMesh.setPadding(0.5 * sqrt(3));
461 
462  // zero
463  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.00)));
464  // general point outside
465  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(3.00, 3.00, 3.00)));
466 
467  // near single-axis maximum
468  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(1.99, 0.00, 0.00)));
469  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(2.00, 0.00, 0.00)));
470  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(2.01, 0.00, 0.00)));
471  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 1.99, 0.00)));
472  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 2.00, 0.00)));
473  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(0.00, 2.01, 0.00)));
474  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.99)));
475  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 0.00, 2.00)));
476  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(0.00, 0.00, 2.01)));
477 
478  // near two-axis maximum
479  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(1.99, 1.99, 0.00)));
480  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(2.00, 2.00, 0.00)));
481  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(2.01, 2.01, 0.00)));
482  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(1.99, 0.00, 1.99)));
483  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(2.00, 0.00, 2.00)));
484  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(2.01, 0.00, 2.01)));
485  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 1.99, 1.99)));
486  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 2.00, 2.00)));
487  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(0.00, 2.01, 2.01)));
488 
489  // near three-axis maximum
490  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(1.99, 1.99, 1.99)));
491  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(2.00, 2.00, 2.00)));
492  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(2.01, 2.01, 2.01)));
493 
494  // near three-axis maximum with translation
495  Eigen::Isometry3d pose;
496  pose.setIdentity();
497  pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0);
498  cubeMesh.setPose(pose);
499 
500  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(2.99, 1.99, 1.99)));
501  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(3.00, 2.00, 2.00)));
502  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(3.01, 2.01, 2.01)));
503 
504  pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0);
505  cubeMesh.setPose(pose);
506  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(1.99, 2.99, 1.99)));
507  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(2.00, 3.00, 2.00)));
508  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(2.01, 3.01, 2.01)));
509 
510  pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0);
511  cubeMesh.setPose(pose);
512  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(1.99, 1.99, 2.99)));
513  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(2.00, 2.00, 3.00)));
514  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(2.01, 2.01, 3.01)));
515  // clang-format on
516 
517  delete ms;
518 }
519 
520 TEST(MeshPointContainment, Pr2Forearm)
521 {
522  shapes::Mesh* ms = shapes::createMeshFromResource(
523  "file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) / "/forearm_roll.stl").string());
524  ASSERT_TRUE(ms != nullptr);
525  bodies::Body* m = new bodies::ConvexMesh(ms);
526  ASSERT_TRUE(m != nullptr);
527  Eigen::Isometry3d t(Eigen::Isometry3d::Identity());
528  t.translation().x() = 1.0;
529  EXPECT_FALSE(m->cloneAt(t)->containsPoint(-1.0, 0.0, 0.0));
530 
531  random_numbers::RandomNumberGenerator r(0);
532  Eigen::Vector3d p;
533  bool found = true;
534  for (int i = 0; i < 10; ++i)
535  {
536  if (m->samplePointInside(r, 10000, p))
537  {
538  found = true;
539  EXPECT_TRUE(m->containsPoint(p));
540  }
541  }
542  EXPECT_TRUE(found);
543 
544  delete m;
545  delete ms;
546 }
547 
548 TEST(MergeBoundingSpheres, MergeTwoSpheres)
549 {
550  std::vector<bodies::BoundingSphere> spheres;
551 
552  bodies::BoundingSphere s1;
553  s1.center = Eigen::Vector3d(5.0, 0.0, 0.0);
554  s1.radius = 1.0;
555 
556  bodies::BoundingSphere s2;
557  s2.center = Eigen::Vector3d(-5.1, 0.0, 0.0);
558  s2.radius = 1.0;
559 
560  spheres.push_back(s1);
561  spheres.push_back(s2);
562 
563  bodies::BoundingSphere merged_sphere;
564  bodies::mergeBoundingSpheres(spheres, merged_sphere);
565 
566  EXPECT_NEAR(merged_sphere.center[0], -.05, .00001);
567  EXPECT_EQ(merged_sphere.radius, 6.05);
568 }
569 
570 int main(int argc, char** argv)
571 {
572  testing::InitGoogleTest(&argc, argv);
573  return RUN_ALL_TESTS();
574 }
bodies::BoundingSphere::radius
double radius
Definition: bodies.h:128
bodies::Sphere
Definition of a sphere.
Definition: bodies.h:327
bodies::Sphere::containsPoint
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:145
getRandomPose
Eigen::Isometry3d getRandomPose(random_numbers::RandomNumberGenerator &g)
Definition: test_point_inclusion.cpp:60
random_numbers::RandomNumberGenerator::uniformReal
double uniformReal(double lower_bound, double upper_bound)
shapes::Cylinder
Definition of a cylinder Length is along z axis. Origin is at center of mass.
Definition: shapes.h:127
bodies::Cylinder::containsPoint
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:319
bodies::Body::containsPoint
bool containsPoint(double x, double y, double z, bool verbose=false) const
Check if a point is inside the body.
Definition: bodies.h:244
shapes::Mesh
Definition of a triangle mesh By convention the "center" of the shape is at the origin....
Definition: shapes.h:281
bodies::Box::containsPoint
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:572
bodies::Body
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
Definition: bodies.h:121
bodies::Body::computeBoundingSphere
virtual void computeBoundingSphere(BoundingSphere &sphere) const =0
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for.
body_operations.h
EXPECT_TRUE
#define EXPECT_TRUE(args)
bodies::BoundingSphere
Definition of a sphere that bounds another object.
Definition: bodies.h:93
bodies::Body::samplePointInside
virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:129
shapes::Box
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:226
random_numbers::RandomNumberGenerator::quaternion
void quaternion(double value[4])
shapes::Sphere
Definition of a sphere.
Definition: shapes.h:106
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
TEST
TEST(SpherePointContainment, Basic)
Definition: test_point_inclusion.cpp:71
EXPECT_SURF
#define EXPECT_SURF
Definition: test_point_inclusion.cpp:45
bodies::Box
Definition of a box.
Definition: bodies.h:443
shapes::createMeshFromResource
Mesh * createMeshFromResource(const std::string &resource)
Load a mesh from a resource that contains a mesh that can be loaded by assimp.
Definition: mesh_operations.cpp:234
r
S r
bodies::Cylinder::samplePointInside
bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const override
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:381
random_numbers::RandomNumberGenerator
shape_operations.h
bodies.h
bodies::BoundingSphere::center
Eigen::Vector3d center
Definition: bodies.h:127
largestComponentForLength2D
double largestComponentForLength2D(const double length)
Definition: test_point_inclusion.cpp:48
M_PI
#define M_PI
bodies::Body::setScale
void setScale(double scale)
If the dimension of the body should be scaled, this method sets the scale. Default is 1....
Definition: bodies.h:152
bodies::mergeBoundingSpheres
void mergeBoundingSpheres(const std::vector< BoundingSphere > &spheres, BoundingSphere &mergedSphere)
Compute a bounding sphere to enclose a set of bounding spheres.
Definition: body_operations.cpp:147
bodies::Body::setPose
void setPose(const Eigen::Isometry3d &pose)
Set the pose of the body. Default is identity.
Definition: bodies.h:205
bodies::Body::setPadding
void setPadding(double padd)
If constant padding should be added to the body, this method sets the padding. Default is 0....
Definition: bodies.h:179
bodies::Box::samplePointInside
bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const override
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:564
main
int main(int argc, char **argv)
Definition: test_point_inclusion.cpp:570
bodies::Cylinder
Definition of a cylinder.
Definition: bodies.h:380
bodies::ConvexMesh
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:506
bodies::ConvexMesh::containsPoint
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:794
EXPECT_EQ
#define EXPECT_EQ(a, b)
EXPECT_FALSE
#define EXPECT_FALSE(args)
bodies::Body::cloneAt
BodyPtr cloneAt(const Eigen::Isometry3d &pose) const
Get a clone of this body, but one that is located at the pose pose.
Definition: bodies.h:290

geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Sun Oct 1 2023 02:40:16