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 TEST(SpherePointContainment, SimpleInside)
45 {
46  shapes::Sphere shape(1.0);
47  bodies::Body* sphere = new bodies::Sphere(&shape);
48  sphere->setScale(1.05);
49  bool contains = sphere->containsPoint(0, 0, 1.0);
51  Eigen::Vector3d p;
52  EXPECT_TRUE(sphere->samplePointInside(r, 100, p));
53  EXPECT_TRUE(sphere->containsPoint(p));
54  EXPECT_TRUE(contains);
55  delete sphere;
56 }
57 
58 TEST(SpherePointContainment, SimpleOutside)
59 {
60  shapes::Sphere shape(1.0);
61  bodies::Body* sphere = new bodies::Sphere(&shape);
62  sphere->setScale(0.95);
63  bool contains = sphere->containsPoint(0, 0, 1.0);
64  delete sphere;
65  EXPECT_FALSE(contains);
66 }
67 
68 TEST(SpherePointContainment, ComplexInside)
69 {
70  shapes::Sphere shape(1.0);
71  bodies::Body* sphere = new bodies::Sphere(&shape);
72  sphere->setScale(0.95);
73  Eigen::Affine3d pose;
74  pose.setIdentity();
75  pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
76  sphere->setPose(pose);
77  bool contains = sphere->containsPoint(0.5, 1, 1.0);
78  delete sphere;
79  EXPECT_TRUE(contains);
80 }
81 
82 TEST(SpherePointContainment, ComplexOutside)
83 {
84  shapes::Sphere shape(1.0);
85  bodies::Body* sphere = new bodies::Sphere(&shape);
86  sphere->setScale(0.95);
87  Eigen::Affine3d pose;
88  pose.setIdentity();
89  pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
90  sphere->setPose(pose);
91  bool contains = sphere->containsPoint(0.5, 0.0, 0.0);
92  delete sphere;
93  EXPECT_FALSE(contains);
94 }
95 
96 TEST(SphereRayIntersection, SimpleRay1)
97 {
98  shapes::Sphere shape(1.0);
99  bodies::Body* sphere = new bodies::Sphere(&shape);
100  sphere->setScale(1.05);
101 
102  Eigen::Vector3d ray_o(5, 0, 0);
103  Eigen::Vector3d ray_d(-1, 0, 0);
105  bool intersect = sphere->intersectsRay(ray_o, ray_d, &p);
106 
107  delete sphere;
108  EXPECT_TRUE(intersect);
109  EXPECT_EQ(2, (int)p.size());
110  EXPECT_NEAR(p[0].x(), 1.05, 1e-6);
111  EXPECT_NEAR(p[1].x(), -1.05, 1e-6);
112 }
113 
114 TEST(SphereRayIntersection, SimpleRay2)
115 {
116  shapes::Sphere shape(1.0);
117  bodies::Body* sphere = new bodies::Sphere(&shape);
118  sphere->setScale(1.05);
119 
120  Eigen::Vector3d ray_o(5, 0, 0);
121  Eigen::Vector3d ray_d(1, 0, 0);
123  bool intersect = sphere->intersectsRay(ray_o, ray_d, &p);
124 
125  delete sphere;
126  EXPECT_FALSE(intersect);
127  EXPECT_EQ(0, (int)p.size());
128 }
129 
130 TEST(BoxPointContainment, SimpleInside)
131 {
132  shapes::Box shape(1.0, 2.0, 3.0);
133  bodies::Body* box = new bodies::Box(&shape);
134  box->setScale(0.95);
135  bool contains = box->containsPoint(0, 0, 1.0);
136  EXPECT_TRUE(contains);
137 
139  Eigen::Vector3d p;
140  EXPECT_TRUE(box->samplePointInside(r, 100, p));
141  EXPECT_TRUE(box->containsPoint(p));
142 
143  delete box;
144 }
145 
146 TEST(BoxPointContainment, SimpleOutside)
147 {
148  shapes::Box shape(1.0, 2.0, 3.0);
149  bodies::Body* box = new bodies::Box(&shape);
150  box->setScale(0.95);
151  bool contains = box->containsPoint(0, 0, 3.0);
152  delete box;
153  EXPECT_FALSE(contains);
154 }
155 
156 TEST(BoxPointContainment, ComplexInside)
157 {
158  shapes::Box shape(1.0, 1.0, 1.0);
159  bodies::Body* box = new bodies::Box(&shape);
160  box->setScale(1.01);
161  Eigen::Affine3d pose(Eigen::AngleAxisd(M_PI / 3.0, Eigen::Vector3d::UnitX()));
162  pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
163  box->setPose(pose);
164 
165  bool contains = box->containsPoint(1.5, 1.0, 1.5);
166  EXPECT_TRUE(contains);
167 
169  Eigen::Vector3d p;
170  for (int i = 0; i < 100; ++i)
171  {
172  EXPECT_TRUE(box->samplePointInside(r, 100, p));
173  EXPECT_TRUE(box->containsPoint(p));
174  }
175 
176  delete box;
177 }
178 
179 TEST(BoxPointContainment, ComplexOutside)
180 {
181  shapes::Box shape(1.0, 1.0, 1.0);
182  bodies::Body* box = new bodies::Box(&shape);
183  box->setScale(1.01);
184  Eigen::Affine3d pose(Eigen::AngleAxisd(M_PI / 3.0, Eigen::Vector3d::UnitX()));
185  pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
186  box->setPose(pose);
187 
188  bool contains = box->containsPoint(1.5, 1.5, 1.5);
189  delete box;
190  EXPECT_FALSE(contains);
191 }
192 
193 TEST(BoxRayIntersection, SimpleRay1)
194 {
195  shapes::Box shape(1.0, 1.0, 3.0);
196  bodies::Body* box = new bodies::Box(&shape);
197  box->setScale(0.95);
198 
199  Eigen::Vector3d ray_o(10, 0.449, 0);
200  Eigen::Vector3d ray_d(-1, 0, 0);
202 
203  bool intersect = box->intersectsRay(ray_o, ray_d, &p);
204 
205  // for (unsigned int i = 0; i < p.size() ; ++i)
206  // printf("intersection at %f, %f, %f\n", p[i].x(), p[i].y(), p[i].z());
207 
208  delete box;
209  EXPECT_TRUE(intersect);
210 }
211 
212 TEST(BoxRayIntersection, SimpleRay2)
213 {
214  shapes::Box shape(0.9, 0.01, 1.2);
215  bodies::Body* box = new bodies::Box(&shape);
216 
217  Eigen::Affine3d pose(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()));
218  pose.translation() = Eigen::Vector3d(0, 0.005, 0.6);
219  box->setPose(pose);
220 
221  Eigen::Vector3d ray_o(0, 5, 1.6);
222  Eigen::Vector3d ray_d(0, -5.195, -0.77);
224 
225  bool intersect = box->intersectsRay(ray_o, ray_d, &p);
226  EXPECT_TRUE(intersect);
227 
228  intersect = box->intersectsRay(ray_o, ray_d.normalized(), &p);
229  EXPECT_TRUE(intersect);
230 
231  delete box;
232 }
233 
234 TEST(BoxRayIntersection, SimpleRay3)
235 {
236  shapes::Box shape(0.02, 0.4, 1.2);
237  bodies::Body* box = new bodies::Box(&shape);
238 
239  Eigen::Affine3d pose(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()));
240  pose.translation() = Eigen::Vector3d(0.45, -0.195, 0.6);
241  box->setPose(pose);
242 
243  Eigen::Vector3d ray_o(0, -2, 1.11);
244  Eigen::Vector3d ray_d(0, 1.8, -0.669);
246 
247  bool intersect = box->intersectsRay(ray_o, ray_d, &p);
248  EXPECT_FALSE(intersect);
249 
250  intersect = box->intersectsRay(ray_o, ray_d.normalized(), &p);
251  EXPECT_FALSE(intersect);
252 
253  delete box;
254 }
255 
256 TEST(CylinderPointContainment, SimpleInside)
257 {
258  shapes::Cylinder shape(1.0, 4.0);
259  bodies::Body* cylinder = new bodies::Cylinder(&shape);
260  cylinder->setScale(1.05);
261  bool contains = cylinder->containsPoint(0, 0, 2.0);
262  delete cylinder;
263  EXPECT_TRUE(contains);
264 }
265 
266 TEST(CylinderPointContainment, SimpleOutside)
267 {
268  shapes::Cylinder shape(1.0, 4.0);
269  bodies::Body* cylinder = new bodies::Cylinder(&shape);
270  cylinder->setScale(0.95);
271  bool contains = cylinder->containsPoint(0, 0, 2.0);
272  delete cylinder;
273  EXPECT_FALSE(contains);
274 }
275 
276 TEST(CylinderPointContainment, CylinderPadding)
277 {
278  shapes::Cylinder shape(1.0, 4.0);
279  bodies::Body* cylinder = new bodies::Cylinder(&shape);
280  bool contains = cylinder->containsPoint(0, 1.01, 0);
281  EXPECT_FALSE(contains);
282  cylinder->setPadding(.02);
283  contains = cylinder->containsPoint(0, 1.01, 0);
284  EXPECT_TRUE(contains);
285  cylinder->setPadding(0.0);
286  bodies::BoundingSphere bsphere;
287  cylinder->computeBoundingSphere(bsphere);
288  EXPECT_TRUE(bsphere.radius > 2.0);
289 
291  Eigen::Vector3d p;
292  for (int i = 0; i < 1000; ++i)
293  {
294  EXPECT_TRUE(cylinder->samplePointInside(r, 100, p));
295  EXPECT_TRUE(cylinder->containsPoint(p));
296  }
297  delete cylinder;
298 }
299 
300 TEST(MeshPointContainment, Pr2Forearm)
301 {
303  "file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) / "/forearm_roll.stl").string());
304  ASSERT_TRUE(ms != NULL);
305  bodies::Body* m = new bodies::ConvexMesh(ms);
306  ASSERT_TRUE(m != NULL);
307  Eigen::Affine3d t(Eigen::Affine3d::Identity());
308  t.translation().x() = 1.0;
309  EXPECT_FALSE(m->cloneAt(t)->containsPoint(-1.0, 0.0, 0.0));
310 
312  Eigen::Vector3d p;
313  bool found = true;
314  for (int i = 0; i < 10; ++i)
315  {
316  if (m->samplePointInside(r, 10000, p))
317  {
318  found = true;
319  EXPECT_TRUE(m->containsPoint(p));
320  }
321  }
322  EXPECT_TRUE(found);
323 
324  delete m;
325  delete ms;
326 }
327 
328 TEST(MergeBoundingSpheres, MergeTwoSpheres)
329 {
330  std::vector<bodies::BoundingSphere> spheres;
331 
333  s1.center = Eigen::Vector3d(5.0, 0.0, 0.0);
334  s1.radius = 1.0;
335 
337  s2.center = Eigen::Vector3d(-5.1, 0.0, 0.0);
338  s2.radius = 1.0;
339 
340  spheres.push_back(s1);
341  spheres.push_back(s2);
342 
343  bodies::BoundingSphere merged_sphere;
344  bodies::mergeBoundingSpheres(spheres, merged_sphere);
345 
346  EXPECT_NEAR(merged_sphere.center[0], -.05, .00001);
347  EXPECT_EQ(merged_sphere.radius, 6.05);
348 }
349 
350 int main(int argc, char** argv)
351 {
352  testing::InitGoogleTest(&argc, argv);
353  return RUN_ALL_TESTS();
354 }
void setPose(const Eigen::Affine3d &pose)
Set the pose of the body. Default is identity.
Definition: bodies.h:135
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...
BodyPtr cloneAt(const Eigen::Affine3d &pose) const
Get a clone of this body, but one that is located at the pose pose.
Definition: bodies.h:191
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:154
Definition of a cylinder.
Definition: bodies.h:277
Definition of a sphere.
Definition: bodies.h:228
#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:108
#define EXPECT_NEAR(a, b, prec)
Eigen::Vector3d center
Definition: bodies.h:61
Definition of a sphere that bounds another object.
Definition: bodies.h:59
#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)
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:397
Definition of a box.
Definition: bodies.h:336
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
TEST(SpherePointContainment, SimpleInside)
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
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:88
void setPadding(double padd)
If constant padding should be added to the body, this method sets the padding. Default is 0...
Definition: bodies.h:122


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Mon Jun 10 2019 13:22:04