GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Software License Agreement (BSD License) |
||
3 |
* |
||
4 |
* Copyright (c) 2011-2014, Willow Garage, Inc. |
||
5 |
* Copyright (c) 2014-2015, Open Source Robotics Foundation |
||
6 |
* All rights reserved. |
||
7 |
* |
||
8 |
* Redistribution and use in source and binary forms, with or without |
||
9 |
* modification, are permitted provided that the following conditions |
||
10 |
* are met: |
||
11 |
* |
||
12 |
* * Redistributions of source code must retain the above copyright |
||
13 |
* notice, this list of conditions and the following disclaimer. |
||
14 |
* * Redistributions in binary form must reproduce the above |
||
15 |
* copyright notice, this list of conditions and the following |
||
16 |
* disclaimer in the documentation and/or other materials provided |
||
17 |
* with the distribution. |
||
18 |
* * Neither the name of Open Source Robotics Foundation nor the names of its |
||
19 |
* contributors may be used to endorse or promote products derived |
||
20 |
* from this software without specific prior written permission. |
||
21 |
* |
||
22 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||
23 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||
24 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||
25 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||
26 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||
27 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||
28 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
29 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
||
30 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||
31 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||
32 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
33 |
* POSSIBILITY OF SUCH DAMAGE. |
||
34 |
*/ |
||
35 |
|||
36 |
/** \author Karsten Knese <Karsten.Knese@googlemail.com> */ |
||
37 |
|||
38 |
#define BOOST_TEST_MODULE FCL_CAPSULE_CAPSULE |
||
39 |
#include <boost/test/included/unit_test.hpp> |
||
40 |
|||
41 |
#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) |
||
42 |
|||
43 |
#include <cmath> |
||
44 |
#include <iostream> |
||
45 |
#include <hpp/fcl/distance.h> |
||
46 |
#include <hpp/fcl/collision.h> |
||
47 |
#include <hpp/fcl/math/transform.h> |
||
48 |
#include <hpp/fcl/collision.h> |
||
49 |
#include <hpp/fcl/collision_object.h> |
||
50 |
#include <hpp/fcl/shape/geometric_shapes.h> |
||
51 |
|||
52 |
#include "utility.h" |
||
53 |
|||
54 |
using namespace hpp::fcl; |
||
55 |
|||
56 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial) { |
57 |
2 |
const double radius = 1.; |
|
58 |
|||
59 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t c1(new Capsule(radius, 0.)); |
60 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t c2(new Capsule(radius, 0.)); |
61 |
|||
62 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s1(new Sphere(radius)); |
63 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s2(new Sphere(radius)); |
64 |
|||
65 |
#ifndef NDEBUG |
||
66 |
2 |
int num_tests = 1e3; |
|
67 |
#else |
||
68 |
int num_tests = 1e6; |
||
69 |
#endif |
||
70 |
|||
71 |
✓✗ | 2 |
Transform3f tf1; |
72 |
✓✗ | 2 |
Transform3f tf2; |
73 |
|||
74 |
✓✓ | 2002 |
for (int i = 0; i < num_tests; ++i) { |
75 |
✓✗✓✗ ✓✗ |
2000 |
Eigen::Vector3d p1 = Eigen::Vector3d::Random() * (2. * radius); |
76 |
✓✗✓✗ ✓✗ |
2000 |
Eigen::Vector3d p2 = Eigen::Vector3d::Random() * (2. * radius); |
77 |
|||
78 |
Eigen::Matrix3d rot1 = |
||
79 |
✓✗✓✗ ✓✗ |
2000 |
Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()) |
80 |
✓✗ | 2000 |
.toRotationMatrix(); |
81 |
Eigen::Matrix3d rot2 = |
||
82 |
✓✗✓✗ ✓✗ |
2000 |
Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()) |
83 |
✓✗ | 2000 |
.toRotationMatrix(); |
84 |
|||
85 |
✓✗ | 2000 |
tf1.setTranslation(p1); |
86 |
✓✗ | 2000 |
tf1.setRotation(rot1); |
87 |
✓✗ | 2000 |
tf2.setTranslation(p2); |
88 |
✓✗ | 2000 |
tf2.setRotation(rot2); |
89 |
|||
90 |
✓✗ | 4000 |
CollisionObject capsule_o1(c1, tf1); |
91 |
✓✗ | 4000 |
CollisionObject capsule_o2(c2, tf2); |
92 |
|||
93 |
✓✗ | 4000 |
CollisionObject sphere_o1(s1, tf1); |
94 |
✓✗ | 4000 |
CollisionObject sphere_o2(s2, tf2); |
95 |
|||
96 |
// Enable computation of nearest points |
||
97 |
✓✗ | 2000 |
CollisionRequest collisionRequest; |
98 |
✓✗✓✗ |
4000 |
CollisionResult capsule_collisionResult, sphere_collisionResult; |
99 |
|||
100 |
✓✗ | 2000 |
size_t sphere_num_collisions = collide( |
101 |
2000 |
&sphere_o1, &sphere_o2, collisionRequest, sphere_collisionResult); |
|
102 |
✓✗ | 2000 |
size_t capsule_num_collisions = collide( |
103 |
2000 |
&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); |
|
104 |
|||
105 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2000 |
BOOST_CHECK_EQUAL(sphere_num_collisions, capsule_num_collisions); |
106 |
✓✓✓✗ |
2000 |
if (sphere_num_collisions == 0 && capsule_num_collisions == 0) |
107 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1466 |
BOOST_CHECK_CLOSE(sphere_collisionResult.distance_lower_bound, |
108 |
capsule_collisionResult.distance_lower_bound, 1e-6); |
||
109 |
} |
||
110 |
2 |
} |
|
111 |
|||
112 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) { |
113 |
2 |
const double radius = 0.01; |
|
114 |
2 |
const double length = 0.2; |
|
115 |
|||
116 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t c1(new Capsule(radius, length)); |
117 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t c2(new Capsule(radius, length)); |
118 |
#ifndef NDEBUG |
||
119 |
2 |
int num_tests = 1e3; |
|
120 |
#else |
||
121 |
int num_tests = 1e6; |
||
122 |
#endif |
||
123 |
|||
124 |
✓✗ | 2 |
Transform3f tf1; |
125 |
✓✗ | 2 |
Transform3f tf2; |
126 |
|||
127 |
✓✗✓✗ |
2 |
Eigen::Vector3d p1 = Eigen::Vector3d::Zero(); |
128 |
Eigen::Vector3d p2_no_collision = |
||
129 |
Eigen::Vector3d(0., 0., |
||
130 |
2 * (length / 2. + radius) + |
||
131 |
✓✗ | 2 |
1e-3); // because capsule are along the Z axis |
132 |
|||
133 |
✓✓ | 2002 |
for (int i = 0; i < num_tests; ++i) { |
134 |
Eigen::Matrix3d rot = |
||
135 |
✓✗✓✗ ✓✗ |
2000 |
Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()) |
136 |
✓✗ | 2000 |
.toRotationMatrix(); |
137 |
|||
138 |
✓✗ | 2000 |
tf1.setTranslation(p1); |
139 |
✓✗ | 2000 |
tf1.setRotation(rot); |
140 |
✓✗ | 2000 |
tf2.setTranslation(p2_no_collision); |
141 |
✓✗ | 2000 |
tf2.setRotation(rot); |
142 |
|||
143 |
✓✗ | 4000 |
CollisionObject capsule_o1(c1, tf1); |
144 |
✓✗ | 4000 |
CollisionObject capsule_o2(c2, tf2); |
145 |
|||
146 |
// Enable computation of nearest points |
||
147 |
✓✗ | 2000 |
CollisionRequest collisionRequest; |
148 |
✓✗ | 4000 |
CollisionResult capsule_collisionResult; |
149 |
|||
150 |
✓✗ | 2000 |
size_t capsule_num_collisions = collide( |
151 |
&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); |
||
152 |
|||
153 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(capsule_num_collisions == 0); |
154 |
} |
||
155 |
|||
156 |
Eigen::Vector3d p2_with_collision = |
||
157 |
✓✗ | 2 |
Eigen::Vector3d(0., 0., std::min(length / 2., radius) * (1. - 1e-2)); |
158 |
✓✓ | 2002 |
for (int i = 0; i < num_tests; ++i) { |
159 |
Eigen::Matrix3d rot = |
||
160 |
✓✗✓✗ ✓✗ |
2000 |
Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()) |
161 |
✓✗ | 2000 |
.toRotationMatrix(); |
162 |
|||
163 |
✓✗ | 2000 |
tf1.setTranslation(p1); |
164 |
✓✗ | 2000 |
tf1.setRotation(rot); |
165 |
✓✗ | 2000 |
tf2.setTranslation(p2_with_collision); |
166 |
✓✗ | 2000 |
tf2.setRotation(rot); |
167 |
|||
168 |
✓✗ | 4000 |
CollisionObject capsule_o1(c1, tf1); |
169 |
✓✗ | 4000 |
CollisionObject capsule_o2(c2, tf2); |
170 |
|||
171 |
// Enable computation of nearest points |
||
172 |
✓✗ | 2000 |
CollisionRequest collisionRequest; |
173 |
✓✗ | 4000 |
CollisionResult capsule_collisionResult; |
174 |
|||
175 |
✓✗ | 2000 |
size_t capsule_num_collisions = collide( |
176 |
&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); |
||
177 |
|||
178 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(capsule_num_collisions > 0); |
179 |
} |
||
180 |
|||
181 |
✓✗ | 2 |
p2_no_collision = Eigen::Vector3d(0., 0., 2 * (length / 2. + radius) + 1e-3); |
182 |
|||
183 |
✓✗ | 2 |
Transform3f geom1_placement(Eigen::Matrix3d::Identity(), |
184 |
✓✗✓✗ |
4 |
Eigen::Vector3d::Zero()); |
185 |
✓✗✓✗ |
2 |
Transform3f geom2_placement(Eigen::Matrix3d::Identity(), p2_no_collision); |
186 |
|||
187 |
✓✓ | 2002 |
for (int i = 0; i < num_tests; ++i) { |
188 |
Eigen::Matrix3d rot = |
||
189 |
✓✗✓✗ ✓✗ |
2000 |
Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()) |
190 |
✓✗ | 2000 |
.toRotationMatrix(); |
191 |
✓✗✓✗ |
2000 |
Eigen::Vector3d trans = Eigen::Vector3d::Random(); |
192 |
|||
193 |
✓✗ | 2000 |
Transform3f displacement(rot, trans); |
194 |
✓✗ | 2000 |
Transform3f tf1 = displacement * geom1_placement; |
195 |
✓✗ | 2000 |
Transform3f tf2 = displacement * geom2_placement; |
196 |
|||
197 |
✓✗ | 4000 |
CollisionObject capsule_o1(c1, tf1); |
198 |
✓✗ | 4000 |
CollisionObject capsule_o2(c2, tf2); |
199 |
|||
200 |
// Enable computation of nearest points |
||
201 |
✓✗ | 2000 |
CollisionRequest collisionRequest; |
202 |
✓✗ | 4000 |
CollisionResult capsule_collisionResult; |
203 |
|||
204 |
✓✗ | 2000 |
size_t capsule_num_collisions = collide( |
205 |
&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); |
||
206 |
|||
207 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(capsule_num_collisions == 0); |
208 |
} |
||
209 |
|||
210 |
// p2_with_collision = |
||
211 |
// Eigen::Vector3d(0.,0.,std::min(length/2.,radius)*(1.-1e-2)); |
||
212 |
✓✗ | 2 |
p2_with_collision = Eigen::Vector3d(0., 0., 0.01); |
213 |
✓✗ | 2 |
geom2_placement.setTranslation(p2_with_collision); |
214 |
|||
215 |
✓✓ | 2002 |
for (int i = 0; i < num_tests; ++i) { |
216 |
Eigen::Matrix3d rot = |
||
217 |
✓✗✓✗ ✓✗ |
2000 |
Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()) |
218 |
✓✗ | 2000 |
.toRotationMatrix(); |
219 |
✓✗✓✗ |
2000 |
Eigen::Vector3d trans = Eigen::Vector3d::Random(); |
220 |
|||
221 |
✓✗ | 2000 |
Transform3f displacement(rot, trans); |
222 |
✓✗ | 2000 |
Transform3f tf1 = displacement * geom1_placement; |
223 |
✓✗ | 2000 |
Transform3f tf2 = displacement * geom2_placement; |
224 |
|||
225 |
✓✗ | 4000 |
CollisionObject capsule_o1(c1, tf1); |
226 |
✓✗ | 4000 |
CollisionObject capsule_o2(c2, tf2); |
227 |
|||
228 |
// Enable computation of nearest points |
||
229 |
✓✗ | 2000 |
CollisionRequest collisionRequest; |
230 |
✓✗ | 4000 |
CollisionResult capsule_collisionResult; |
231 |
|||
232 |
✓✗ | 2000 |
size_t capsule_num_collisions = collide( |
233 |
&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult); |
||
234 |
|||
235 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(capsule_num_collisions > 0); |
236 |
} |
||
237 |
2 |
} |
|
238 |
|||
239 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin) { |
240 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s1(new Capsule(5, 10)); |
241 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s2(new Capsule(5, 10)); |
242 |
|||
243 |
✓✗ | 2 |
Transform3f tf1; |
244 |
✓✗✓✗ |
2 |
Transform3f tf2(Vec3f(20.1, 0, 0)); |
245 |
|||
246 |
✓✗ | 4 |
CollisionObject o1(s1, tf1); |
247 |
✓✗ | 4 |
CollisionObject o2(s2, tf2); |
248 |
|||
249 |
// Enable computation of nearest points |
||
250 |
✓✗ | 2 |
DistanceRequest distanceRequest(true); |
251 |
✓✗ | 2 |
DistanceResult distanceResult; |
252 |
|||
253 |
✓✗ | 2 |
distance(&o1, &o2, distanceRequest, distanceResult); |
254 |
|||
255 |
✓✗ | 2 |
std::cerr << "Applied translation on two capsules"; |
256 |
✓✗✓✗ |
2 |
std::cerr << " T1 = " << tf1.getTranslation() |
257 |
✓✗✓✗ ✓✗ |
2 |
<< ", T2 = " << tf2.getTranslation() << std::endl; |
258 |
✓✗✓✗ |
2 |
std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] |
259 |
✓✗✓✗ |
2 |
<< ", p2 = " << distanceResult.nearest_points[1] |
260 |
✓✗✓✗ ✓✗ |
2 |
<< ", distance = " << distanceResult.min_distance << std::endl; |
261 |
|||
262 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6); |
263 |
2 |
} |
|
264 |
|||
265 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY) { |
266 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s1(new Capsule(5, 10)); |
267 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s2(new Capsule(5, 10)); |
268 |
|||
269 |
✓✗ | 2 |
Transform3f tf1; |
270 |
✓✗✓✗ |
2 |
Transform3f tf2(Vec3f(20, 20, 0)); |
271 |
|||
272 |
✓✗ | 4 |
CollisionObject o1(s1, tf1); |
273 |
✓✗ | 4 |
CollisionObject o2(s2, tf2); |
274 |
|||
275 |
// Enable computation of nearest points |
||
276 |
✓✗ | 2 |
DistanceRequest distanceRequest(true); |
277 |
✓✗ | 2 |
DistanceResult distanceResult; |
278 |
|||
279 |
✓✗ | 2 |
distance(&o1, &o2, distanceRequest, distanceResult); |
280 |
|||
281 |
✓✗ | 2 |
std::cerr << "Applied translation on two capsules"; |
282 |
✓✗✓✗ |
2 |
std::cerr << " T1 = " << tf1.getTranslation() |
283 |
✓✗✓✗ ✓✗ |
2 |
<< ", T2 = " << tf2.getTranslation() << std::endl; |
284 |
✓✗✓✗ |
2 |
std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] |
285 |
✓✗✓✗ |
2 |
<< ", p2 = " << distanceResult.nearest_points[1] |
286 |
✓✗✓✗ ✓✗ |
2 |
<< ", distance = " << distanceResult.min_distance << std::endl; |
287 |
|||
288 |
2 |
FCL_REAL expected = sqrt(800) - 10; |
|
289 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(distanceResult.min_distance, expected, 1e-6); |
290 |
2 |
} |
|
291 |
|||
292 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ) { |
293 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s1(new Capsule(5, 10)); |
294 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s2(new Capsule(5, 10)); |
295 |
|||
296 |
✓✗ | 2 |
Transform3f tf1; |
297 |
✓✗✓✗ |
2 |
Transform3f tf2(Vec3f(0, 0, 20.1)); |
298 |
|||
299 |
✓✗ | 4 |
CollisionObject o1(s1, tf1); |
300 |
✓✗ | 4 |
CollisionObject o2(s2, tf2); |
301 |
|||
302 |
// Enable computation of nearest points |
||
303 |
✓✗ | 2 |
DistanceRequest distanceRequest(true); |
304 |
✓✗ | 2 |
DistanceResult distanceResult; |
305 |
|||
306 |
✓✗ | 2 |
distance(&o1, &o2, distanceRequest, distanceResult); |
307 |
|||
308 |
✓✗ | 2 |
std::cerr << "Applied translation on two capsules"; |
309 |
✓✗✓✗ |
2 |
std::cerr << " T1 = " << tf1.getTranslation() |
310 |
✓✗✓✗ ✓✗ |
2 |
<< ", T2 = " << tf2.getTranslation() << std::endl; |
311 |
✓✗✓✗ |
2 |
std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0] |
312 |
✓✗✓✗ |
2 |
<< ", p2 = " << distanceResult.nearest_points[1] |
313 |
✓✗✓✗ ✓✗ |
2 |
<< ", distance = " << distanceResult.min_distance << std::endl; |
314 |
|||
315 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(distanceResult.min_distance, 0.1, 1e-6); |
316 |
2 |
} |
|
317 |
|||
318 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) { |
319 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s1(new Capsule(5, 10)); |
320 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s2(new Capsule(5, 10)); |
321 |
|||
322 |
✓✗ | 2 |
Transform3f tf1; |
323 |
✓✗✓✗ ✓✗ |
2 |
Transform3f tf2(makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), Vec3f(0, 0, 25.1)); |
324 |
|||
325 |
✓✗ | 4 |
CollisionObject o1(s1, tf1); |
326 |
✓✗ | 4 |
CollisionObject o2(s2, tf2); |
327 |
|||
328 |
// Enable computation of nearest points |
||
329 |
✓✗ | 2 |
DistanceRequest distanceRequest(true); |
330 |
✓✗ | 2 |
DistanceResult distanceResult; |
331 |
|||
332 |
✓✗ | 2 |
distance(&o1, &o2, distanceRequest, distanceResult); |
333 |
|||
334 |
✓✗✓✗ |
2 |
std::cerr << "Applied rotation and translation on two capsules" << std::endl; |
335 |
✓✗✓✗ ✓✗ |
2 |
std::cerr << "R1 = " << tf1.getRotation() << std::endl |
336 |
✓✗✓✗ ✓✗✓✗ |
2 |
<< "T1 = " << tf1.getTranslation().transpose() << std::endl |
337 |
✓✗✓✗ ✓✗ |
2 |
<< "R2 = " << tf2.getRotation() << std::endl |
338 |
✓✗✓✗ ✓✗✓✗ |
2 |
<< "T2 = " << tf2.getTranslation().transpose() << std::endl; |
339 |
✓✗✓✗ |
2 |
std::cerr << "Closest points:" << std::endl |
340 |
✓✗✓✗ ✓✗ |
2 |
<< "p1 = " << distanceResult.nearest_points[0].transpose() |
341 |
✓✗ | 2 |
<< std::endl |
342 |
✓✗✓✗ ✓✗ |
2 |
<< "p2 = " << distanceResult.nearest_points[1].transpose() |
343 |
✓✗ | 2 |
<< std::endl |
344 |
✓✗✓✗ ✓✗ |
2 |
<< "distance = " << distanceResult.min_distance << std::endl; |
345 |
|||
346 |
2 |
const Vec3f& p1 = distanceResult.nearest_points[0]; |
|
347 |
2 |
const Vec3f& p2 = distanceResult.nearest_points[1]; |
|
348 |
|||
349 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6); |
350 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
CHECK_CLOSE_TO_0(p1[0], 1e-4); |
351 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
CHECK_CLOSE_TO_0(p1[1], 1e-4); |
352 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(p1[2], 10, 1e-4); |
353 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
CHECK_CLOSE_TO_0(p2[0], 1e-4); |
354 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
CHECK_CLOSE_TO_0(p2[1], 1e-4); |
355 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(p2[2], 20.1, 1e-4); |
356 |
2 |
} |
Generated by: GCOVR (Version 4.2) |