GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: test/distance_lower_bound.cpp Lines: 125 125 100.0 %
Date: 2024-02-09 12:57:42 Branches: 337 656 51.4 %

Line Branch Exec Source
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>
43
#include <hpp/fcl/narrowphase/narrowphase.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;
50
using hpp::fcl::CollisionGeometryPtr_t;
51
using hpp::fcl::CollisionObject;
52
using hpp::fcl::CollisionRequest;
53
using hpp::fcl::CollisionResult;
54
using hpp::fcl::DistanceRequest;
55
using hpp::fcl::DistanceResult;
56
using hpp::fcl::FCL_REAL;
57
using hpp::fcl::OBBRSS;
58
using hpp::fcl::shared_ptr;
59
using hpp::fcl::Transform3f;
60
using hpp::fcl::Triangle;
61
using hpp::fcl::Vec3f;
62
63
2202
bool testDistanceLowerBound(const Transform3f& tf,
64
                            const CollisionGeometryPtr_t& m1,
65
                            const CollisionGeometryPtr_t& m2,
66
                            FCL_REAL& distance) {
67

2202
  Transform3f pose1(tf), pose2;
68
69
2202
  CollisionRequest request(hpp::fcl::NO_REQUEST, 1);
70
2202
  request.enable_distance_lower_bound = true;
71
72
4404
  CollisionResult result;
73
4404
  CollisionObject co1(m1, pose1);
74
4404
  CollisionObject co2(m2, pose2);
75
76
2202
  hpp::fcl::collide(&co1, &co2, request, result);
77
2202
  distance = result.distance_lower_bound;
78
79
4404
  return result.isCollision();
80
}
81
82
100
bool testCollide(const Transform3f& tf, const CollisionGeometryPtr_t& m1,
83
                 const CollisionGeometryPtr_t& m2) {
84

100
  Transform3f pose1(tf), pose2;
85
86
100
  CollisionRequest request(hpp::fcl::NO_REQUEST, 1);
87
100
  request.enable_distance_lower_bound = false;
88
89
200
  CollisionResult result;
90
200
  CollisionObject co1(m1, pose1);
91
200
  CollisionObject co2(m2, pose2);
92
93
100
  hpp::fcl::collide(&co1, &co2, request, result);
94
200
  return result.isCollision();
95
}
96
97
2202
bool testDistance(const Transform3f& tf, const CollisionGeometryPtr_t& m1,
98
                  const CollisionGeometryPtr_t& m2, FCL_REAL& distance) {
99

2202
  Transform3f pose1(tf), pose2;
100
101
2202
  DistanceRequest request;
102
2202
  DistanceResult result;
103
4404
  CollisionObject co1(m1, pose1);
104
4404
  CollisionObject co2(m2, pose2);
105
106
2202
  hpp::fcl::distance(&co1, &co2, request, result);
107
2202
  distance = result.min_distance;
108
109
2202
  if (result.min_distance <= 0) {
110
417
    return true;
111
  } else {
112
1785
    return false;
113
  }
114
}
115
116
















4
BOOST_AUTO_TEST_CASE(mesh_mesh) {
117
4
  std::vector<Vec3f> p1, p2;
118
4
  std::vector<Triangle> t1, t2;
119
4
  boost::filesystem::path path(TEST_RESOURCES_DIR);
120
121

2
  loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
122

2
  loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
123
124

4
  shared_ptr<BVHModel<OBBRSS> > m1(new BVHModel<OBBRSS>);
125

4
  shared_ptr<BVHModel<OBBRSS> > m2(new BVHModel<OBBRSS>);
126
127
2
  m1->beginModel();
128
2
  m1->addSubModel(p1, t1);
129
2
  m1->endModel();
130
131
2
  m2->beginModel();
132
2
  m2->addSubModel(p2, t2);
133
2
  m2->endModel();
134
135
4
  std::vector<Transform3f> transforms;
136
2
  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
137
2
  std::size_t n = 100;
138
139
2
  generateRandomTransforms(extents, transforms, n);
140
141
  // collision
142
202
  for (std::size_t i = 0; i < transforms.size(); ++i) {
143
    FCL_REAL distanceLowerBound, distance;
144
    bool col1, col2, col3;
145
200
    col1 = testDistanceLowerBound(transforms[i], m1, m2, distanceLowerBound);
146
200
    col2 = testDistance(transforms[i], m1, m2, distance);
147
200
    col3 = testCollide(transforms[i], m1, m2);
148



200
    std::cout << "col1 = " << col1 << ", col2 = " << col2 << ", col3 = " << col3
149
200
              << std::endl;
150

200
    std::cout << "distance lower bound = " << distanceLowerBound << std::endl;
151

200
    std::cout << "distance             = " << distance << std::endl;
152



200
    BOOST_CHECK(col1 == col3);
153
200
    if (!col1) {
154




120
      BOOST_CHECK_MESSAGE(distanceLowerBound <= distance,
155
                          "distance = " << distance << ", lower bound = "
156
                                        << distanceLowerBound);
157
    }
158
  }
159
2
}
160
161
















