distance_lower_bound.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014-2016, CNRS-LAAS
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 CNRS-LAAS 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 
35 #define BOOST_TEST_MODULE FCL_DISTANCE_LOWER_BOUND
36 #include <boost/test/included/unit_test.hpp>
37 #include <boost/filesystem.hpp>
38 
39 #include <hpp/fcl/fwd.hh>
40 #include <hpp/fcl/data_types.h>
41 #include <hpp/fcl/BV/OBBRSS.h>
42 #include <hpp/fcl/BVH/BVH_model.h>
44 #include <hpp/fcl/collision.h>
45 #include <hpp/fcl/distance.h>
46 #include "utility.h"
47 #include "fcl_resources/config.h"
48 
49 using hpp::fcl::BVHModel;
56 using hpp::fcl::FCL_REAL;
57 using hpp::fcl::OBBRSS;
58 using hpp::fcl::shared_ptr;
60 using hpp::fcl::Triangle;
61 using hpp::fcl::Vec3f;
62 
64  const CollisionGeometryPtr_t& m1,
65  const CollisionGeometryPtr_t& m2,
66  FCL_REAL& distance) {
67  Transform3f pose1(tf), pose2;
68 
70  request.enable_distance_lower_bound = true;
71 
72  CollisionResult result;
73  CollisionObject co1(m1, pose1);
74  CollisionObject co2(m2, pose2);
75 
76  hpp::fcl::collide(&co1, &co2, request, result);
78 
79  return result.isCollision();
80 }
81 
83  const CollisionGeometryPtr_t& m2) {
84  Transform3f pose1(tf), pose2;
85 
87  request.enable_distance_lower_bound = false;
88 
89  CollisionResult result;
90  CollisionObject co1(m1, pose1);
91  CollisionObject co2(m2, pose2);
92 
93  hpp::fcl::collide(&co1, &co2, request, result);
94  return result.isCollision();
95 }
96 
99  Transform3f pose1(tf), pose2;
100 
101  DistanceRequest request;
102  DistanceResult result;
103  CollisionObject co1(m1, pose1);
104  CollisionObject co2(m2, pose2);
105 
106  hpp::fcl::distance(&co1, &co2, request, result);
107  distance = result.min_distance;
108 
109  if (result.min_distance <= 0) {
110  return true;
111  } else {
112  return false;
113  }
114 }
115 
117  std::vector<Vec3f> p1, p2;
118  std::vector<Triangle> t1, t2;
119  boost::filesystem::path path(TEST_RESOURCES_DIR);
120 
121  loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
122  loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
123 
124  shared_ptr<BVHModel<OBBRSS> > m1(new BVHModel<OBBRSS>);
125  shared_ptr<BVHModel<OBBRSS> > m2(new BVHModel<OBBRSS>);
126 
127  m1->beginModel();
128  m1->addSubModel(p1, t1);
129  m1->endModel();
130 
131  m2->beginModel();
132  m2->addSubModel(p2, t2);
133  m2->endModel();
134 
135  std::vector<Transform3f> transforms;
136  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
137  std::size_t n = 100;
138 
139  generateRandomTransforms(extents, transforms, n);
140 
141  // collision
142  for (std::size_t i = 0; i < transforms.size(); ++i) {
143  FCL_REAL distanceLowerBound, distance;
144  bool col1, col2, col3;
145  col1 = testDistanceLowerBound(transforms[i], m1, m2, distanceLowerBound);
146  col2 = testDistance(transforms[i], m1, m2, distance);
147  col3 = testCollide(transforms[i], m1, m2);
148  std::cout << "col1 = " << col1 << ", col2 = " << col2 << ", col3 = " << col3
149  << std::endl;
150  std::cout << "distance lower bound = " << distanceLowerBound << std::endl;
151  std::cout << "distance = " << distance << std::endl;
152  BOOST_CHECK(col1 == col3);
153  if (!col1) {
154  BOOST_CHECK_MESSAGE(distanceLowerBound <= distance,
155  "distance = " << distance << ", lower bound = "
156  << distanceLowerBound);
157  }
158  }
159 }
160 
161 BOOST_AUTO_TEST_CASE(box_sphere) {
162  shared_ptr<hpp::fcl::Sphere> sphere(new hpp::fcl::Sphere(0.5));
163  shared_ptr<hpp::fcl::Box> box(new hpp::fcl::Box(1., 1., 1.));
164 
165  Transform3f M1;
166  M1.setIdentity();
167  Transform3f M2;
168  M2.setIdentity();
169 
170  std::vector<Transform3f> transforms;
171  FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.};
172  const std::size_t n = 1000;
173 
174  generateRandomTransforms(extents, transforms, n);
175 
176  FCL_REAL distanceLowerBound, distance;
177  bool col1, col2;
178  col1 = testDistanceLowerBound(M1, sphere, box, distanceLowerBound);
179  col2 = testDistance(M1, sphere, box, distance);
180  BOOST_CHECK(col1 == col2);
181  BOOST_CHECK(distanceLowerBound <= distance);
182 
183  for (std::size_t i = 0; i < transforms.size(); ++i) {
184  FCL_REAL distanceLowerBound, distance;
185  bool col1, col2;
186  col1 =
187  testDistanceLowerBound(transforms[i], sphere, box, distanceLowerBound);
188  col2 = testDistance(transforms[i], sphere, box, distance);
189  BOOST_CHECK(col1 == col2);
190  if (!col1) {
191  BOOST_CHECK_MESSAGE(distanceLowerBound <= distance,
192  "distance = " << distance << ", lower bound = "
193  << distanceLowerBound);
194  }
195  }
196 }
197 
198 BOOST_AUTO_TEST_CASE(sphere_sphere) {
199  shared_ptr<hpp::fcl::Sphere> sphere1(new hpp::fcl::Sphere(0.5));
200  shared_ptr<hpp::fcl::Sphere> sphere2(new hpp::fcl::Sphere(1.));
201 
202  Transform3f M1;
203  M1.setIdentity();
204  Transform3f M2;
205  M2.setIdentity();
206 
207  std::vector<Transform3f> transforms;
208  FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.};
209  const std::size_t n = 1000;
210 
211  generateRandomTransforms(extents, transforms, n);
212 
213  FCL_REAL distanceLowerBound, distance;
214  bool col1, col2;
215  col1 = testDistanceLowerBound(M1, sphere1, sphere2, distanceLowerBound);
216  col2 = testDistance(M1, sphere1, sphere2, distance);
217  BOOST_CHECK(col1 == col2);
218  BOOST_CHECK(distanceLowerBound <= distance);
219 
220  for (std::size_t i = 0; i < transforms.size(); ++i) {
221  FCL_REAL distanceLowerBound, distance;
222  bool col1, col2;
223  col1 = testDistanceLowerBound(transforms[i], sphere1, sphere2,
224  distanceLowerBound);
225  col2 = testDistance(transforms[i], sphere1, sphere2, distance);
226  BOOST_CHECK(col1 == col2);
227  if (!col1) {
228  BOOST_CHECK_MESSAGE(distanceLowerBound <= distance,
229  "distance = " << distance << ", lower bound = "
230  << distanceLowerBound);
231  }
232  }
233 }
234 
236  std::vector<Vec3f> p1;
237  std::vector<Triangle> t1;
238  boost::filesystem::path path(TEST_RESOURCES_DIR);
239 
240  loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
241 
242  shared_ptr<BVHModel<OBBRSS> > m1(new BVHModel<OBBRSS>);
243  shared_ptr<hpp::fcl::Box> m2(new hpp::fcl::Box(500, 200, 150));
244 
245  m1->beginModel();
246  m1->addSubModel(p1, t1);
247  m1->endModel();
248 
249  std::vector<Transform3f> transforms;
250  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
251  std::size_t n = 100;
252 
253  generateRandomTransforms(extents, transforms, n);
254 
255  // collision
256  for (std::size_t i = 0; i < transforms.size(); ++i) {
257  FCL_REAL distanceLowerBound, distance;
258  bool col1, col2;
259  col1 = testDistanceLowerBound(transforms[i], m1, m2, distanceLowerBound);
260  col2 = testDistance(transforms[i], m1, m2, distance);
261  BOOST_CHECK(col1 == col2);
262  if (!col1) {
263  BOOST_CHECK_MESSAGE(distanceLowerBound <= distance,
264  "distance = " << distance << ", lower bound = "
265  << distanceLowerBound);
266  }
267  }
268 }
data_types.h
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::loadOBJFile
void loadOBJFile(const char *filename, std::vector< Vec3f > &points, std::vector< Triangle > &triangles)
Load an obj mesh file.
Definition: utility.cpp:96
collision_manager.box
box
Definition: collision_manager.py:11
collision_manager.sphere
sphere
Definition: collision_manager.py:4
hpp::fcl::CollisionRequest::enable_distance_lower_bound
bool enable_distance_lower_bound
Whether a lower bound on distance is returned when objects are disjoint.
Definition: collision_data.h:244
testDistanceLowerBound
bool testDistanceLowerBound(const Transform3f &tf, const CollisionGeometryPtr_t &m1, const CollisionGeometryPtr_t &m2, FCL_REAL &distance)
Definition: distance_lower_bound.cpp:63
hpp::fcl::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(mesh_mesh)
Definition: distance_lower_bound.cpp:116
distance
double distance(const std::vector< Transform3f > &tf, const BVHModel< BV > &m1, const BVHModel< BV > &m2, bool verbose)
Definition: benchmark.cpp:93
utility.h
hpp::fcl::distance
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:181
hpp::fcl::CollisionGeometryPtr_t
shared_ptr< CollisionGeometry > CollisionGeometryPtr_t
Definition: include/hpp/fcl/fwd.hh:96
hpp::fcl::Transform3f::setIdentity
void setIdentity()
set the transform to be identity transform
Definition: transform.h:194
extents
FCL_REAL extents[6]
Definition: test/geometric_shapes.cpp:51
testCollide
bool testCollide(const Transform3f &tf, const CollisionGeometryPtr_t &m1, const CollisionGeometryPtr_t &m2)
Definition: distance_lower_bound.cpp:82
narrowphase.h
hpp::fcl::collide
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts,...
Definition: src/collision.cpp:63
distance.h
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
testDistance
bool testDistance(const Transform3f &tf, const CollisionGeometryPtr_t &m1, const CollisionGeometryPtr_t &m2, FCL_REAL &distance)
Definition: distance_lower_bound.cpp:97
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
hpp::fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:215
hpp::fcl::CollisionResult
collision result
Definition: collision_data.h:302
hpp::fcl::CollisionResult::isCollision
bool isCollision() const
return binary collision result
Definition: collision_data.h:339
hpp::fcl::generateRandomTransforms
void generateRandomTransforms(FCL_REAL extents[6], std::vector< Transform3f > &transforms, std::size_t n)
Generate n random transforms whose translations are constrained by extents.
Definition: utility.cpp:224
collision.path
path
Definition: scripts/collision.py:6
hpp::fcl::DistanceResult::min_distance
FCL_REAL min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0.
Definition: collision_data.h:424
octree.p1
tuple p1
Definition: octree.py:54
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
OBBRSS.h
hpp::fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH/BVH_model.h:273
hpp::fcl::DistanceRequest
request to the distance computation
Definition: collision_data.h:392
hpp::fcl::DistanceResult
distance result
Definition: collision_data.h:420
BVH_model.h
fwd.hh
hpp::fcl::Triangle
Triangle with 3 indices for points.
Definition: data_types.h:96
hpp::fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: BV/OBBRSS.h:54
hpp::fcl::CollisionResult::distance_lower_bound
FCL_REAL distance_lower_bound
Definition: collision_data.h:312
collision.h
hpp::fcl::NO_REQUEST
@ NO_REQUEST
Definition: collision_data.h:231
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:13