| 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 Willow Garage, Inc. 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 |
/** \author Florent Lamiraux <florent@laas.fr> */ |
||
36 |
|||
37 |
#define _USE_MATH_DEFINES |
||
38 |
#include <cmath> |
||
39 |
|||
40 |
#define BOOST_TEST_MODULE FCL_BOX_BOX |
||
41 |
#include <boost/test/included/unit_test.hpp> |
||
42 |
|||
43 |
#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) |
||
44 |
|||
45 |
#include <cmath> |
||
46 |
#include <iostream> |
||
47 |
#include <hpp/fcl/distance.h> |
||
48 |
#include <hpp/fcl/math/transform.h> |
||
49 |
#include <hpp/fcl/collision.h> |
||
50 |
#include <hpp/fcl/collision_object.h> |
||
51 |
#include <hpp/fcl/shape/geometric_shapes.h> |
||
52 |
|||
53 |
#include "utility.h" |
||
54 |
|||
55 |
using hpp::fcl::CollisionGeometryPtr_t; |
||
56 |
using hpp::fcl::CollisionObject; |
||
57 |
using hpp::fcl::DistanceRequest; |
||
58 |
using hpp::fcl::DistanceResult; |
||
59 |
using hpp::fcl::Transform3f; |
||
60 |
using hpp::fcl::Vec3f; |
||
61 |
|||
62 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(distance_box_box_1) { |
63 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s1(new hpp::fcl::Box(6, 10, 2)); |
64 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s2(new hpp::fcl::Box(2, 2, 2)); |
65 |
|||
66 |
✓✗ | 2 |
Transform3f tf1; |
67 |
✓✗✓✗ |
2 |
Transform3f tf2(Vec3f(25, 20, 5)); |
68 |
|||
69 |
✓✗ | 4 |
CollisionObject o1(s1, tf1); |
70 |
✓✗ | 4 |
CollisionObject o2(s2, tf2); |
71 |
|||
72 |
// Enable computation of nearest points |
||
73 |
✓✗ | 2 |
DistanceRequest distanceRequest(true, 0, 0); |
74 |
✓✗ | 2 |
DistanceResult distanceResult; |
75 |
|||
76 |
✓✗ | 2 |
hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); |
77 |
|||
78 |
✓✗✓✗ |
2 |
std::cerr << "Applied transformations on two boxes" << std::endl; |
79 |
✓✗✓✗ ✓✗ |
2 |
std::cerr << " T1 = " << tf1.getTranslation() << std::endl |
80 |
✓✗✓✗ ✓✗ |
2 |
<< " R1 = " << tf1.getRotation() << std::endl |
81 |
✓✗✓✗ ✓✗ |
2 |
<< " T2 = " << tf2.getTranslation() << std::endl |
82 |
✓✗✓✗ ✓✗ |
2 |
<< " R2 = " << tf2.getRotation() << std::endl; |
83 |
✓✗✓✗ |
2 |
std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] |
84 |
✓✗✓✗ |
2 |
<< ", p2 = " << distanceResult.nearest_points[1] |
85 |
✓✗✓✗ ✓✗ |
2 |
<< ", distance = " << distanceResult.min_distance << std::endl; |
86 |
2 |
double dx = 25 - 3 - 1; |
|
87 |
2 |
double dy = 20 - 5 - 1; |
|
88 |
2 |
double dz = 5 - 1 - 1; |
|
89 |
|||
90 |
2 |
const Vec3f& p1 = distanceResult.nearest_points[0]; |
|
91 |
2 |
const Vec3f& p2 = distanceResult.nearest_points[1]; |
|
92 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(distanceResult.min_distance, |
93 |
sqrt(dx * dx + dy * dy + dz * dz), 1e-4); |
||
94 |
|||
95 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(p1[0], 3, 1e-6); |
96 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(p1[1], 5, 1e-6); |
97 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(p1[2], 1, 1e-6); |
98 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(p2[0], 24, 1e-6); |
99 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(p2[1], 19, 1e-6); |
100 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(p2[2], 4, 1e-6); |
101 |
2 |
} |
|
102 |
|||
103 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(distance_box_box_2) { |
104 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s1(new hpp::fcl::Box(6, 10, 2)); |
105 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s2(new hpp::fcl::Box(2, 2, 2)); |
106 |
static double pi = M_PI; |
||
107 |
✓✗ | 2 |
Transform3f tf1; |
108 |
Transform3f tf2( |
||
109 |
✓✗ | 2 |
hpp::fcl::makeQuat(cos(pi / 8), sin(pi / 8) / sqrt(3), |
110 |
2 |
sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3)), |
|
111 |
✓✗✓✗ |
4 |
Vec3f(0, 0, 10)); |
112 |
|||
113 |
✓✗ | 4 |
CollisionObject o1(s1, tf1); |
114 |
✓✗ | 4 |
CollisionObject o2(s2, tf2); |
115 |
|||
116 |
// Enable computation of nearest points |
||
117 |
✓✗ | 2 |
DistanceRequest distanceRequest(true, 0, 0); |
118 |
✓✗ | 2 |
DistanceResult distanceResult; |
119 |
|||
120 |
✓✗ | 2 |
hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); |
121 |
|||
122 |
✓✗✓✗ |
2 |
std::cerr << "Applied transformations on two boxes" << std::endl; |
123 |
✓✗✓✗ ✓✗ |
2 |
std::cerr << " T1 = " << tf1.getTranslation() << std::endl |
124 |
✓✗✓✗ ✓✗ |
2 |
<< " R1 = " << tf1.getRotation() << std::endl |
125 |
✓✗✓✗ ✓✗ |
2 |
<< " T2 = " << tf2.getTranslation() << std::endl |
126 |
✓✗✓✗ ✓✗ |
2 |
<< " R2 = " << tf2.getRotation() << std::endl; |
127 |
✓✗✓✗ |
2 |
std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] |
128 |
✓✗✓✗ |
2 |
<< ", p2 = " << distanceResult.nearest_points[1] |
129 |
✓✗✓✗ ✓✗ |
2 |
<< ", distance = " << distanceResult.min_distance << std::endl; |
130 |
|||
131 |
2 |
const Vec3f& p1 = distanceResult.nearest_points[0]; |
|
132 |
2 |
const Vec3f& p2 = distanceResult.nearest_points[1]; |
|
133 |
2 |
double distance = -1.62123444 + 10 - 1; |
|
134 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); |
135 |
|||
136 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(p1[0], 0.60947571, 1e-4); |
137 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(p1[1], 0.01175873, 1e-4); |
138 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(p1[2], 1, 1e-6); |
139 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(p2[0], 0.60947571, 1e-4); |
140 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(p2[1], 0.01175873, 1e-4); |
141 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(p2[2], -1.62123444 + 10, 1e-4); |
142 |
2 |
} |
|
143 |
|||
144 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(distance_box_box_3) { |
145 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s1(new hpp::fcl::Box(1, 1, 1)); |
146 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s2(new hpp::fcl::Box(1, 1, 1)); |
147 |
static double pi = M_PI; |
||
148 |
✓✗ | 2 |
Transform3f tf1(hpp::fcl::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)), |
149 |
✓✗✓✗ |
4 |
Vec3f(-2, 1, .5)); |
150 |
✓✗ | 2 |
Transform3f tf2(hpp::fcl::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0), |
151 |
✓✗✓✗ |
4 |
Vec3f(2, .5, .5)); |
152 |
|||
153 |
✓✗ | 4 |
CollisionObject o1(s1, tf1); |
154 |
✓✗ | 4 |
CollisionObject o2(s2, tf2); |
155 |
|||
156 |
// Enable computation of nearest points |
||
157 |
✓✗ | 2 |
DistanceRequest distanceRequest(true, 0, 0); |
158 |
✓✗ | 2 |
DistanceResult distanceResult; |
159 |
|||
160 |
✓✗ | 2 |
hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); |
161 |
|||
162 |
✓✗✓✗ |
2 |
std::cerr << "Applied transformations on two boxes" << std::endl; |
163 |
✓✗✓✗ ✓✗ |
2 |
std::cerr << " T1 = " << tf1.getTranslation() << std::endl |
164 |
✓✗✓✗ ✓✗ |
2 |
<< " R1 = " << tf1.getRotation() << std::endl |
165 |
✓✗✓✗ ✓✗ |
2 |
<< " T2 = " << tf2.getTranslation() << std::endl |
166 |
✓✗✓✗ ✓✗ |
2 |
<< " R2 = " << tf2.getRotation() << std::endl; |
167 |
✓✗✓✗ |
2 |
std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] |
168 |
✓✗✓✗ |
2 |
<< ", p2 = " << distanceResult.nearest_points[1] |
169 |
✓✗✓✗ ✓✗ |
2 |
<< ", distance = " << distanceResult.min_distance << std::endl; |
170 |
|||
171 |
2 |
const Vec3f& p1 = distanceResult.nearest_points[0]; |
|
172 |
2 |
const Vec3f& p2 = distanceResult.nearest_points[1]; |
|
173 |
2 |
double distance = 4 - sqrt(2); |
|
174 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); |
175 |
|||
176 |
✓✗ | 2 |
const Vec3f p1Ref(sqrt(2) / 2 - 2, 1, .5); |
177 |
✓✗ | 2 |
const Vec3f p2Ref(2 - sqrt(2) / 2, 1, .5); |
178 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(p1[0], p1Ref[0], 1e-4); |
179 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(p1[1], p1Ref[1], 1e-4); |
180 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(p1[2], p1Ref[2], 1e-4); |
181 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(p2[0], p2Ref[0], 1e-4); |
182 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(p2[1], p2Ref[1], 1e-4); |
183 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(p2[2], p2Ref[2], 1e-4); |
184 |
|||
185 |
// Apply the same global transform to both objects and recompute |
||
186 |
✓✗ | 2 |
Transform3f tf3(hpp::fcl::makeQuat(0.435952844074, -0.718287018243, |
187 |
0.310622451066, 0.444435113443), |
||
188 |
✓✗✓✗ |
4 |
Vec3f(4, 5, 6)); |
189 |
✓✗✓✗ |
2 |
tf1 = tf3 * tf1; |
190 |
✓✗✓✗ |
2 |
tf2 = tf3 * tf2; |
191 |
✓✗✓✗ |
2 |
o1 = CollisionObject(s1, tf1); |
192 |
✓✗✓✗ |
2 |
o2 = CollisionObject(s2, tf2); |
193 |
|||
194 |
✓✗ | 2 |
distanceResult.clear(); |
195 |
✓✗ | 2 |
hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); |
196 |
|||
197 |
✓✗✓✗ |
2 |
std::cerr << "Applied transformations on two boxes" << std::endl; |
198 |
✓✗✓✗ ✓✗ |
2 |
std::cerr << " T1 = " << tf1.getTranslation() << std::endl |
199 |
✓✗✓✗ ✓✗ |
2 |
<< " R1 = " << tf1.getRotation() << std::endl |
200 |
✓✗✓✗ ✓✗ |
2 |
<< " T2 = " << tf2.getTranslation() << std::endl |
201 |
✓✗✓✗ ✓✗ |
2 |
<< " R2 = " << tf2.getRotation() << std::endl; |
202 |
✓✗✓✗ |
2 |
std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] |
203 |
✓✗✓✗ |
2 |
<< ", p2 = " << distanceResult.nearest_points[1] |
204 |
✓✗✓✗ ✓✗ |
2 |
<< ", distance = " << distanceResult.min_distance << std::endl; |
205 |
|||
206 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); |
207 |
|||
208 |
✓✗ | 2 |
const Vec3f p1Moved = tf3.transform(p1Ref); |
209 |
✓✗ | 2 |
const Vec3f p2Moved = tf3.transform(p2Ref); |
210 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(p1[0], p1Moved[0], 1e-4); |
211 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(p1[1], p1Moved[1], 1e-4); |
212 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(p1[2], p1Moved[2], 1e-4); |
213 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(p2[0], p2Moved[0], 1e-4); |
214 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(p2[1], p2Moved[1], 1e-4); |
215 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(p2[2], p2Moved[2], 1e-4); |
216 |
2 |
} |
|
217 |
|||
218 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(distance_box_box_4) { |
219 |
✓✗ | 4 |
hpp::fcl::Box s1(1, 1, 1); |
220 |
✓✗ | 4 |
hpp::fcl::Box s2(1, 1, 1); |
221 |
|||
222 |
// Enable computation of nearest points |
||
223 |
✓✗ | 2 |
DistanceRequest distanceRequest(true, 0, 0); |
224 |
✓✗ | 2 |
DistanceResult distanceResult; |
225 |
double distance; |
||
226 |
|||
227 |
✓✗✓✗ |
2 |
Transform3f tf1(Vec3f(2, 0, 0)); |
228 |
✓✗ | 2 |
Transform3f tf2; |
229 |
✓✗ | 2 |
hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); |
230 |
|||
231 |
2 |
distance = 1.; |
|
232 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); |
233 |
|||
234 |
✓✗✓✗ |
2 |
tf1.setTranslation(Vec3f(1.01, 0, 0)); |
235 |
✓✗ | 2 |
distanceResult.clear(); |
236 |
✓✗ | 2 |
hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); |
237 |
|||
238 |
2 |
distance = 0.01; |
|
239 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); |
240 |
|||
241 |
✓✗✓✗ |
2 |
tf1.setTranslation(Vec3f(0.99, 0, 0)); |
242 |
✓✗ | 2 |
distanceResult.clear(); |
243 |
✓✗ | 2 |
hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); |
244 |
|||
245 |
2 |
distance = -0.01; |
|
246 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); |
247 |
|||
248 |
✓✗✓✗ |
2 |
tf1.setTranslation(Vec3f(0, 0, 0)); |
249 |
✓✗ | 2 |
distanceResult.clear(); |
250 |
✓✗ | 2 |
hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); |
251 |
|||
252 |
2 |
distance = -1; |
|
253 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); |
254 |
2 |
} |
| Generated by: GCOVR (Version 4.2) |