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::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:592
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:184
main
int main(int argc, char **argv)
Definition: test_point_inclusion.cpp:570
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:168
bodies::Body::setPose
void setPose(const Eigen::Isometry3d &pose)
Set the pose of the body. Default is identity.
Definition: bodies.h:175
shapes::Cylinder
Definition of a cylinder Length is along z axis. Origin is at center of mass.
Definition: shapes.h:127
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:210
bodies::Cylinder
Definition of a cylinder.
Definition: bodies.h:357
random_numbers::RandomNumberGenerator::quaternion
void quaternion(double value[4])
bodies::Sphere
Definition of a sphere.
Definition: bodies.h:293
getRandomPose
Eigen::Isometry3d getRandomPose(random_numbers::RandomNumberGenerator &g)
Definition: test_point_inclusion.cpp:60
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:122
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
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:409
EXPECT_SURF
#define EXPECT_SURF
Definition: test_point_inclusion.cpp:45
bodies::BoundingSphere::center
Eigen::Vector3d center
Definition: bodies.h:63
bodies::BoundingSphere
Definition of a sphere that bounds another object.
Definition: bodies.h:61
EXPECT_FALSE
#define EXPECT_FALSE(args)
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
EXPECT_EQ
#define EXPECT_EQ(a, b)
bodies::ConvexMesh
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:504
body_operations.h
bodies::Box
Definition of a box.
Definition: bodies.h:428
random_numbers::RandomNumberGenerator::uniformReal
double uniformReal(double lower_bound, double upper_bound)
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:256
shapes::Sphere
Definition of a sphere.
Definition: shapes.h:106
largestComponentForLength2D
double largestComponentForLength2D(const double length)
Definition: test_point_inclusion.cpp:48
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...
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:202
shapes::Mesh
Definition of a triangle mesh By convention the "center" of the shape is at the origin. For a mesh this implies that the AABB of the mesh is centered at the origin. Some methods may not work with arbitrary meshes whose AABB is not centered at the origin. Padding is not applied to vertices plainly coordinate-wise, but instead the padding value is added to the length of the direction vector between centroid and each vertex.
Definition: shapes.h:281
shapes::Box
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:226
TEST
TEST(SpherePointContainment, Basic)
Definition: test_point_inclusion.cpp:71
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:788
shape_operations.h
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:584
r
r
EXPECT_TRUE
#define EXPECT_TRUE(args)
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:347
bodies::Body
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
Definition: bodies.h:89
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:149
random_numbers::RandomNumberGenerator

geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Fri Apr 14 2023 02:14:40