GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
} |
Generated by: GCOVR (Version 4.2) |