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 
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 TEST(SpherePointContainment, Basic)
61 {
62  shapes::Sphere shape(1.0);
63  bodies::Sphere sphere(&shape);
64 
65  // clang-format off
66  // zero
67  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.00)));
68  // general point outside
69  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(1.00, 1.00, 1.00)));
70 
71  // near single-axis maximum
72  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.99, 0.00, 0.00)));
73  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(1.00, 0.00, 0.00)));
74  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(1.01, 0.00, 0.00)));
75  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.00, 0.99, 0.00)));
76  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(0.00, 1.00, 0.00)));
77  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.00, 1.01, 0.00)));
78  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.99)));
79  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.00)));
80  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.01)));
81 
82  // near two-axis maximum
83  const double sq2e = largestComponentForLength2D(1.0);
84  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.70, 0.70, 0.00)));
85  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(sq2e, sq2e, 0.00)));
86  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.71, 0.71, 0.00)));
87  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.70, 0.00, 0.70)));
88  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(sq2e, 0.00, sq2e)));
89  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.71, 0.00, 0.71)));
90  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.00, 0.70, 0.70)));
91  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(0.00, sq2e, sq2e)));
92  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.00, 0.71, 0.71)));
93 
94  // near three-axis maximum
95  const double sq3 = sqrt(3) / 3;
96  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.57, 0.57, 0.57)));
97  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(sq3 , sq3 , sq3 )));
98  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.58, 0.58, 0.58)));
99 
100  // near three-axis maximum with translation
101  Eigen::Isometry3d pose;
102  pose.setIdentity();
103  pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0);
104  sphere.setPose(pose);
105 
106  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(1.57, 0.57, 0.57)));
107  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(1+sq3,sq3 , sq3 )));
108  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(1.58, 0.58, 0.58)));
109 
110  pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0);
111  sphere.setPose(pose);
112  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.57, 1.57, 0.57)));
113  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(sq3 ,1+sq3, sq3 )));
114  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.58, 1.58, 0.58)));
115 
116  pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0);
117  sphere.setPose(pose);
118  EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.57, 0.57, 1.57)));
119  EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(sq3 , sq3, 1+sq3)));
120  EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.58, 0.58, 1.58)));
121  // clang-format on
122 }
123 
124 TEST(SpherePointContainment, SimpleInside)
125 {
126  shapes::Sphere shape(1.0);
127  bodies::Body* sphere = new bodies::Sphere(&shape);
128  sphere->setScale(1.05);
129  bool contains = sphere->containsPoint(0, 0, 1.0);
131  Eigen::Vector3d p;
132  EXPECT_TRUE(sphere->samplePointInside(r, 100, p));
133  EXPECT_TRUE(sphere->containsPoint(p));
134  EXPECT_TRUE(contains);
135  delete sphere;
136 }
137 
138 TEST(SpherePointContainment, SimpleOutside)
139 {
140  shapes::Sphere shape(1.0);
141  bodies::Body* sphere = new bodies::Sphere(&shape);
142  sphere->setScale(0.95);
143  bool contains = sphere->containsPoint(0, 0, 1.0);
144  delete sphere;
145  EXPECT_FALSE(contains);
146 }
147 
148 TEST(SpherePointContainment, ComplexInside)
149 {
150  shapes::Sphere shape(1.0);
151  bodies::Body* sphere = new bodies::Sphere(&shape);
152  sphere->setScale(0.95);
153  Eigen::Isometry3d pose;
154  pose.setIdentity();
155  pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
156  sphere->setPose(pose);
157  bool contains = sphere->containsPoint(0.5, 1, 1.0);
158  delete sphere;
159  EXPECT_TRUE(contains);
160 }
161 
162 TEST(SpherePointContainment, ComplexOutside)
163 {
164  shapes::Sphere shape(1.0);
165  bodies::Body* sphere = new bodies::Sphere(&shape);
166  sphere->setScale(0.95);
167  Eigen::Isometry3d pose;
168  pose.setIdentity();
169  pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
170  sphere->setPose(pose);
171  bool contains = sphere->containsPoint(0.5, 0.0, 0.0);
172  delete sphere;
173  EXPECT_FALSE(contains);
174 }
175 
176 TEST(SphereRayIntersection, SimpleRay1)
177 {
178  shapes::Sphere shape(1.0);
179  bodies::Body* sphere = new bodies::Sphere(&shape);
180  sphere->setScale(1.05);
181 
182  Eigen::Vector3d ray_o(5, 0, 0);
183  Eigen::Vector3d ray_d(-1, 0, 0);
185  bool intersect = sphere->intersectsRay(ray_o, ray_d, &p);
186 
187  delete sphere;
188  EXPECT_TRUE(intersect);
189  EXPECT_EQ(2, (int)p.size());
190  EXPECT_NEAR(p[0].x(), 1.05, 1e-6);
191  EXPECT_NEAR(p[1].x(), -1.05, 1e-6);
192 }
193 
194 TEST(SphereRayIntersection, SimpleRay2)
195 {
196  shapes::Sphere shape(1.0);
197  bodies::Body* sphere = new bodies::Sphere(&shape);
198  sphere->setScale(1.05);
199 
200  Eigen::Vector3d ray_o(5, 0, 0);
201  Eigen::Vector3d ray_d(1, 0, 0);
203  bool intersect = sphere->intersectsRay(ray_o, ray_d, &p);
204 
205  delete sphere;
206  EXPECT_FALSE(intersect);
207  EXPECT_EQ(0, (int)p.size());
208 }
209 
210 TEST(BoxPointContainment, Basic)
211 {
212  shapes::Box shape(2.0, 2.0, 2.0);
213  bodies::Box box(&shape);
214 
215  // clang-format off
216  // zero
217  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.00)));
218  // general point outside
219  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(2.00, 2.00, 2.00)));
220 
221  // near single-axis maximum
222  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 0.00, 0.00)));
223  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 0.00, 0.00)));
224  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 0.00, 0.00)));
225  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.00, 0.99, 0.00)));
226  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(0.00, 1.00, 0.00)));
227  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(0.00, 1.01, 0.00)));
228  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.99)));
229  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.00)));
230  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.01)));
231 
232  // near two-axis maximum
233  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 0.99, 0.00)));
234  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 1.00, 0.00)));
235  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 1.01, 0.00)));
236  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 0.00, 0.99)));
237  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 0.00, 1.00)));
238  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 0.00, 1.01)));
239  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.00, 0.99, 0.99)));
240  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(0.00, 1.00, 1.00)));
241  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(0.00, 1.01, 1.01)));
242 
243  // near three-axis maximum
244  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 0.99, 0.99)));
245  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 1.00, 1.00)));
246  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 1.01, 1.01)));
247 
248  // near three-axis maximum with translation
249  Eigen::Isometry3d pose;
250  pose.setIdentity();
251  pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0);
252  box.setPose(pose);
253 
254  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(1.99, 0.99, 0.99)));
255  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(2.00, 1.00, 1.00)));
256  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(2.01, 1.01, 1.01)));
257 
258  pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0);
259  box.setPose(pose);
260  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 1.99, 0.99)));
261  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 2.00, 1.00)));
262  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 2.01, 1.01)));
263 
264  pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0);
265  box.setPose(pose);
266  EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 0.99, 1.99)));
267  EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 1.00, 2.00)));
268  EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 1.01, 2.01)));
269  // clang-format on
270 }
271 
272 TEST(BoxPointContainment, SimpleInside)
273 {
274  shapes::Box shape(1.0, 2.0, 3.0);
275  bodies::Body* box = new bodies::Box(&shape);
276  box->setScale(0.95);
277  bool contains = box->containsPoint(0, 0, 1.0);
278  EXPECT_TRUE(contains);
279 
281  Eigen::Vector3d p;
282  EXPECT_TRUE(box->samplePointInside(r, 100, p));
283  EXPECT_TRUE(box->containsPoint(p));
284 
285  delete box;
286 }
287 
288 TEST(BoxPointContainment, SimpleOutside)
289 {
290  shapes::Box shape(1.0, 2.0, 3.0);
291  bodies::Body* box = new bodies::Box(&shape);
292  box->setScale(0.95);
293  bool contains = box->containsPoint(0, 0, 3.0);
294  delete box;
295  EXPECT_FALSE(contains);
296 }
297 
298 TEST(BoxPointContainment, ComplexInside)
299 {
300  shapes::Box shape(1.0, 1.0, 1.0);
301  bodies::Body* box = new bodies::Box(&shape);
302  box->setScale(1.01);
303  Eigen::Isometry3d pose(Eigen::AngleAxisd(M_PI / 3.0, Eigen::Vector3d::UnitX()));
304  pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
305  box->setPose(pose);
306 
307  bool contains = box->containsPoint(1.5, 1.0, 1.5);
308  EXPECT_TRUE(contains);
309 
311  Eigen::Vector3d p;
312  for (int i = 0; i < 100; ++i)
313  {
314  EXPECT_TRUE(box->samplePointInside(r, 100, p));
315  EXPECT_TRUE(box->containsPoint(p));
316  }
317 
318  delete box;
319 }
320 
321 TEST(BoxPointContainment, ComplexOutside)
322 {
323  shapes::Box shape(1.0, 1.0, 1.0);
324  bodies::Body* box = new bodies::Box(&shape);
325  box->setScale(1.01);
326  Eigen::Isometry3d pose(Eigen::AngleAxisd(M_PI / 3.0, Eigen::Vector3d::UnitX()));
327  pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
328  box->setPose(pose);
329 
330  bool contains = box->containsPoint(1.5, 1.5, 1.5);
331  delete box;
332  EXPECT_FALSE(contains);
333 }
334 
335 TEST(BoxRayIntersection, SimpleRay1)
336 {
337  shapes::Box shape(1.0, 1.0, 3.0);
338  bodies::Body* box = new bodies::Box(&shape);
339  box->setScale(0.95);
340 
341  Eigen::Vector3d ray_o(10, 0.449, 0);
342  Eigen::Vector3d ray_d(-1, 0, 0);
344 
345  bool intersect = box->intersectsRay(ray_o, ray_d, &p);
346 
347  // for (unsigned int i = 0; i < p.size() ; ++i)
348  // printf("intersection at %f, %f, %f\n", p[i].x(), p[i].y(), p[i].z());
349 
350  delete box;
351  EXPECT_TRUE(intersect);
352 }
353 
354 TEST(BoxRayIntersection, SimpleRay2)
355 {
356  shapes::Box shape(0.9, 0.01, 1.2);
357  bodies::Body* box = new bodies::Box(&shape);
358 
359  Eigen::Isometry3d pose(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()));
360  pose.translation() = Eigen::Vector3d(0, 0.005, 0.6);
361  box->setPose(pose);
362 
363  Eigen::Vector3d ray_o(0, 5, 1.6);
364  Eigen::Vector3d ray_d(0, -5.195, -0.77);
366 
367  bool intersect = box->intersectsRay(ray_o, ray_d, &p);
368  EXPECT_TRUE(intersect);
369 
370  intersect = box->intersectsRay(ray_o, ray_d.normalized(), &p);
371  EXPECT_TRUE(intersect);
372 
373  delete box;
374 }
375 
376 TEST(BoxRayIntersection, SimpleRay3)
377 {
378  shapes::Box shape(0.02, 0.4, 1.2);
379  bodies::Body* box = new bodies::Box(&shape);
380 
381  Eigen::Isometry3d pose(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()));
382  pose.translation() = Eigen::Vector3d(0.45, -0.195, 0.6);
383  box->setPose(pose);
384 
385  Eigen::Vector3d ray_o(0, -2, 1.11);
386  Eigen::Vector3d ray_d(0, 1.8, -0.669);
388 
389  bool intersect = box->intersectsRay(ray_o, ray_d, &p);
390  EXPECT_FALSE(intersect);
391 
392  intersect = box->intersectsRay(ray_o, ray_d.normalized(), &p);
393  EXPECT_FALSE(intersect);
394 
395  delete box;
396 }
397 
398 TEST(CylinderPointContainment, Basic)
399 {
400  shapes::Cylinder shape(1.0, 4.0);
401  bodies::Cylinder cylinder(&shape);
402 
403  // clang-format off
404  // zero
405  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.00)));
406  // general point outside
407  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(1.00, 1.00, 4.00)));
408 
409  // near single-axis maximum
410  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.99, 0.00, 0.00)));
411  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(1.00, 0.00, 0.00)));
412  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(1.01, 0.00, 0.00)));
413  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.00, 0.99, 0.00)));
414  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(0.00, 1.00, 0.00)));
415  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.00, 1.01, 0.00)));
416  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.99)));
417  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(0.00, 0.00, 2.00)));
418  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.00, 0.00, 2.01)));
419 
420  // near two-axis maximum
421  const double sq2 = sqrt(2) / 2;
422  const double sq2e = largestComponentForLength2D(1);
423 
424  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.70, 0.70, 0.00)));
425  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(sq2e, sq2e, 0.00)));
426  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.71, 0.71, 0.00)));
427  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.99, 0.00, 1.99)));
428  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(1.00, 0.00, 2.00)));
429  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(1.01, 0.00, 2.01)));
430  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.00, 0.99, 1.99)));
431  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(0.00, 1.00, 2.00)));
432  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.00, 1.01, 2.01)));
433 
434  // near three-axis maximum
435  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.70, 0.70, 1.99)));
436  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(sq2e, sq2e, 2.00)));
437  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.71, 0.71, 2.01)));
438 
439  // near three-axis maximum with translation
440  Eigen::Isometry3d pose;
441  pose.setIdentity();
442  pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0);
443  cylinder.setPose(pose);
444 
445  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(1.70, 0.70, 1.99)));
446  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(1+sq2,sq2 , 2.00)));
447  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(1.71, 0.71, 2.01)));
448 
449  pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0);
450  cylinder.setPose(pose);
451  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.70, 1.70, 1.99)));
452  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(sq2 ,1+sq2, 2.00)));
453  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.71, 1.71, 2.01)));
454 
455  pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0);
456  cylinder.setPose(pose);
457  EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.70, 0.70, 2.99)));
458  EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(sq2e, sq2e, 3.00)));
459  EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.71, 0.71, 3.01)));
460  // clang-format on
461 }
462 
463 TEST(CylinderPointContainment, SimpleInside)
464 {
465  shapes::Cylinder shape(1.0, 4.0);
466  bodies::Body* cylinder = new bodies::Cylinder(&shape);
467  cylinder->setScale(1.05);
468  bool contains = cylinder->containsPoint(0, 0, 2.0);
469  delete cylinder;
470  EXPECT_TRUE(contains);
471 }
472 
473 TEST(CylinderPointContainment, SimpleOutside)
474 {
475  shapes::Cylinder shape(1.0, 4.0);
476  bodies::Body* cylinder = new bodies::Cylinder(&shape);
477  cylinder->setScale(0.95);
478  bool contains = cylinder->containsPoint(0, 0, 2.0);
479  delete cylinder;
480  EXPECT_FALSE(contains);
481 }
482 
483 TEST(CylinderPointContainment, CylinderPadding)
484 {
485  shapes::Cylinder shape(1.0, 4.0);
486  bodies::Body* cylinder = new bodies::Cylinder(&shape);
487  bool contains = cylinder->containsPoint(0, 1.01, 0);
488  EXPECT_FALSE(contains);
489  cylinder->setPadding(.02);
490  contains = cylinder->containsPoint(0, 1.01, 0);
491  EXPECT_TRUE(contains);
492  cylinder->setPadding(0.0);
493  bodies::BoundingSphere bsphere;
494  cylinder->computeBoundingSphere(bsphere);
495  EXPECT_TRUE(bsphere.radius > 2.0);
496 
498  Eigen::Vector3d p;
499  for (int i = 0; i < 1000; ++i)
500  {
501  EXPECT_TRUE(cylinder->samplePointInside(r, 100, p));
502  EXPECT_TRUE(cylinder->containsPoint(p));
503  }
504  delete cylinder;
505 }
506 
507 TEST(MeshPointContainment, Basic)
508 {
509  // clang-format off
511  (boost::filesystem::path(TEST_RESOURCES_DIR) / "/box.dae").string());
512  ASSERT_TRUE(ms != nullptr);
513  bodies::ConvexMesh cubeMesh(ms);
514 
515  // zero
516  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.00)));
517  // general point outside
518  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(2.00, 2.00, 2.00)));
519 
520  // near single-axis maximum
521  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.99, 0.00, 0.00)));
522  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(1.00, 0.00, 0.00)));
523  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(1.01, 0.00, 0.00)));
524  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 0.99, 0.00)));
525  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 1.00, 0.00)));
526  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(0.00, 1.01, 0.00)));
527  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.99)));
528  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.00)));
529  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.01)));
530 
531  // near two-axis maximum
532  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.99, 0.99, 0.00)));
533  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(1.00, 1.00, 0.00)));
534  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(1.01, 1.01, 0.00)));
535  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.99, 0.00, 0.99)));
536  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(1.00, 0.00, 1.00)));
537  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(1.01, 0.00, 1.01)));
538  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 0.99, 0.99)));
539  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 1.00, 1.00)));
540  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(0.00, 1.01, 1.01)));
541 
542  // near three-axis maximum
543  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.99, 0.99, 0.99)));
544  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(1.00, 1.00, 1.00)));
545  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(1.01, 1.01, 1.01)));
546 
547  // near three-axis maximum with translation
548  Eigen::Isometry3d pose;
549  pose.setIdentity();
550  pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0);
551  cubeMesh.setPose(pose);
552 
553  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(1.99, 0.99, 0.99)));
554  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(2.00, 1.00, 1.00)));
555  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(2.01, 1.01, 1.01)));
556 
557  pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0);
558  cubeMesh.setPose(pose);
559  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.99, 1.99, 0.99)));
560  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(1.00, 2.00, 1.00)));
561  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(1.01, 2.01, 1.01)));
562 
563  pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0);
564  cubeMesh.setPose(pose);
565  EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.99, 0.99, 1.99)));
566  EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(1.00, 1.00, 2.00)));
567  EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(1.01, 1.01, 2.01)));
568  // clang-format on
569 
570  delete ms;
571 }
572 
573 TEST(MeshPointContainment, Pr2Forearm)
574 {
576  "file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) / "/forearm_roll.stl").string());
577  ASSERT_TRUE(ms != nullptr);
578  bodies::Body* m = new bodies::ConvexMesh(ms);
579  ASSERT_TRUE(m != nullptr);
580  Eigen::Isometry3d t(Eigen::Isometry3d::Identity());
581  t.translation().x() = 1.0;
582  EXPECT_FALSE(m->cloneAt(t)->containsPoint(-1.0, 0.0, 0.0));
583 
585  Eigen::Vector3d p;
586  bool found = true;
587  for (int i = 0; i < 10; ++i)
588  {
589  if (m->samplePointInside(r, 10000, p))
590  {
591  found = true;
592  EXPECT_TRUE(m->containsPoint(p));
593  }
594  }
595  EXPECT_TRUE(found);
596 
597  delete m;
598  delete ms;
599 }
600 
601 TEST(MergeBoundingSpheres, MergeTwoSpheres)
602 {
603  std::vector<bodies::BoundingSphere> spheres;
604 
606  s1.center = Eigen::Vector3d(5.0, 0.0, 0.0);
607  s1.radius = 1.0;
608 
610  s2.center = Eigen::Vector3d(-5.1, 0.0, 0.0);
611  s2.radius = 1.0;
612 
613  spheres.push_back(s1);
614  spheres.push_back(s2);
615 
616  bodies::BoundingSphere merged_sphere;
617  bodies::mergeBoundingSpheres(spheres, merged_sphere);
618 
619  EXPECT_NEAR(merged_sphere.center[0], -.05, .00001);
620  EXPECT_EQ(merged_sphere.radius, 6.05);
621 }
622 
623 int main(int argc, char** argv)
624 {
625  testing::InitGoogleTest(&argc, argv);
626  return RUN_ALL_TESTS();
627 }
int main(int argc, char **argv)
virtual bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=NULL, unsigned int count=0) const =0
Check if a ray intersects the body, and find the set of intersections, in order, along the ray...
void setPose(const Eigen::Isometry3d &pose)
Set the pose of the body. Default is identity.
Definition: bodies.h:136
Definition of a cylinder Length is along z axis. Origin is at center of mass.
Definition: shapes.h:127
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
bool containsPoint(double x, double y, double z, bool verbose=false) const
Check if a point is inside the body.
Definition: bodies.h:155
Definition of a cylinder.
Definition: bodies.h:283
Definition of a sphere.
Definition: bodies.h:233
#define M_PI
void setScale(double scale)
If the dimension of the body should be scaled, this method sets the scale. Default is 1...
Definition: bodies.h:109
#define EXPECT_NEAR(a, b, prec)
#define EXPECT_SURF
Eigen::Vector3d center
Definition: bodies.h:62
Definition of a sphere that bounds another object.
Definition: bodies.h:60
#define EXPECT_FALSE(args)
void mergeBoundingSpheres(const std::vector< BoundingSphere > &spheres, BoundingSphere &mergedSphere)
Compute a bounding sphere to enclose a set of bounding spheres.
#define EXPECT_EQ(a, b)
virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:278
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:405
Definition of a box.
Definition: bodies.h:343
virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:713
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:196
Definition of a sphere.
Definition: shapes.h:106
virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result)
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:115
double largestComponentForLength2D(const double length)
virtual void computeBoundingSphere(BoundingSphere &sphere) const =0
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
Mesh * createMeshFromResource(const std::string &resource)
Load a mesh from a resource that contains a mesh that can be loaded by assimp.
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.
Definition: shapes.h:196
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:174
TEST(SpherePointContainment, Basic)
virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:514
virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:131
double x
r
#define EXPECT_TRUE(args)
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
Definition: bodies.h:89
void setPadding(double padd)
If constant padding should be added to the body, this method sets the padding. Default is 0...
Definition: bodies.h:123


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Mon Mar 23 2020 03:16:29