4
BOOST_AUTO_TEST_CASE(box_sphere) {
162

4
  shared_ptr<hpp::fcl::Sphere> sphere(new hpp::fcl::Sphere(0.5));
163

4
  shared_ptr<hpp::fcl::Box> box(new hpp::fcl::Box(1., 1., 1.));
164
165
2
  Transform3f M1;
166
2
  M1.setIdentity();
167
2
  Transform3f M2;
168
2
  M2.setIdentity();
169
170
4
  std::vector<Transform3f> transforms;
171
2
  FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.};
172
2
  const std::size_t n = 1000;
173
174
2
  generateRandomTransforms(extents, transforms, n);
175
176
  FCL_REAL distanceLowerBound, distance;
177
  bool col1, col2;
178
2
  col1 = testDistanceLowerBound(M1, sphere, box, distanceLowerBound);
179
2
  col2 = testDistance(M1, sphere, box, distance);
180



2
  BOOST_CHECK(col1 == col2);
181



2
  BOOST_CHECK(distanceLowerBound <= distance);
182
183
2002
  for (std::size_t i = 0; i < transforms.size(); ++i) {
184
    FCL_REAL distanceLowerBound, distance;
185
    bool col1, col2;
186
    col1 =
187
2000
        testDistanceLowerBound(transforms[i], sphere, box, distanceLowerBound);
188
2000
    col2 = testDistance(transforms[i], sphere, box, distance);
189



2000
    BOOST_CHECK(col1 == col2);
190
2000
    if (!col1) {
191




1742
      BOOST_CHECK_MESSAGE(distanceLowerBound <= distance,
192
                          "distance = " << distance << ", lower bound = "
193
                                        << distanceLowerBound);
194
    }
195
  }
196
2
}
197
198
















4
BOOST_AUTO_TEST_CASE(sphere_sphere) {
199

4
  shared_ptr<hpp::fcl::Sphere> sphere1(new hpp::fcl::Sphere(0.5));
200

4
  shared_ptr<hpp::fcl::Sphere> sphere2(new hpp::fcl::Sphere(1.));
201
202
2
  Transform3f M1;
203
2
  M1.setIdentity();
204
2
  Transform3f M2;
205
2
  M2.setIdentity();
206
207
4
  std::vector<Transform3f> transforms;
208
2
  FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.};
209
2
  const std::size_t n = 1000;
210
211
2
  generateRandomTransforms(extents, transforms, n);
212
213
  FCL_REAL distanceLowerBound, distance;
214
  bool col1, col2;
215
2
  col1 = testDistanceLowerBound(M1, sphere1, sphere2, distanceLowerBound);
216
2
  col2 = testDistance(M1, sphere1, sphere2, distance);
217



2
  BOOST_CHECK(col1 == col2);
218



2
  BOOST_CHECK(distanceLowerBound <= distance);
219
220
2002
  for (std::size_t i = 0; i < transforms.size(); ++i) {
221
    FCL_REAL distanceLowerBound, distance;
222
    bool col1, col2;
223
2000
    col1 = testDistanceLowerBound(transforms[i], sphere1, sphere2,
224
                                  distanceLowerBound);
225
2000
    col2 = testDistance(transforms[i], sphere1, sphere2, distance);
226



2000
    BOOST_CHECK(col1 == col2);
227
2000
    if (!col1) {
228




1548
      BOOST_CHECK_MESSAGE(distanceLowerBound <= distance,
229
                          "distance = " << distance << ", lower bound = "
230
                                        << distanceLowerBound);
231
    }
232
  }
233
2
}
234
235
















4
BOOST_AUTO_TEST_CASE(box_mesh) {
236
4
  std::vector<Vec3f> p1;
237
4
  std::vector<Triangle> t1;
238
4
  boost::filesystem::path path(TEST_RESOURCES_DIR);
239
240

2
  loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
241
242

4
  shared_ptr<BVHModel<OBBRSS> > m1(new BVHModel<OBBRSS>);
243

4
  shared_ptr<hpp::fcl::Box> m2(new hpp::fcl::Box(500, 200, 150));
244
245
2
  m1->beginModel();
246
2
  m1->addSubModel(p1, t1);
247
2
  m1->endModel();
248
249
4
  std::vector<Transform3f> transforms;
250
2
  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
251
2
  std::size_t n = 100;
252
253
2
  generateRandomTransforms(extents, transforms, n);
254
255
  // collision
256
202
  for (std::size_t i = 0; i < transforms.size(); ++i) {
257
    FCL_REAL distanceLowerBound, distance;
258
    bool col1, col2;
259
200
    col1 = testDistanceLowerBound(transforms[i], m1, m2, distanceLowerBound);
260
200
    col2 = testDistance(transforms[i], m1, m2, distance);
261



200
    BOOST_CHECK(col1 == col2);
262
200
    if (!col1) {
263




160
      BOOST_CHECK_MESSAGE(distanceLowerBound <= distance,
264
                          "distance = " << distance << ", lower bound = "
265
                                        << distanceLowerBound);
266
    }
267
  }
268
2
}